概要

ROSの入門編その２です。前回の記事の続きです。

• rosedは単純だったので飛ばしました
• 「シンプルなPublisherとSubscriberを実行してみる」のPython編も飛ばしました
• 動作環境や本記事の目的などは前回の記事をみてください

ROSメッセージ

$ cd ~/catkin_ws/src/beginner_tutorials
$ mkdir msg
# int64のnumというフィールドだけのNumメッセージを作成します
$ echo "int64 num" > msg/Num.msg

• rosed を使って、beginner_tutorialsのpackage.xmlを修正します。

$ rosed beginner_tutorials package.xml
# 以下をそれぞれ追記します
# <build_depend>message_generation</build_depend>
# <exec_depend>message_runtime</exec_depend>

$ cat ~/catkin_ws/src/beginner_tutorials/package.xml
<?xml version="1.0"?>
<package format="2">
  <name>beginner_tutorials</name>
  <version>0.0.0</version>
  <description>The beginner_tutorials package</description>
  <maintainer email="[email protected]">Hiroki Tanaka</maintainer>
  <license>MIT</license>
  <author email="[email protected]">Hiroki Tanaka</author>
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>
</package>

• CMakeLists.txtを編集します

$ rosed beginner_tutorials CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation  # この行を追加
)
catkin_package(
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime  # この行を追加
)
# 以下のblockを追加
add_message_files(
  FILES
  Num.msg
)
# 以下のblockを追加
generate_messages(
  DEPENDENCIES
  std_msgs
)

• ROSメッセージの定義を確認

$ rosmsg show Num
[beginner_tutorials/Num]:
int64 num

サービス（srv）

$ roscd beginner_tutorials
$ mkdir srv
# rospy_tutorialsのAddTwoInts.srvサービスをコピーします
$ roscp rospy_tutorials AddTwoInts.srv srv/AddTwoInts.srv

• CMakeLists.txtを編集します

$ rosed beginner_tutorials CMakeLists.txt

# 以下のblockを追加
add_service_files(
  FILES
  AddTwoInts.srv
)

• srvの定義を確認

rossrv show AddTwoInts
[beginner_tutorials/AddTwoInts]:
int64 a
int64 b
---
int64 sum

[rospy_tutorials/AddTwoInts]:
int64 a
int64 b
---
int64 sum

build

ソースコードを書くまえにいったんbuildします

$ cd ~/catkin_ws
$ catkin_make

Publisher & Subscriber in C++

$ roscd beginner_tutorials/src
$ cat <<EOF > talker.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "talker");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

  ros::Rate loop_rate(10);

  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */
  int count = 0;
  while (ros::ok())
  {
    /**
     * This is a message object. You stuff it with data, and then publish it.
     */
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

    ROS_INFO("%s", msg.data.c_str());

    /**
     * The publish() function is how you send messages. The parameter
     * is the message object. The type of this object must agree with the type
     * given as a template parameter to the advertise<>() call, as was done
     * in the constructor above.
     */
    chatter_pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}
EOF

$ cat <<EOF > listener.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "listener");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
  ros::spin();

  return 0;
}
EOF

• CMakeLists.txtを編集します

$ rosed beginner_tutorials CMakeLists.txt

最終的に以下のようになりました

cmake_minimum_required(VERSION 3.0.2)
project(beginner_tutorials)

## Find catkin macros and libraries
find_package(
  catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

## Generate messages in the 'msg' folder
add_message_files(
  FILES
  Num.msg
)

## Generate services in the 'srv' folder
add_service_files(
  FILES
  AddTwoInts.srv
)

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
)

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

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)

Publisher & Subscriber の実行

• roscoreは実行済です

