概要
gazeboとROS2の連携
gazeboを使う利点の一つは、作成した制御プログラムを実機で動かす前に最低限のデバッグをシミュレーション上で行えることです。制御プログラムはROS2を用いて書かれることが多いです。故に、制御プログラムのデバッグを行う際には、実戦用に作成したROS2のコードでgazebo上のロボットを動かすことができれば都合が良いのです。
この記事の内容
この記事ではgazeboのごく基本的な操作を理解した人向けに、gazebo上のロボットに動力を付けるところから、ROS2を使ってそのロボットを操作するまでを説明します。ただし、公式ドキュメントと重複する内容は基本的に書きませんーその都度公式ドキュメントへのリンクを貼ります。また、本記事を完了して実際に自分自身の開発を始める前に、公式ドキュメントに目を通しておくことー少なくとも目次を見て、何が書いてあるのかを何となく把握しておくこと、を強く推奨します。
この記事では前回の記事で作成した自作ロボットを基に作業を進めていくので、そちらの内容にも目を通しておくとスムーズでしょう。前回の記事の内容やロボットのモデル、この記事で扱った内容を実装したコードは私のgithubに公開しています。前回の内容はリンク先のgz_ws/MakeYourRobotに、今回の内容に相当する部分はリンク先のgz_ws/MoveTherobot及びros2_ws/nav_devに収められています。
また、記事の内容に誤りや不備等ございましたら、ご指摘いただけると幸いです。
大まかな手順
sdfファイルを編集してgazeboモデルに動力を追加します。その後は、ROS2を使ってロボットを操作するための簡単なパッケージを作ります。PCのキー入力でロボットを操作できるようにしますが、その際には既存のキー入力取得用パッケージを用います。今回に限らずROS2では様々な機能のパッケージがネット上に多く公開されているので、積極的に活用していきましょう。その後はROS2からのメッセージをgazeboのメッセージに変換する(ブリッジする)方法を説明し、ROS2のプログラムでgazebo上のロボットを動かしていきます。
機能的には以上でこの記事の内容は終了ですが、今後の開発をスムーズにするためにlaunchファイルを用いるやり方を説明します。さらに、ROS2とgazeboのメッセージのブリッジもcofigファイルを作成してまとめて作成できるようにしていきます。
動作環境・バージョン
動作環境
Ubuntu22.04 ：デュアルブートした実機PC
VScode ：コーディングとターミナルの操作は基本的にVScodeで行った
バージョン
ROS2 : humble
gazebo : ignition fortless ：ROS2公式がhumbleとの連携相手として推奨するバージョン
前提条件
以下のような読者を想定して記事を執筆しました
・上記の動作環境またはそれと同等の機能を持つ環境を有すること
・ 基本的なubuntuの操作が理解できること
・ROS2についての基本的な知識があること
・gazeboのごく基本的な知識があること
コンテンツ
gazebo上のロボットに動力を付ける
sdfフォーマットにおける駆動部
sdfフォーマットではCADファイルを読み込んで、面倒なモデリング無しにロボットを表示させることができますが、それにも限界があります。それは、可動部のデータはCADファイルからは読み取ることはできないということです。つまり、ロボットをシミュレーター上で動かしたければ、駆動部は自分自身でsdfファイルを書いて定義する必要があります。理想論を言えば、ロボットのCADファイルを作成する段階で駆動ブロックごとに別々に出力して、そのジョイントをsdfファイルで定義するのが良いです。しかし、それは面倒なので、今回は既にCADファイルからインポートしたモデルに強引に車輪とキャスターを取り付けて無理やり動くようにしていきたいと思います。勿論、そうすることはシミュレーションの正確性を損ないますが、ある程度のズレを許容するならこのやり方が楽でしょう。
駆動部の取り付け
駆動部の取り付けですが、例によって公式チュートリアルが充実しているので、こちらを読んでsdfの書き方を理解して下さい。今回の記事で作成するロボットは差動二輪ロボットで、チュートリアルが扱っているのも差動二輪ロボットです。なので、基本的にはチュートリアルで行われていることを、前回のモデルに対して行えばよいということになります。
詳しくは公式チュートリアルを参照してもらいたいですが、以下に簡単な手順を示します。
先ず、車輪を追加しましょう。
<!-- Wheel left -->
<link name="wheel_left_link">
<pose>0.0 0.095 0.023 -1.57 0 0</pose>
<inertial>
<inertia>
<ixx>1.1175580e-05</ixx>
<ixy>-4.2369783e-11</ixy>
<ixz>-5.9381719e-09</ixz>
<iyy>1.1192413e-05</iyy>
<iyz>-1.4400107e-11</iyz>
<izz>2.0712558e-05</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="wheel_left_collision">
<geometry>
<cylinder>
<radius>0.033</radius>
<length>0.018</length>
</cylinder>
</geometry>
</collision>
<visual name="wheel_left_visual">
<geometry>
<cylinder>
<radius>0.033</radius>
<length>0.018</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
</link>
忘れずにジョイントも定義します。
チュートリアルでは回転のリミットを定義していますが、指定しなくても動作します。
<joint name="wheel_left_joint" type="revolute">
<parent>base_link</parent>
<child>wheel_left_link</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
右の車輪も同様に追加して下さい。
車輪の次はキャスターです。githubのコードではCADモデルに近づけるため２つ付けていますが、別に１つでも(この記事の趣旨においては)問題ありません。ジョイントの定義もまとめて示します。
<link name='caster_back_right_link'>
<pose>0.15 -0.05 -0.004 -1.57 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0.000</ixy>
<ixz>0.000</ixz>
<iyy>0.00001</iyy>
<iyz>0.000</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<visual name='caster_back_right_visual'>
<geometry>
<sphere>
<radius>0.005</radius>
</sphere>
</geometry>
<material>
<ambient>0.0 1 0.0 1</ambient>
<diffuse>0.0 1 0.0 1</diffuse>
<specular>0.0 1 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.005</radius>
</sphere>
</geometry>
</collision>
</link>
<joint name='caster_back_right_joint' type='ball'>
<parent>base_link</parent>
<child>caster_back_right_link</child>
</joint>
次に差動二輪の動きを再現するためのプラグインを定義します。このプラグインはmodelタグ内に書くことに注意して下さい。
<!-- Differential drive -->
<plugin filename="libignition-gazebo-diff-drive-system.so"
name="ignition::gazebo::systems::DiffDrive">
<left_joint>wheel_left_joint</left_joint>
<right_joint>wheel_right_joint</right_joint>
<wheel_separation>0.19</wheel_separation>
<wheel_radius>0.033</wheel_radius>
<topic>cmd_vel</topic>
</plugin>
自分自身のロボットを作成している読者は、自身の定義したジョイント合わせてパラメータを変更して下さい。
さらに、車輪間の距離と車輪の半径にも適切な値を設定して下さい。
次に、worldタグ配下にキー入力に応じてロボットに司令を送るためのプラグインを定義して下さい。
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress"> <!--受信するトピックを指定-->
<match field="data">16777235</match> <!--↑キーに一致するという条件を指定-->
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel"> <!--送信するトピックを指定-->
linear: {x: 0.5}, angular: {z: 0.0} <!--トピックの内容を指定-->
</output>
</plugin>
これは↑キーが押された際の挙動を書いたものです。同様に、→←↓キーの入力に対しても挙動を定義します。
設定するパラメータは以下を参考にして下さい。
Left ➞ 16777234 ➞ linear: {x: 0.0}, angular: {z: 0.5}
Up ➞ 16777235 ➞ linear: {x: 0.5}, angular: {z: 0.0}
Right ➞ 16777236 ➞ linear: {x: 0.0}, angular: {z: -0.5}
Down ➞ 16777237 ➞ linear: {x: 0.5}, angular: {z: 0.0}
以上のことを済ませた状態で一旦シミュレーションをスタートしてみると、ロボットが奈落へ落下していくのが見えると思います。地面を作っていませんでした。以下のように地面を作成しましょう。
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
ここまでの内容を実装したsdfファイルはgithubのgz_ws/MoveTheRobot/にあります。
以上のことが完了したら、いよいよ準備完了です。world.sdfのあるディレクトリに移動してからign gazebo world.sdfでgazeboを起動しましょう。
開始ボタンを押す前にー別に押した後でも問題ありませんが、右上の検索バーにkeyとでも入力してkey publisher選択し、起動して下さい。これをやらないとキー入力をgazeboが認識してくれません。
開始ボタンを押して、例えば↑キーを押したらロボットが前に動き出しましたか？
※ロボットは動き出したらキーから指を離しても止まらないと思います。これは仕様であり、お使いのgazeboは正常です。
操作用のROS2パッケージを作成する
パッケージ作成
では、先程のロボットをRSO2からの司令によって動かすためにプログラムを書いていきましょう。ROS2のワークスペースに移動して新しいパッケージを作成して下さい。今回私はnav_devというパッケージを作成しました。パッケージを作ったら忘れないうちにCMakeファイルを書き換えるのがオススメです。ament_cmake_autoを用いると大幅にCMakeの記述を短縮できますが、パッケージを作成した段階ではこれが設定されていません。
Cmakeとpackageの例
cmake_minimum_required(VERSION 3.8)
project(nav_dev)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_auto_package()
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav_dev</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="jwithelis@gmail.com">n622/jwith</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rosidl_default_generators</depend>
<depend>rclcpp</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
パッケージのインポート
さて、現在はキー入力をROS2側で取得し、それに応じたメッセージを発信するROS2nodeの作成を目指しています。キー入力に応じたメッセージの発信は簡単ですが、キー入力をリアルタイムで取得するのは少々面倒なタスクだと言えます。ROS2に限らず、何らかの開発をしているとこういう状況にはよく遭遇しますが、本質的でない面倒なタスクに対しては既存のプログラムを活用できないか検討すべきです。今回はnek009様のkey_eventパッケージを発見しました。このパッケージを活用させて頂きましょう。リンク先のREADMEにインストールの仕方と、動作確認の仕方が書いてあります。記述に従ってパッケージをworkspase/src配下にインストールし、tests nodeを動かして動作確認を行って下さい。
teleop nodeの作成
さて、動作確認が済んだらいよいよ今回使うnodeを作成していきましょう.。key_event_msgsとkey_event_nodes以外は今回使用しないのでワークスペースから削除して下さい。パッケージのsrc配下にteleop_node.cppを作成して下さい。
先ず理解すべきは先程インストールしたキー入力取得用パッケージの仕様です。これについては、パッケージを作成してくださった方の説明を読むしかありません。今回ではおおよそ次のようになっています。
- key_event_nodeを起動する
- キー入力を検出したら、key_evet_nodeが
key_event_msgs::msg::KeyEvent型のメッセージをpublishする
つまり、キー入力に対応してロボットへ司令を送るためには、key_event_msgs::msg::KeyEvent型メッセージのsubsucliberを作成し、そのコールバックの中でロボットへの司令をpublishすれば良い、ということになります。
ロボットへの司令は何型にすればよいのか？と思われるでしょうが、ここでは一旦geometry_msgs::msg::Twist型を使うのだと了解してください。
さて、以上のことを実装したコードが以下になります(main関数の一部があとから追記するので欠けています)。
#include <functional>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp_components/register_node_macro.hpp"
//使いたいパッケージのincludeディレクトリ以下から、欲しいヘッダファイルまでのパスを書く
//もしincludeディレクトリがなければ、相手パッケージのCMakeファイルにincludeを作るよう記述する
#include "key_event_msgs/msg/key_event.hpp"
using namespace std::chrono_literals;
class TeleopNode : public rclcpp::Node {
public:
//送信用のメッセージを宣言
geometry_msgs::msg::Twist message = geometry_msgs::msg::Twist();
TeleopNode(
const std::string& name_space="",
const rclcpp::NodeOptions& options = rclcpp::NodeOptions()
) : Node("teleop_node",name_space,options) {
publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
auto subscribe_callback = [this](const key_event_msgs::msg::KeyEvent::SharedPtr msg) -> void {
//キーボードの入力から進路を決定
switch (msg->key)
{
//cahr型なのでシングルクオーテーションを用いる必要があります
case 's':
message.linear.x = -1;
message.angular.z = 0;
break;
case 'w':
message.linear.x = 1;
message.angular.z = 0;
break;
case 'a':
message.linear.x = 0;
message.angular.z = 1;
break;
case 'd':
message.linear.x = 0;
message.angular.z = -1;
break;
default:
message.linear.x = 0;
message.angular.z = 0;
break;
}
this->publisher_->publish(message);
};
//キーボードの値取得用のsubscriber
sub_ = this->create_subscription<key_event_msgs::msg::KeyEvent>(
"key_hit_event",
rclcpp::QoS(10),
subscribe_callback
);
}
private:
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
rclcpp::Subscription<key_event_msgs::msg::KeyEvent>::SharedPtr sub_;
};
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::shutdown();
return 0;
}
重要なことは、使用したいメッセージ型を以下のようにしてincludeしていることです。
#include "geometry_msgs/msg/twist.hpp"
#include "key_event_msgs/msg/key_event.hpp"
さらに、この処理を実行するために、package.xmlにも追記が必要です。
<depend>geometry_msgs</depend>
<depend>key_event_msgs</depend>
以上の手順によって、メッセージをsubscribeして、それに対応する別のメッセージをpublishするというnodeの処理を実装できます。
geometry_msgsの仕様
geometry_msgsの使い方について少し補足しておきます。
message.linear.x は前後方向の速度を表し、
message.angular.z は左右方向の角速度を表します
nodeの複数起動
さて、以上でteleop_nodeの作成はほぼ完了しました。しかし、もう一つだけ考えるべきことが残っています。このnodeが動作するためには、key_event_nodeが作動していることが必要です。勿論、ターミナルを２つ開いてros2 runコマンドを二回打つことで実現できますが、余り手間はかけたくありません。実はmain関数内の記述を少し工夫すれば、一回のros2 runコマンドで２つのnodeを起動できます。
先ず、package.xmlに次の依存関係を追記します<depend>key_event_nodes</depend>。
続いて、cppファイルに次のincludeを追記します#include "key_event_nodes/key_hit_event_node.hpp"。
それができたら、main関数を以下のように記述することで一度のros2 runコマンドで２つのnodeが起動するようになります。
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor exec;
//共有ライブラリから呼び出し
auto node1 = std::make_shared<key_event::KeyHitEventNode>();
exec.add_node(node1);
auto node2 = std::make_shared<TeleopNode>();
exec.add_node(node2);
exec.spin();
rclcpp::shutdown();
return 0;
}
但し、このような運用をするためには、呼び出される側のnodeでincludeファイルを作成するようにCMakeを設定する必要があります。
teleop_node.cppの実装はgithubのros2_ws/src/nav_dev/src/teleop_node.cppにあります。
nodeの起動
最後にCMake.txtに実行ファイルを追加して完了です
ament_auto_add_executable(
teleop_node src/teleop_node.cpp
)
ros2 run nav_dev teleop_nodeでnodeを起動しましょう。ros2 topic echo /cmd_velを使えば、適当なキーを押した際に/cmd_velからトピックが送信されていることが確認できます。
ROS2とgazeboのメッセージをブリッジする
ROS2 node の作成は完了しました。しかし、gazebo上のロボットはgazeboのignition.msgs.Twist型のメッセージを受け取って動くようにできており、ROS2のgeometry_msgs/msg/Twist型のメッセージには反応しません。そこで、ROS2のメッセージとgazeboのメッセージを相互に変換してくれるros_ign_bridgeを使用します。詳しくはgazeboの公式チュートリアルを参照してください。
今回では、ros2 run ros_ign_bridge parameter_bridge /cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twistを実行すればメッセージをブリッジできます。
teleop_nodeを起動した上で、上記のコマンドを実行してください。
ign topic -lで/cmd_velがpublishされていることが確認できます。
さらに、ign topic -e -t /cmd_velでgazebo側にメッセージが送られてくることが確認できます。
ここまでくれば、ロボットをROS2で操作することが可能です。gazeboとROS2 node、ign_ros_bridgeを立ち上げてteleopの成功を確認しましょう。
ROS2とgazeboをlaunchファイルで起動する
さて、teleopの成功を確認したあなたはその結果に一先ず安堵しながらも、この程度のことを行うためにターミナルを３つも起動して、ign_ros_bridgeの長いコマンドを打ったりするのは面倒だ、と思ったことでしょう。実際に開発を行う際には、何度もこの作業ーgazebo. ROS2, ign_ros_bridgeの立ち上げ、を行うわけですから、これは相当非効率です。なので、launchファイルというものを作成して、これらの操作を一つのコマンドで行えるようにしましょう。
launchファイルとは、ROS2を起動する際に実行する指示を一つにまとめておくファイルです。launchファイルを指定すれば起動時の操作をROS側が読み込んで勝手に行ってくれるようになります
シンプルなlaunchファイル
先ずは、そこの記述を参考にして以下のようなtelep nodeを起動する単純なlaunchファイルを作成しましょう。パッケージにlaunchディレクトリを作成して下さい。そこに、nav_teleop.launch.pyというファイルを作成して下さい。launchファイルはpythonではなくxmlファイルで書くこともでき、実際そのようにしている記事もネット上には少なからずあるのですが、pythonによるlaunchファイルの方が処理の自由度が高いので、pythonを使いましょう。
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
def generate_launch_description():
teleop_node = Node(
package='nav_dev',
executable='teleop_node',
output='screen',
#別ターミナルで起動する設定
prefix="xterm -e"
)
return LaunchDescription([
teleop_node,
])
xtermのインストール
一箇所だけ注意が必要な所があります。それは、今回のteleop_nodeはkey_event_nodeの仕様上、teleop_nodeを起動したターミナルでキー入力を行う必要があるということです。これは、現段階では何の問題もありませんが、今後gazeboなどをlaunchファイルで起動するようになって、launchのログがターミナルに流れて来るようになったときに、ログとキー入力が混ざるという事態を引き起こします。気にしなければそれまでなのですが、今回はxtermを用いてこれを解決します。恐らく、皆さんはxtermをインストールしていないでしょうから、sudo apt install xtermでインストールして下さい。
起動確認
最後に、CMakeファイルに以下の文言を追記して、launchファイルがROS2によって認識できるようにします。
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)
これは、共有ディレクトリにパッケージ内のlaunchディレクトリをインストールするという設定で、今後も何かROS2に認識させたいディレクトリができたら、その都度ここに書き足していきます。
それができたら、ros2 launch nav_dev nav_teleop.launch.pyを実行してください。ros2 node listを使ってteleop_nodeが立ち上がっていることを確認しましょう。
環境変数の設定
それでは、gazeboも同時に起動できるように、このlaunchファイルに追記していきます。
先ず、ターミナルを立ち上げるたびにgazeboの環境変数を設定するのは面倒なので、この宣言もlaunchファイルに書いてしまいます。
pkg_share_dir = get_package_share_directory('nav_dev')
model_path = os.path.join(pkg_share_dir, "models")
#ignition gazeboがモデルにアクセスできるように設定
ign_resource_path = SetEnvironmentVariable(
name='IGN_GAZEBO_RESOURCE_PATH',value=[
os.path.join("/opt/ros/humble", "share"),
":" +
model_path])
ここでは、環境変数としてnav_devパッケージ内のmodelsディレクトリを指定しています。ROS2と連携した開発を行う際は、モデルも同じパッケージ内にあると何かと便利なので、移動しておきましょう。
CMakeファイルのinstallにmodelsを追記するのを忘れないようにして下さい。
install(DIRECTORY launch models
DESTINATION share/${PROJECT_NAME}
)
gazeboの起動
gazeboの起動設定は以下のようになります
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
world_name = LaunchConfiguration('world_name', default='world')
#ignition gazeboの起動設定
ign_gz = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_ign_gazebo'),
'launch', 'ign_gazebo.launch.py')]),
launch_arguments=[('ign_args', [' -r -v 3 ' +
os.path.join(model_path, "world.sdf")
])])
この実装ではパッケージ内のmodelsディレクトリをworld.sdf読み込むことになっていますので、そこにworld.sdfを移動しておきます。meshディレクトリも忘れすに移動しましょう。
ros_ign_bridgeの起動
ros_ign_bridgeの起動設定は以下のようになります
#ros_ign_bridgeの起動設定
bridge = Node(
package='ros_ign_bridge',
executable='parameter_bridge',
parameters=[{'use_sim_time': use_sim_time}],
arguments=[
# Velocity command (ROS2 -> IGN)
'/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist',
],
remappings=[
("/odom/tf", "tf"),
],
output='screen'
)
起動
launchファイルにあるreturnの内容を以下のようにして下さい。
return LaunchDescription([
ign_resource_path,
ign_gz,
DeclareLaunchArgument(
'use_sim_time',
default_value=use_sim_time,
description='If true, use simulated clock'),
DeclareLaunchArgument(
'world_name',
default_value=world_name,
description='World name'),
bridge,
teleop_node,
])
以上でlaunchファイルの記述も完了です。ros2 launch nav_dev teleop_node.launch.pyで先程と同様にteleopができることを確認して下さい。当然ですが、colcon buildをしないとlaunchファイルに加えた変更は反映されないことに留意して下さい。
ros_ign_bridgeのconfigファイル
さて、今書いたlaunchファイルでは、ros_ign_bridgeで変換するメッセージはargumentsとしてコマンドを書くようにして指定していました。今はまだ変換するメッセージが１つなので問題ありませんが、将来的に扱うメッセージ数が増えてくると煩雑になっていくことが予想されます。これはよろしくないので、configファイルを書いて、変換するメッセージの指定はそちらで行うこととします。また、そうすることで変換するメッセージのテンプレートを作成することにもなるので、一石二鳥です。
パッケージ内にconfigディレクトリを作成して、そこにnav_teleop.yamlファイルを作成して下さい。yamlファイルはxmlファイルの親戚みたいなもんだと思って下さいー見た目は随分異なってますが。そして、以下の内容を貼り付けて下さい。
---
- ros_topic_name: "/cmd_vel"
gz_topic_name: "/cmd_vel"
ros_type_name: "geometry_msgs/msg/Twist"
gz_type_name: "ignition.msgs.Twist"
direction: ROS_TO_GZ
各パラメータの意味は名前の通りです。そして、先程のlaunchファイルのign_ros_bridgeの部分を以下のように変更します。
#ros_ign_bridgeの起動設定
bridge = Node(
package='ros_ign_bridge',
executable='parameter_bridge',
parameters=[{
#brigdeの設定ファイルを指定
'config_file': os.path.join(pkg_share_dir, 'config', 'nav_teleop.yaml'),
},{'use_sim_time': use_sim_time}],
output='screen'
)
忘れずにconfigファイルを共有ディレクトリにinstallしてください。それが済んだら、ランチファイルを実行して問題なくteleopできることを確認して下さい。
以上の実装を済ませたlaunchファイルはgithubのros2_ws/src/nav_dev/launch/teleop.launchにあります。
今回のディレクトリ構成
今回の記事に従って作業をした場合、以下のようなディレクトリ構成になっているはずです
WorkSpace/
src/
nav_dev/
config/
teleop.yaml
launch/
nav_teleop.launch.py
models/
meshes/
2DRoboPrac.stl
2DRoboPrac.dae
world.sdf
src/
teleop_node.cpp
CMakeList.txt
package.xml
トラブルシューティング（追記求む）
一般論
・とりあえずsudo apt updateする
・gazeboのゾンビ化を疑う
gazeboは乱暴な閉じ方をしたときーそして丁寧に行った場合でもそれなりの頻度で、正常に終了せずプロセスがゾンビ化します
先ず、ps aux|grep ignで検索をかけます。正しく終了できていれば２つのプロセスが表示されます。
３つ以上ならプロセスのゾンビ化が発生しています。kill -9 <プロセス番号>で最初と最後のプロセス以外を強制終了します。
プロセスがゾンビ化しているときの代表的な挙動
・モデルやワールドの記述を変更したのに反映されていない
・何故かロボットが２つ表示される
・シミュレーションの開始ボタンのオンオフが高速で切り替わる
個別の対策
ロボットが跳ねる
モデルが見つからない或いは読み込めないと言われる
環境変数が正しく設定できているか確認しましょう。
printenv IGN_GAZEBO_RESOURCE_PATHで環境変数を確認できます。
間違っていた場合は、export IGN_GAZEBO_RESOURCE_PATH="Paht/to/your/models/directry"で正しい環境変数を指定して下さい。
sdfファイルでモデルを読み込む際のパスが正しいかも確認しましょう
↑のprintenvで出力されたパスに続くような形で書かれていますか？
configファイルが読み込めない或いは全く動作していないように見える
先ず、CMakeでconfigディレクトリをincludeしていることを確認して下さい
次に、<workspace>/install/<package>/share/<package>/config以下にconfigファイルが生成されていることを確認して下さい
稀にですが、CMakeに書いたincludeがcolcon buildで反映されないことがあります。その際はcolcon build --cmake-clean-cacheのオプションでビルドを行ってください。
終わりに
この記事ではgazebo上のロボットに動力を追加し、ROS2と連携させて簡単な操作ができるようにしました。前回の自作ロボットのCADファイルを読み込んでgazeboに表示するからは大分進歩したと言えるでしょう。
次回はRvizを使用してロボットの状態を可視化する(gz)です。rvizはgazeboと並ぶ可視化ツールですが、よりロボットの制御プログラム開発に適した機能が提供されています。興味があれば是非こちらもご覧ください。
またこれらの記事の全体像が知りたければ、ignition gazebo+Rviz2の開発環境構築も覗いてみてください。ここにはgazeboとrvizに関する私の書いた記事がまとめられています。
この記事で扱った内容を実装したコードは私のgithubに公開しています。今回の内容に相当する部分はリンク先のgz_ws/MoveTheRobot及びros2_ws/nav_devに収められています。