🐢

ROS2プログラミング入門 #6 亀を制御する

2022/05/21に公開

C++

この記事では、ROS2プログラミングで亀を制御するノードを作ってみます。

はじめに

ROSを学び始めた人が最初に動かすデモパッケージの1つに turtlesim があります。

ros-foxy-desktop をインストールした時に turtlesim も自動でインストールされているはずなので、ターミナルで次のコマンドを実行すると

ros2 run turtlesim turtlesim_node

次のような感じで「亀」が画面に表示されると思います。

turtlesim_nodeを実行した様子

今回は、この亀 turtlesim_node を自動で動かすノードを作ってみようと思います。

全体像

今のところ、作ろうとしているものの全体像は下図のようになります。

turtle_controller ノードから何かのトピックを介して turtlesim_node ノードへメッセージを送り、そのメッセージによって亀が動く、ということですね。

ノードとトピックを調べる

今回のように動かしたいノードが既にある場合、そのノードを起動してからトピックを調べると分かりやすいと思います。

はじめに、何もノードを起動していない状態で次のコマンドを実行してみましょう。

ros2 node list

ros2 topic list

ノードはなくても /parameter_events と /rosout というトピックはあることが確認できます。

ノードはなくてもトピックはある

続いて、別ターミナルで ros2 run turtlesim turtlesim_node を実行し、もう一度ノードとトピックの状態を調べましょう。そうすると、次のようにノードとトピックの情報が変化します。

turtlesim_nodeの実行でノードとトピックが増えた

ここからまず分かることは、当たり前ですが /turtlesim というノードがあることです。

それから /turtle1/cmd_vel /turtle1/color_sensor /turtle1/pose というトピックが増えたことも見て取れます。さらにコマンドを実行してノードを詳しく調べます。

ros2 node info /turtlesim

次のように表示されると思います。

/turtlesim ノードの情報

これで /turtle1/cmd_vel はサブスクライブ、また /turtle1/color_sensor と /turtle1/pose はパブリッシュしているトピックということが分かりました。

図示すると次のような感じでしょうか。

つまり /turtle1/cmd_vel トピックにメッセージをパブリッシュすると /turtlesim ノードがサブスクライブしてくれる、ということですね。

それでは /turtle1/cmd_vel トピックを詳しく調べてみましょう。

ros2 topic info /turtle1/cmd_vel

次のように表示されると思います。

/turtle1/cmd_vel トピックの情報

結論として /turtle1/cmd_vel トピックへ geometry_msgs/msg/Twist 型のメッセージをパブリッシュすれば /turtlesim がサブスクライブして動いてくれそうな気がしてきました。

試してみましょう。

ノードを作る

これまで同様 my_first_package でソースファイルを作り、ビルド設定ファイルを編集します。

ソースファイル

my_turtle_controller.cpp という名前で、中身は次のように書いてみました。

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("turtle_controller");
  auto publisher = node->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 1);

  rclcpp::WallRate loop(1);
  while (rclcpp::ok()) {
    auto msg = geometry_msgs::msg::Twist();
    msg.linear.x = 1.0;
    msg.angular.z = 1.0;
    publisher->publish(msg);
    loop.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

前回作ったパブリッシャーとかなり似ていて、違うのはノード名、トピック名、メッセージの型、メッセージのデータの作り方ぐらいです。

Twist 型に含まれる liner はXYZ軸における並進速度、もう一方の angular はXYZ軸周りの回転速度と思われます。

今回は円を描くように動かしたかったので linear.x と angular.z に値を設定してみました。

ビルド設定ファイル

変更内容としては、メッセージの型を使えるようにする以下の行と

+ find_package(geometry_msgs REQUIRED)

実行ファイルをビルドするための以下の行を追加しました。

+ add_executable(my_turtle_controller src/my_turtle_controller.cpp)
+ ament_target_dependencies(my_turtle_controller rclcpp geometry_msgs)
+ install(TARGETS my_turtle_controller DESTINATION lib/${PROJECT_NAME})

動作確認

(cd ~/dev_ws/ && colcon build) でビルドして、次のようにコマンドを実行します。

ros2 run my_first_package my_turtle_controller

亀がぐるぐる円を描くように動くはずです。

my_turtle_controllerを実行した様子

おわりに

今回は、ROS2プログラミングで亀を制御するノードを作ってみました。どなたかのお役に立てば幸いです。