ros::Time::now()を数値にしてcsvに書き込もうとしても同じ値が出る件

はじめに

実験とかのためにROSトピックの値をcsvファイルに出力したいことが良くあるんですが、ROSの時刻を出力するのにはまったので解決までのメモです。

問題点

ROSの時間を数値(double)に変換するには

double t = ros::Time::now().toSec();

でOK。そんでcsvファイルに出力する

#include <ros/ros.h>
#include <fstream>

int main(int argc, char** argv){
  ros::init(argc, argv, "record_to_csv");
  ros::NodeHandle nh;
  ros::Rate loop_rate(20);
  
  std::ofstream fout("data.csv");
  fout << "Time[s]" << std::endl;

  while(ros::ok()){

    double t = ros::Time::now().toSec();
    fout << t << std::endl;

    loop_rate.sleep();
  }
}

と思ったら、全部同じ値になってしまった!

Time
0
1529990000
1529990000
1529990000
1529990000
1529990000
1529990000
1529990000
...

慌てて色々試したところ、ROS_ERRORではちゃんと表示される模様。

ROS_INFO("%lf",ros::Time::now().toSec());
[ INFO] [1530608049.263032619]: 0.000000
[ INFO] [1530608049.566322356, 1529991633.616588554]: 1529991633.616589
[ INFO] [1530608049.566409625, 1529991633.616588554]: 1529991633.616589
[ INFO] [1530608049.617069613, 1529991633.667305116]: 1529991633.667305
[ INFO] [1530608049.667780029, 1529991633.717981430]: 1529991633.717981
[ INFO] [1530608049.718522891, 1529991633.768572181]: 1529991633.768572

ところがstd::coutでは出力されない。

std::cout << ros::Time::now().toSec() << std::endl;
0
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09

解決

表示がROS_INFOと違うので疑っていたが、やはり桁数の問題だった。ファイル書き込みではデフォルトで6桁以降は切り捨てられるようだ。ROS関係のほうを疑ったり、ros::Time(0)を試してみたりしたけど全然だめで、ros::Time::now().secとros::Time::now().nsecを読んで(どちらもuint型)合計するとかやってしまった…。
std::coutもfoutも次のようにすれば桁数の設定ができる。

fout << std::setprecision(20) << ros::Time::now().toSec() << std::endl;
0
1529991603.1314094067
1529991603.1314094067
1529991603.1819009781
1529991603.2322974205
1529991603.2829723358
1529991603.3336381912
1529991603.3842408657

プログラミング上手い人にとっては常識なのかも知れないけど、そんなこともあるのか。勉強になりました。

コメントを残す

メールアドレスが公開されることはありません。