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

独自シリアライザの作成

独自シリアライザの作成

CMakeLists.txtの作成

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

 #OpenRTM-aistの検出
 find_package(OpenRTM 2.0 REQUIRED)
 find_package(ROSTransport REQUIRED)

 #ROSの検出
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(roscpp roscpp)
 
 if(NOT roscpp_FOUND)
     message(FATAL_ERROR "can not find roscpp.")
 endif()
 
 #Boostの検出
 find_package(Boost COMPONENTS chrono filesystem system)
 if(NOT Boost_FOUND)
     find_package(Boost REQUIRED COMPONENTS chrono filesystem signals system)
 endif()

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

 #インクルードディレクトリ、リンクディレクトリ、コンパイル時のフラグの設定
 include_directories(${OPENRTM_INCLUDE_DIRS})
 add_definitions(${OPENRTM_CFLAGS})
 
 link_directories(${OPENRTM_LIBRARY_DIRS})
 link_directories(${roscpp_LIBRARY_DIRS})
 
 #動的リンクライブラリの生成
 add_library(${PROJECT_NAME} SHARED TestRosSerializer.cpp)
 
 
 #生成するライブラリ名をTestRosSerializer.dll(もしくは.so)に設定
 set_target_properties(${PROJECT_NAME} PROPERTIES PREFIX "")
 
 #OpenRTM-aist、ROS、Boostのライブラリとリンク
 target_include_directories(${PROJECT_NAME} SYSTEM
        PRIVATE ${roscpp_INCLUDE_DIRS}
        PRIVATE ${Boost_INCLUDE_DIRS}
        PRIVATE ${ROSTRANSPORT_INCLUDE_DIRS})
 
 target_link_libraries(${PROJECT_NAME} ${OPENRTM_LIBRARIES} ${roscpp_LIBRARIES} ${Boost_LIBRARIES} ${ROSTRANSPORT_LIBRARIES})

CPPファイルの作成

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

 #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/ROSTransport/ROSSerializer.h>
 #include <geometry_msgs/TwistStamped.h>

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

 class TestRosSerializer : public RTC::ROSSerializerBase<RTC::TimedVelocity3D>
 {
 public:
    TestRosSerializer(){};

以下のようにTestRosSerializerクラスのメンバ関数としてserialize関数を定義する。 serialize関数では、RTC::TimedVelocity3D型のデータをgeometry_msgs/TwistStamped型のデータに変換し、その後バイト列データに変換しています。

    bool serialize(const RTC::TimedVelocity3D& data) override
    {
        geometry_msgs::TwistStamped msg;
        msg.header.stamp.sec = data.tm.sec;
        msg.header.stamp.nsec = 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;
 
        RTC::ROSSerializerBase<RTC::TimedVelocity3D>::m_message = ros::serialization::serializeMessage<geometry_msgs::TwistStamped>(msg);
        return true;
    }

次にdeserialize関数を定義します。 deserialize関数では、バイト列データをgeometry_msgs/TwistStamped型のデータに変換し、その後RTC::TimedVelocity3D型のデータに変換しています。

    bool deserialize(RTC::TimedVelocity3D& data) override
    {
        geometry_msgs::TwistStamped msg;
        ros::serialization::deserializeMessage(ROSSerializerBase<RTC::TimedVelocity3D>::m_message, msg);
        data.tm.sec = msg.header.stamp.sec;
        data.tm.nsec = msg.header.stamp.nsec;
        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 true;
    }

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

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

動作確認

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

testOpenRTMSerializer

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

 source /opt/ros/noetic/setup.bash

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

 manager.modules.load_path: /usr/lib/openrtm-2.0/transport/, ~/TestRosSerializer/build
 manager.modules.preload: ROSTransport.so, TestRosSerializer.so

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

 manager.components.preconnect: testVelocity3DIn0.in?interface_type=ros&marshaling_type=ros:geometry_msgs/TwistStamped&ros.topic=chatter&ros.so_keepalive=NO&ros.node.name=testVelocity3DIn0, testVelocity3DOut0.out?interface_type=ros&marshaling_type=ros:geometry_msgs/TwistStamped&ros.topic=chatter&ros.node.name=testVelocity3DOut0

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

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

 testVelocity3DOutComp -f rtc.conf

 testVelocity3DInComp -f rtc.conf

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

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

OpenRTM-aist