【Choreonoid tips⑬】ROS1からROS2への移行に伴うシンプルコントローラの変更点

本記事では、Choreonoidのシンプルコントローラ（ロボット制御コントローラ）におけるROS1からROS2への移行に伴う変更点について紹介します。

使用した環境

・Ubuntu 22.04
・Choreonoid 2.2
・ROS2 Humble Hawksbill

ROS2のインストールとChoreonoid関連のビルド

ROS2のインストールとChoreonoid関連のビルド方法については、Choreonoidの公式HPの「ROS2との連携」をご参照ください。
https://choreonoid.org/ja/documents/latest/ros2/index.html

Choreonoid本体をインストールする前に以下のコマンドを実行し、Gitをインストールします。インストールを行っていない場合、git clone時にエラーが発生します。

sudo apt install git

【エラー内容】

git clone https://choreonoid.org/ja/documents/latest/ros2/index.html
コマンド 'git' が見つかりません。次の方法でインストールできます:
sudo apt install git

 
また、choreonoid_rosの依存パッケージのインストールの前に以下のコマンドを実行し、rosdepのインストール、初期化、依存関係を取得するためのデータベースの更新を行ってください。以下のコマンドを実行していないと、rosdep install時にエラーが発生します。

sudo apt install ros-dev-tools
sudo rosdep init
rosdep update

【エラー内容】

rosdep install -y –from-paths ~/ros2_ws/src –ignore-src
ERROR: your rosdep installation has not been initialized yet.  Please run:

sudo rosdep init
rosdep update

TurtleBot3関連パッケージのインストールとダウンロード

以下のコマンドを実行し、TurtleBot3関連のパッケージをダウンロードします。Ubuntu 22.04を使用しているためgit cloneを行う際に、-bオプションを付けて”humble-devel”ブランチを指定しています。

cd ~/ros2_ws/src
sudo apt install -y ros-humble-turtlebot3-msgs
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
rosdep install -y --from-paths ~/ros2_ws/src --ignore-src

.bashrcファイルの編集

以下のコマンドを実行し、ワークスペースセットアップスクリプトの読み込みと環境変数の設定を追加します。今回はTURTLEBOT3_MODELにwaffle_piを指定しています。

echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
echo "export TURTLEBOT3_MODEL=waffle_pi" >> ~/.bashrc

Choreonoidのmaterials.yamlファイルの編集

使用するTurtleBot3モデルのマテリアル設定を追加します。以下のコマンドを実行しmaterials.yamlファイルを開きます。

cd ~/ros2_ws/src/choreonoid/share/default
cp -fp materials.yaml materials_bkyyyyMMdd.yaml

※バックアップファイル作成時のyyyyMMddは、編集年月日を指定します。

gedit materials.yaml

「materials」キー(74行目)と「contact_materials」キー(最終行)に以下を追加します。
・materials

  -
    name: Tb3_tire
    youngs_modulus: 1e10
    roughness: 1.0
    viscosity: 1.0
  -
    name: Tb3_caster
    youngs_modulus: 1e10
    roughness: 1.0
    viscocity: 1.0

・contact_materials

  -
    materials: [ Ground, Tb3_tire ]
    youngs_modulus: 1e10
    friction: 1.0
    restitution: 0.0
    friction_model: [ cone, direct ]
  -
    materials: [ Ground, Tb3_caster ]
    youngs_modulus: 1e10
    friction: 0.1
    restitution: 0.0
    friction_model: [ cone, direct ]

cnoid_turtlebot3_pkgsパッケージのダウンロード

cnoid_turtlebot3_pkgs.zipをダウンロードし、ros2_ws/src以下に移動したらZIPファイルを解凍します。
cnoid_turtlebot3_pkgs
ここでは、ダウンロードディレクトリにcnoid_turtlebot3_pkgs.zipファイルが保存されている場合の手順となります。別のディレクトリに格納している場合は適宜ファイルパスを変更してください。

unzip cnoid_turtlebot3_pkgs.zip
mv ~/ダウンロード/cnoid_turtlebot3_pkgs ~/ros2_ws/src

シンボリック・リンクの追加

