独自シリアライザの実装手順(ROS2、C++)

独自シリアライザの作成

独自シリアライザの作成

CMakeLists.txtの作成

OpenRTM-aistのデータ型とROSのメッセージ型を変換するシリアライザの作成のためには、ROSTransport、ROS2のヘッダーファイルのインクルード、ライブラリのリンクが必要になります。以下のようにCMakeLists.txtでOpenRTM-aist、ROS2、FastDDSを検出します。

 #OpenRTM-aistの検出
 find_package(OpenRTM 2.0 REQUIRED)
 find_package(ROS2Transport REQUIRED)
 
 #ROS2の検出
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
 #FastDDSの検出
 find_package(fastcdr REQUIRED)
 find_package(fastrtps REQUIRED)

以下のように動的リンクライブラリの生成、検出したライブラリのリンク、インクルードディレクトリの設定を行います。

 #インクルードディレクトリ、リンクディレクトリ、コンパイル時のフラグの設定
 include_directories(${OPENRTM_INCLUDE_DIRS})
 add_definitions(${OPENRTM_CFLAGS})
 
 link_directories(${OPENRTM_LIBRARY_DIRS})
 
 #動的リンクライブラリの生成
 add_library(${PROJECT_NAME} SHARED TestRos2Serializer.cpp)
 
 
 #生成するライブラリ名をTestRos2Serializer.dll(もしくは.so)に設定
 set_target_properties(${PROJECT_NAME} PROPERTIES PREFIX "")
 
 
 target_include_directories(${PROJECT_NAME} SYSTEM
         PUBLIC ${rclcpp_INCLUDE_DIRS}
         PUBLIC ${std_msgs_INCLUDE_DIRS}
         PUBLIC ${geometry_msgs_INCLUDE_DIRS}
         PUBLIC ${sensor_msgs_INCLUDE_DIRS}
         PUBLIC ${ROS2TRANSPORT_INCLUDE_DIRS})
 
 #OpenRTM-aistのライブラリとリンク
 target_link_libraries(${PROJECT_NAME} ${OPENRTM_LIBRARIES} ${ROS2TRANSPORT_LIBRARIES}
         ${rclcpp_LIBRARIES} ${std_msgs_LIBRARIES} ${geometry_msgs_LIBRARIES} ${sensor_msgs_LIBRARIES} 
         ${std_msgs_LIBRARIES__rosidl_typesupport_fastrtps_cpp} 
         ${geometry_msgs_LIBRARIES__rosidl_typesupport_fastrtps_cpp} 
         ${sensor_msgs_LIBRARIES__rosidl_typesupport_fastrtps_cpp}
         fastcdr fastrtps)
 
 target_compile_definitions(${PROJECT_NAME} PRIVATE STD_MSGS_VERSION_MAJOR=${std_msgs_VERSION_MAJOR} 
                                 GEOMETORY_MSGS_VERSION_MAJOR=${geometry_msgs_VERSION_MAJOR} 
                                 SENSOR_MSGS_VERSION_MAJOR=${sensor_msgs_VERSION_MAJOR} 
                                 STD_MSGS_VERSION_MINOR=${std_msgs_VERSION_MINOR} 
                                 GEOMETORY_MSGS_VERSION_MINOR=${geometry_msgs_VERSION_MINOR} 
                                 SENSOR_MSGS_VERSION_MINOR==${sensor_msgs_VERSION_MINOR})

CPPファイルの作成

ROS用シリアライザを作成する場合、CPPファイルでROS2Serializer.hをインクルードします。また、今回はROS2のgeometry_msgs/TwistStamped型を使用するため、geometry_msgs/msg/twist_stamped.hpp、geometry_msgs/msg/detail/twist_stamped__rosidl_typesupport_fastrtps_cpp.hppをインクルードします。このインクルードファイルは使用するメッセージ型により変更します。

 #include <rtm/ByteDataStreamBase.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
 #include <rtm/idl/ExtendedDataTypesSkel.h>
 #include <coil/Factory.h>
 #include <rtm/Manager.h>
 #include <rtm/ext/ROS2Transport/ROS2Serializer.h>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #if (STD_MSGS_VERSION_MAJOR >= 2)
 #include <geometry_msgs/msg/detail/twist_stamped__rosidl_typesupport_fastrtps_cpp.hpp>
 #else
 #include <geometry_msgs/msg/twist_stamped__rosidl_typesupport_fastrtps_cpp.hpp>
 #endif

