ROS　Pub&Sub通信　C++編

by Yoshiaki Toyamain ROSon 投稿日:

概要

本投稿では、以前に投稿したROSのPub&Sub通信　Python編に続いて、C++によるPublisherとSubscriberの実装方法について解説を行います。Pub&Sub通信に関する説明は前回の記事を参考にしてください。

Pythonによる記述はコンパイル不要で実行が容易である反面、ロボットの制御において、Pythonはプログラムの処理速度が遅いため、C++で記述した方が良い場合があります。そのため、PythonとC++の両方でPub&Sub通信をマスターしておくことが重要です。

実行環境

本記事は、以下の環境で実行しています。

CPUCore i7-10875H
Ubuntu18.04.5
ROSMelodic

C++によるPublisherの記述

C++によるPublisherの記述は下記のように行います。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "publisher_test_cpp");
    ros::NodeHandle nh;
    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("cmd_test", 10);
    ros::Rate loop_rate(10);

    while (false == ros::isShuttingDown()){
        geometry_msgs::Twist cmd;

        cmd.linear.x = 3.0;
        cmd.angular.z = 0.5;

        cmd_pub.publish(cmd);
        loop_rate.sleep();
    }
    return 0;
}

Publisherの解説

コードに関して、簡単に解説をします。まずは、１行目でROSのインストールでインストール済みのヘッダファイルをincludeします。２行目の記述は、今回Publishするロボットの速度指令を格納するTwist型をincludeする記述です。その後で、メイン関数の記述を行っています。メイン関数の引数として記載されている int argc, char** argv はコマンドライン引数と呼ばれます。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char** argv)
{
/*

*/
    return 0;
}

ノード・Publisherの定義

ros::init(argc, argv, "~~~~~~~"); の記述により、ノード名を定義しています。また、NodeHandleの記述により、ノードの名前空間にアクセスができるようになります。Publisherの宣言は、

nh.advertise<Publishしたい変数の型>(”トピック名”, バッファサイズ);

の形で記述をします。ここでは、メイン関数内のwhileループの周期も設定しています。

    ros::init(argc, argv, "publisher_test_cpp");
    ros::NodeHandle nh;
    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("cmd_test", 10);
    ros::Rate loop_rate(10);

whileループ・トピックのPublish

以下の記述で、メイン関数内でwhileループを繰り返します。loop_rate.sleep(); によって、指定した周期で（今回の場合は10Hz）処理が停止されます。C++における変数の定義は、型名　変数名で行います。ここでは、Twist型のcmdという名前の変数を定義して、linear.xとangular.zに値を代入しています。

トピックのPublishには、上で宣言したcmd_pubを使用し、Publisher名.publish(変数名) の形でPublishを実行しています。

    while (false == ros::isShuttingDown()){
        geometry_msgs::Twist cmd;

        cmd.linear.x = 3.0;
        cmd.angular.z = 0.5;

        cmd_pub.publish(cmd);
        loop_rate.sleep();
    }

Subscriberの記述

次にSubscriber（受け取り側）の記載方法です。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

void cmd_callback(const geometry_msgs::Twist& msg_cmd){
    ROS_INFO("vel_linear_x = %3.2f, vel_angular_z = %3.2f", \
    msg_cmd.linear.x, msg_cmd.angular.z);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "subscriber_test_cpp");
    ros::NodeHandle nh;
    ros::Subscriber cmd_sub = nh.subscribe("cmd_test", 10, cmd_callback);

    ros::spin();
    return 0;
}

Callback関数の定義

１・２行目に関しては、Publisherと同様です。Publisherとの違いとして、Python編でも述べた通りCallback関数を定義しています。今回の例の場合は、cmd_testというトピックがPublishされる度に、このCallback関数にmsg_cmdとして入ります。C++での記述方法を見ると、C++　関数の基本を学ぶで解説をした、参照渡しの方法でCallback関数に値を渡していることが分かります。

ここでは、ROS LoggerのROS_INFOを使用して、Subscribeした値を画面出力させています。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

void cmd_callback(const geometry_msgs::Twist& msg_cmd){
    ROS_INFO("vel_linear_x = %3.2f, vel_angular_z = %3.2f", \
    msg_cmd.linear.x, msg_cmd.angular.z);
}

ノード・Subscriberの定義

下記のメイン関数では、Publisherの場合と同様に、はじめにノード名の定義を行います。Subscriberの定義の仕方は、

nh.Subscribe(”トピック名”, バッファサイズ, Callback関数);

の形で定義をします。これによって、指定したトピックがPublishされると、Callback関数に参照渡しされることとなります。複数のトピックを１つのソース内でSubscribeする場合には、トピックの分だけCallback関数を用意しておく必要があります。ros::spin(); の記述で、トピックがPublishされるとCallback関数にアクセスする処理となります。

int main(int argc, char** argv)
{
    ros::init(argc, argv, "subscriber_test_cpp");
    ros::NodeHandle nh;
    ros::Subscriber cmd_sub = nh.subscribe("cmd_test", 10, cmd_callback);

    ros::spin();
    return 0;
}

ros::spinと ros::spinOnce の違い

上記の例では、Publisherのようにメイン関数内に、whileループを記述していません。Callback関数にアクセスする方法は主に２つあり、下表のような関係となっています。

記述方法処理
ros::spin();トピックがPublishされるとCallback関数にアクセス
ros::spinOnce();その行が実行されるとCallback関数にアクセス

よって、ros::spinOnce();の記述方法を用いる場合には、メイン関数内でwhileループを実行して、その中でCallback関数にアクセスする記述をする必要があります。

CMakeListsの設定

C++の記述では、コンパイルを実行する必要があるため、作成したパッケージディレクトリにあるCMakeLists.txtを下記のように編集します。

cmake_minimum_required(VERSION 3.0.2)
project(simple_local_planner)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(publisher_test src/publisher_test.cpp)

add_executable(subscriber_test src/subscriber_test.cpp)

target_link_libraries(publisher_test
  ${catkin_LIBRARIES}
)

target_link_libraries(subscriber_test
  ${catkin_LIBRARIES}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/package_test.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/package_test_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

catkin build

ビルドの方法は、まずTerminalを立ち上げて、下記のコマンドで作成したcatkin_wsに移動し、catkin buildを実行します。

cd catkin_ws

catkin build

作成したスクリプトを実行する

C++で記述した場合には、Pythonと異なりchmodで実行権限を付与する必要はありません。

Terminalで実行する

プログラムを実行するには、下の画像のようにTerminalを３つ立上げ、roscore、rosrun packge_test publisher_test、rosrun packge_test subscriber_testの３つを実行します。注意点として、Pythonの場合はスクリプト名の記述で拡張子.pyを記載しましたが、C++の場合には拡張子の記述は必要ありません。

上手く実行出来ていれば、Subscriberで"/cmd_test"の値を確認することが出来ます。

rqt_graphによる確認

Terminalを立ち上げ、rqt_graphを実行することで、立ち上げたノード間でのトピックのやり取りを可視化することができます。

まとめ

本投稿では、Python編に続いて、C++によるPub＆Sub通信についてまとめました。

PythonとC++の両方で記述ができるよう、参考にして頂ければ幸いです。