cnoid_turtlebot3_pkgsパッケージのモデルファイルでturtlebot3パッケージとturtlebot3_simulationsパッケージのメッシュファイルを参照しているので、以下のコマンドを実行しシンボリック・リンクを追加します。

cd ~/ros2_ws/src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/model/resource/
ln -s ~/ros2_ws/src/turtlebot3/turtlebot3_description/ turtlebot3_description
ln -s ~/ros2_ws/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_world/meshes/ turtlebot3_world

ChoreonoidとTurtleBot3関連パッケージのビルド

以下のコマンドを実行し、ChoreonoidとTurtleBot3関連パッケージのビルドを行います。

cd ~/ros2_ws
colcon build --symlink-install --cmake-args -DBUILD_CHOREONOID_EXECUTABLE=OFF -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

TurtleBot3の遠隔操作

以下のコマンドを実行し、TurtleBot3の遠隔操作用launchファイルを起動します。

ros2 launch cnoid_turtlebot3_bringup waffle_turtlebot_world.launch

別ターミナルを起動し、TurtleBot3の遠隔操作プログラムを起動します。

ros2 run turtlebot3_teleop teleop_keyboard

TurtleBot3の遠隔操作プログラムを起動したターミナル上で、キーボードからw(直進)、 x(後退)、 d(右旋回)、 a(左旋回)、 s(停止)を入力することでTurtleBot3を動かすことができます。

遠隔操作によりChoreonoidのTurtleBot3を操作

ROS1からROS2への移行に伴う変更点

ChoreonoidのシンプルコントローラをROS１からROS2に移行する場合は、以下の点を修正します。
 
①　include文の修正
APIの変更に伴いinclude文を以下のように修正します。

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

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>

 
② C++APIの修正
ROS2では、roscppライブラリの代わりにrclcppライブラリを使用するようになったため、ノードやサブスクライバの宣言は以下のように修正します。

ros::NodeHandle nh_;
ros::Subscriber cmd_vel_sub_;
cmd_vel_sub_ = nh_.subscribe(“cmd_vel”, 1, &TurtleBot3RosController::cmdvelCallback, this);

rclcpp::Node::SharedPtr nh_;
nh_ = std::make_shared<rclcpp::Node>(“TurtleBot3RosController”);
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
cmd_vel_sub_ = nh_->create_subscription<geometry_msgs::msg::Twist>(“cmd_vel”, 1, std::bind(&TurtleBot3RosController::cmdvelCallback, this, std::placeholders::_1));

また、サブスクライバで呼び出しているコールバック関数は以下のように修正します。

void TurtleBot3RosController::cmdvelCallback(const geometry_msgs::Twist& msg)
{
    cmd_vel[0] = msg.linear.x;
    cmd_vel[1] = msg.linear.y;
    cmd_vel[2] = msg.linear.z;
    cmd_vel_th[0] = msg.angular.x;
    cmd_vel_th[1] = msg.angular.y;
    cmd_vel_th[2] = msg.angular.x;
}

void TurtleBot3RosController::cmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    cmd_vel[0] = msg->linear.x;
    cmd_vel[1] = msg->linear.y;
    cmd_vel[2] = msg->linear.z;
    cmd_vel_th[0] = msg->angular.x;
    cmd_vel_th[1] = msg->angular.y;
    cmd_vel_th[2] = msg->angular.x;
}

 
今回は使用していないですが、パブリッシャは以下のように修正します。

ros::Publisher msg_pub_;
msg_pub_ = nh_.advertise<std_msgs::String>(“str_msg”, 10);
std_msgs:: String msg;
msg.data = “Hello World”;
msg_pub_.publish(msg);

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr msg_pub_;
msg_pub_ = nh_->create_publisher<std_msgs::msg::String>(“str_msg”, 10);
std_msgs::msg::String msg;
msg.data = “Hello World”;
msg_pub_.publish(msg);

 
シンプルコントローラ以外でROS1からROS2に移行する場合は上記以外の変更が必要な場合があります。詳細については、以下サイトをご参照ください。
https://docs.ros.org/en/humble/How-To-Guides/Migrating-from-ROS1.html

いかがでしたでしょうか。
ChoreonoidのシンプルコントローラをROS1からROS2へ移行する方の参考になれば幸いです。