はじめに
実験とかのために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
プログラミング上手い人にとっては常識なのかも知れないけど、そんなこともあるのか。勉強になりました。