ROS2でros::Time::now()の代用の自作

※シミュレーション系におけるROS::Timeについては考慮していません。

　ROS2とROS1の違いとしてROS::Time関数の仕様が挙げられます。名称変更ならよかったのですが、（確か）rclcppのnow().nanoseconds()は、ROS1なら小数点以下がint32_tで取得できましたが、ROS2はuint32_tで、秒とナノ秒がくっついている状態になります。つまり小数点以下の数値を取得したければ関数を自作しないといけないみたいです。（もしかしたら、従来のように分離する宣言方法があるのかもしれませんが…）

　そこで、過去にそれぞれ秒、ミリ秒、ナノ秒を取得するクラスを作成していたので、そのプログラムを示します。自分のプログラムから引っ張ってきて少し書き換えをしています。

　以下のプログラムをhppとして保存して、使用するプログラムで継承します。time_now_ns()を呼び出すだけでROS::Time::now().nsみたいな挙動をします。

#ifndef RCLCPP_TIME
#define RCLCPP_TIME

#include <rclcpp/rclcpp.hpp>
#define ONE_GIGA   1000000000

rclcpp::Clock system_clock(RCL_ROS_TIME);

class rostime
{
public:
    uint64_t time_now_raw(){
        return system_clock.now().nanoseconds();
    }
    int32_t time_now_s(){
        return system_clock.now().seconds();
    }
    uint32_t time_now_ns(){
        uint64_t time_now = system_clock.now().nanoseconds();
        uint64_t time_now_s = system_clock.now().seconds();
        return uint32_t( (time_now - time_now_s*ONE_GIGA));
    }
    uint32_t time_now_ms(){
        return uint32_t(int64_t(time_now_ns())*10/ONE_GIGA);
    }
};
#endif

参考資料

rclcpp: rclcpp::Clock Class Reference

rclcpp: rclcpp::Time Class Reference

ROS1 -> ROS2のポーティングときにハマった点のまとめ - Qiita