ROS2SerializerBaseクラスを継承したクラスを定義します。 ROS2SerializerBaseクラスのテンプレート引数には使用するOpenRTM-aistのデータ型を指定します。今回はRTC::TimedVelocity3D型を使用するため、RTC::TimedVelocity3Dを指定します。

 class TestRos2Serializer : public RTC::ROS2SerializerBase<RTC::TimedVelocity3D>
 {
 public:
    TestRos2Serializer(){};

以下のようにTestRos2Serializerクラスのメンバ関数としてserialize関数を定義する。 serialize関数では、RTC::TimedVelocity3D型のデータをgeometry_msgs/TwistStamped型のデータに変換し、その後バイト列データに変換しています。 geometry_msgs/TwistStamped型のデータからバイト列データへの変換は、geometrymsg_serialize関数を使用しています。 std_msgパッケージのメッセージ型の場合はstdmsg_serialize関数、geometry_msgsパッケージのメッセージ型の場合はgeometrymsg_serialize関数、sensor_msgパッケージのメッセージ型の場合はsensormsg_serialize関数を使用します。

    bool serialize(const RTC::TimedVelocity3D& data) override
    {
        geometry_msgs::msg::TwistStamped msg;
        msg.header.stamp.sec = data.tm.sec;
        msg.header.stamp.nanosec = data.tm.nsec;
        msg.twist.linear.x = data.data.vx;
        msg.twist.linear.y = data.data.vy;
        msg.twist.linear.z = data.data.vz;
        msg.twist.angular.x = data.data.vr;
        msg.twist.angular.y = data.data.vp;
        msg.twist.angular.z = data.data.va;

        return RTC::ROS2SerializerBase<RTC::TimedVelocity3D>::geometrymsg_serialize(msg);
    }

std_msg、geometry_msgs、sensor_msg以外のパッケージのメッセージ型の場合は、stdmsg_serialize関数の実装を参考にして記述してください。

https://github.com/OpenRTM/OpenRTM-aist/blob/d93245bc24caa2dd947cb3288fb7ca58d432f920/src/ext/transport/ROS2Transport/ROS2Serializer.h#L274-L290

次にdeserialize関数を定義します。 deserialize関数では、バイト列データをgeometry_msgs/TwistStamped型のデータに変換し、その後RTC::TimedVelocity3D型のデータに変換しています。バイト列データからgeometry_msgs/TwistStamped型のデータへの変換は、geometrymsg_deserialize関数を使用しています。 std_msgパッケージのメッセージ型の場合はstdmsg_deserialize関数、geometry_msgsパッケージのメッセージ型の場合はgeometrymsg_deserialize関数、sensor_msgパッケージのメッセージ型の場合はsensormsg_deserialize関数を使用します。

    bool deserialize(RTC::TimedVelocity3D& data) override
    {
        geometry_msgs::msg::TwistStamped msg;
        bool ret = ROS2SerializerBase<RTC::TimedVelocity3D>::geometrymsg_deserialize(msg);
        
        data.tm.sec = msg.header.stamp.sec;
        data.tm.nsec = msg.header.stamp.nanosec;
        data.data.vx = msg.twist.linear.x;
        data.data.vy = msg.twist.linear.y;
        data.data.vz = msg.twist.linear.z;
        data.data.vr = msg.twist.angular.x;
        data.data.vp = msg.twist.angular.y;
        data.data.va = msg.twist.angular.z;

        return ret;
    }

std_msg、geometry_msgs、sensor_msg以外のパッケージのメッセージ型の場合は、stdmsg_deserialize関数の実装を参考にして記述してください。

https://github.com/OpenRTM/OpenRTM-aist/blob/d93245bc24caa2dd947cb3288fb7ca58d432f920/src/ext/transport/ROS2Transport/ROS2Serializer.h#L308-L325

以下ように{ライブラリ名}Init関数を定義すると、動的リンクライブラリのロード時にこの関数が呼ばれます。 TestRos2SerializerInit関数内でファクトリにシリアライザを追加することにより、RTC起動時に実装したシリアライザが使えるようにします。

extern "C" {

    //以下はモジュールロード時に呼び出される関数
    DLL_EXPORT void TestRos2SerializerInit(RTC::Manager* /*manager*/)
    {
        //シリアライザの登録
        RTC::addRos2Serializer<RTC::TimedVelocity3D, geometry_msgs::msg::TwistStamped, TestRos2Serializer>("ros2:geometry_msgs/TwistStamped");
    }

}

動作確認

以下から作成済みのシリアライザ、動作確認用のRTCをダウンロードできます。

testOpenRTMSerializer

作成したTestRos2Serializerをビルドしてください。ビルド、実行時にはROS2の環境変数が設定されている必要があるため、以下のコマンドを実行してください。

 source /opt/ros/foxy/setup.bash

rtc.confを作成します。 FastRTPSTransport.so、ROS2Transport.soと、作成したシリアライザTestRos2Serializer.soをロードするように設定します。 TestRos2Serializer.soのパスは、ビルドしたディレクトリのパスに変更してください。

 manager.modules.load_path: /usr/lib/openrtm-2.0/transport/, ~/TestRos2Serializer/build
 manager.modules.preload: FastRTPSTransport.so, ROS2Transport.so, TestRos2Serializer.so

以下のようにサンプルコンポーネントのtestVelocity3DInのInPort、testVelocity3DOutのOutPortでコネクションを生成します。

 manager.components.preconnect: testVelocity3DIn0.in?interface_type=fast-rtps&marshaling_type=ros2:geometry_msgs/TwistStamped&fast-rtps.topic=chatter, testVelocity3DOut0.out?interface_type=fast-rtps&marshaling_type=ros2:geometry_msgs/TwistStamped&fast-rtps.topic=chatter

動作確認する場合は、testOpenRTMSerializerに含まれているtestVelocity3DIn、testVelocity3DOutのサンプルコンポーネントをビルドしてください。

以下のコマンド実行後、RTCをアクティブ化するとtestVelocity3DOutのPublisherからtestVelocity3DInのSubscriberにデータが送信されて、testVelocity3DInを起動した画面に数値が表示されます。

 testVelocity3DOutComp -f rtc.conf

 testVelocity3DInComp -f rtc.conf

今回の動作確認ではRTC同士でROS2のトピック通信を実行しましたが、ROS2ノードと通信を確認する場合は、geometry_msgs/TwistStamped型のPublisher、Subscriberを持つROS2ノードを作成して試してください。

RT-Component	153.5
RT-Middleware	35
Tools	22
Documentation	2

OpenRTM-aist