$ cd ~/catkin_ws
# publisherを実行
$ rosrun beginner_tutorials talker
[ INFO] [1603376210.426369525]: hello world 0
[ INFO] [1603376210.526457373]: hello world 1
[ INFO] [1603376210.626455165]: hello world 2
[ INFO] [1603376210.726559493]: hello world 3
[ INFO] [1603376210.826484491]: hello world 4
[ INFO] [1603376210.926445267]: hello world 5
[ INFO] [1603376211.026510950]: hello world 6
[ INFO] [1603376211.126528268]: hello world 7
[ INFO] [1603376211.226548101]: hello world 8
... (中略) ...
[ INFO] [1603376234.826548893]: hello world 244
[ INFO] [1603376234.926554847]: hello world 245
[ INFO] [1603376235.026547130]: hello world 246
[ INFO] [1603376235.126552281]: hello world 247
[ INFO] [1603376235.226548325]: hello world 248
[ INFO] [1603376235.326537521]: hello world 249
[ INFO] [1603376235.426537503]: hello world 250
[ INFO] [1603376235.526550766]: hello world 251
[ INFO] [1603376235.626521874]: hello world 252
[ INFO] [1603376235.726546779]: hello world 253
[ INFO] [1603376235.826522493]: hello world 254
[ INFO] [1603376235.926551702]: hello world 255
[ INFO] [1603376236.026472538]: hello world 256
[ INFO] [1603376236.126421674]: hello world 257
[ INFO] [1603376236.226488493]: hello world 258
[ INFO] [1603376236.326539414]: hello world 259
[ INFO] [1603376236.426564354]: hello world 260
[ INFO] [1603376236.526488794]: hello world 261
[ INFO] [1603376236.626554490]: hello world 262
[ INFO] [1603376236.726487617]: hello world 263
[ INFO] [1603376236.826548155]: hello world 264
[ INFO] [1603376236.926558722]: hello world 265
[ INFO] [1603376237.026475720]: hello world 266

# 別タブでlistenerを実行
$ rosrun beginner_tutorials listener
[ INFO] [1603376234.827445525]: I heard: [hello world 244]
[ INFO] [1603376234.927214425]: I heard: [hello world 245]
[ INFO] [1603376235.027172208]: I heard: [hello world 246]
[ INFO] [1603376235.127233117]: I heard: [hello world 247]
[ INFO] [1603376235.227291853]: I heard: [hello world 248]
[ INFO] [1603376235.327035815]: I heard: [hello world 249]
[ INFO] [1603376235.427000166]: I heard: [hello world 250]
[ INFO] [1603376235.527069419]: I heard: [hello world 251]
[ INFO] [1603376235.627128705]: I heard: [hello world 252]
[ INFO] [1603376235.727142543]: I heard: [hello world 253]
[ INFO] [1603376235.827062589]: I heard: [hello world 254]
[ INFO] [1603376235.927131465]: I heard: [hello world 255]
[ INFO] [1603376236.026852485]: I heard: [hello world 256]
[ INFO] [1603376236.126615307]: I heard: [hello world 257]
[ INFO] [1603376236.226727480]: I heard: [hello world 258]
[ INFO] [1603376236.327026875]: I heard: [hello world 259]
[ INFO] [1603376236.427189047]: I heard: [hello world 260]
[ INFO] [1603376236.527177079]: I heard: [hello world 261]
[ INFO] [1603376236.627105702]: I heard: [hello world 262]
[ INFO] [1603376236.727174769]: I heard: [hello world 263]
[ INFO] [1603376236.827274873]: I heard: [hello world 264]
[ INFO] [1603376236.927204708]: I heard: [hello world 265]
[ INFO] [1603376237.026893929]: I heard: [hello world 266]

• 別タブで実行状況をwatch

$ rosnode list
/listener  # これが追加されている
/talker  # これが追加されている

$ rostopic list
/chatter  # これが追加されている

$ rosrun rqt_graph rqt_graph
# /talker - /chatter -> /listener を確認

$ rostopic type /chatter | rosmsg show
string data

要点

• C++のコードで、トピック（/chatter）を作成し、stringのメッセージを配信するコード = talker.cpp を作成して起動できました
• C++のコードで、トピック（/chatter）を購読し、stringのメッセージを受け取るコード = listener.cpp を作成して起動できました
• rosのライブラリ群（catkin_LIBRARIES）を使って処理する部分が多く、半分以上おまじないなので、結構ROSのライブラリを知らないと応用できなそうな予感がします

いったん本記事ではここで終了して、
次回「C++でシンプルなサービスとクライアントを書く」から続きをやってみようと思います。

以上です。