こんにちは、shotaです。
本稿はROS/ROS2 advent calenderの19日目の記事です。
私はこれまでにCRANE+ V2(クラインプラスブイツー)のROS 2パッケージに関する記事をいくつか投稿してきました。(詳しくは下記リンクを参照してください)
今回はROS 2パッケージを公開するまでの裏事情をお話します。
この記事がMoveIt 2やros2_controlを使ってROS 2パッケージを作りたい人の参考になると嬉しいです。
(製品の宣伝少なくてごめんなさい to 会社の人)
パッケージの概要
CRANE+ V2のROS 2パッケージはrt-net/crane_plusとしてGitHubに公開されています。(以下、このパッケージをcrane_plusと呼びます。)
crane_plusは以下のパッケージで構成されています。
- crane_plus_control【CRANE+ V2を制御するパッケージ】
- crane_plus_description【CRANE+ V2のモデル情報を格納するパッケージ】
- crane_plus_examples【MoveIt 2を使うサンプルコード集】
- crane_plus_gazebo【Gazeboシミュレーション用のパッケージ】
- crane_plus_moveit_config【MoveIt 2用のパッケージ】
図で示すとこのような構造になっています。
後ほど、これらのパッケージを開発時系列順に紹介していきます。
パッケージのコンセプト
crane_plusの開発コンセプトは「crane_x7_rosとほとんど同じパッケージにすること」です。
弊社にはCRANE-X7という別のアームロボットがあり、rt-net/crane_x7_rosというROSパッケージを公開しています。(2020年12月時点では、ROS 2パッケージはありません)
crane_plusでROS 2化のノウハウ(主にMoveIt 2とros2_controlの使い方)を習得し、そのノウハウをもとにcrane_x7_rosをROS 2化する、というのが作戦で、crane_plusの開発コンセプトの背景です。
ということで、crane_x7_rosのパッケージ構成を知っていれば、それを真似してROS 2化するだけでcrane_plusは完成します。(もちろん、そう簡単には進みませんでした)
ちなみに、開発するパッケージのROS 2バージョンは最新のROS 2 Foxyに決めました。
パッケージの依存関係の開発状況
crane_x7_rosをベースにcrane_plusを作るにあたって、まずは依存パッケージの調査からはじめました。
crane_x7_rosに主に依存するパッケージはMoveItとros_controlでした。
そのため、crane_plusにも同じようにMoveItとros_controlのROS 2バージョンのパッケージが依存するということです。
MoveIt 2の開発状況
MoveItのROS 2バージョンはros-planning/moveit2としてGitHubに公開されています。
MoveIt 2は、crane_plus開発時点ではROS 2 Foxy向けにリリースされていません。
そのため、ソースファイルをビルドして使用することになります。
依存パッケージはapt(もしくはrosdepコマンド)でインストールできたほうがユーザにとっては便利ですが、無いものは仕方ないです。
MoveIt 2のビルド方法はMoveIt公式ページに書かれているので、ちょっと手間はかかりますがビルドして動かせます。
MoveIt 2をビルドする際には、いくつかの依存パッケージもソースビルドします。
どのパッケージをダウンロードしてビルドするのかは、moveit2.reposというファイルに書かれています。
repositories: moveit2: type: git url: https://github.com/ros-planning/moveit2 version: main moveit_msgs: type: git url: https://github.com/ros-planning/moveit_msgs version: ros2 # ----- 省略 ----- # TODO(#283): Switch to master after fixing breaking changes in ros2_control ros2_control: type: git url: https://github.com/ros-controls/ros2_control version: 2c3aad1cb8ad4d23925bceeb5c26e22d6cf859ef ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers version: 8587e079490acdd0fabeb2742a3bec3bf551bbc5 # ----- 省略 -----
なんと、都合が良いことにros_controlのROS 2バージョンである、ros2_controlもダウンロードされます。
ros2_controlの開発状況
ということで、ros2_controlもFoxy向けにリリースされていませんでした。
またmoveit2.reposファイルを見ると、ros2_controlのversionには、コミットのハッシュ値が指定されていることがわかります。
これを言い換えると、最新バージョンのros2_controlではMoveIt 2(もしくはMoveIt 2のデモプログラム)が動作しない、ということです。
最新のros2_controlで動作しない理由は、ここ数ヶ月でros2_controlのインタフェースに変更があったためです。
よって、いずれはMoveIt 2も、crane_plusも最新のros2_controlに対応するためにコードを修正しなければなりません。
関連Issueはこちら。
rosdistroリポジトリの情報ですが、MoveIt 2はまもなくFoxy 向けにリリースされそうです。(関連プルリクエスト)
ros2_controlはリリースの準備が進んでいます。(関連プルリクエスト)
来年(早くて1月)からはリリース版のパッケージを使えそうですね。
パッケージの使いかた
crane_plusのインストール方法、使い方については過去のブログ記事を参照してください。
crane_plus開発の裏事情
それではいよいよ本題です。
crane_plus開発時には、ログが残るようにGitHubのプルリクエスト機能を使っていました。
このログを見ながらお話します。
CRANE+ V2のxacroファイルを作成する【crane_plus_description】
最初に作成したパッケージはcrane_plus_descriptionです。
メインの作業はロボットの構造を定義したファイル(URDF、xacro)を作成することです。
URDFを作成する際に変数や計算機能を使いたかったので、URDFの拡張バージョンであるxacroを使って記述しました。
xacroファイルを作成できると、このようにCRANE+ V2をRViz2に表示させることが出来ます。
xacroファイルを作成するには、CRANE+ V2の各リンクの長さ、ジョイントの位置、ジョイントの回転リミット、メッシュファイル(STL)等のデータが必要です。
これらのデータはCRANE+ V2の製品ページからダウンロードしました。(正確に言うと、私は社員なので製品を買わなくてもデータを取得できます)
最終的に、crane_plus_descriptionパッケージはxacroファイルと、メッシュファイル、モデル描画用のlaunchファイルで構成されています。
関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/1
CRANE+ V2のモデルをMoveIt 2で動かす【crane_plus_moveit_config】
ROS 2の空間上に(RViz2上に)CRANE+ V2を描画できたので、次はこれをMoveIt 2で動かします。
moveit2にはアームロボットPandaを動かすデモプログラムがあります。
このプログラムを、crane_plus_descriptionのxacroファイルを読み出すように書き換えて、CRANE+ V2を動かしました。
最終的にcrane_plus_moveit_configパッケージは、MoveIt 2のノードを呼び出すlaunchファイルと、moveit2用のパラメータを定義したコンフィグファイルで構成されています。
関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/2
CRANE+ V2の実機を動かす【crane_plus_control】
いよいよCRANE+ V2の実機を動かします。このパッケージの作成に一番時間がかかりました。
crane_plus_controlを作成する際にros-controls/ros2_controlと、JafarAbdi/fake_jointというパッケージを参考にしました。
ros2_controlには(当たり前ですが)ros2_controlの使い方が書かれているので参考になります。
fake_jointはMoveIt 2のソースビルド時にダウンロードされるパッケージです。
こちらはros2_controlの実装例として参考にしました。
最初にCRANE+ V2のサーボモータの角度情報を取得するread部分を実装し、実機のCRANE+ V2を動かして、RViz2上のCRANE+ V2が同じように動くかどうか確認しました。
その後、サーボモータに目標角度指令を送るwrite部分を作成し、RViz2上でCRANE+ V2を動かして、実機のCRANE+ V2が同じように動くかどうか確認しました。
最終的にcrane_plus_controlはros2_controlを扱うためのノードと、launchファイル、ros2_controlとDynamixel用のコンフィグファイルで構成されています。
関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/3
CRANE+ V2をGazeboシミュレータで動かす【crane_plus_gazebo】
さて、これまで一切触れてこなかったGazeboシミュレータの話です。
結論を言うと、Gazebo上でCRANE+ V2を動かせませんでした。
その理由は、gazebo_ros2_controlのリリース版が出ていないためです。
やはりこちらのパッケージも、ros2_controlの安定版が出るのを待っているようです。(関連Issueはこちら)
ということで、Gazebo空間上にCRANE+ V2のモデルを表示させるところまで作業し、公開しています。
関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/5
サンプルコードの作成【crane_plus_examples】
一通りパッケージが揃ったのでサンプルコードを作成しました。
どうやってmoveit2を扱うのかを理解しやすくするために、C++のサンプルコードを用意しています。
こちらもmoveit2のデモプログラムを参考にして作成しています。
関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/13
その他の微修正
CRANE+ V2をROS 2で動かすところまでは出来ました。しかし、公開するまでには他にも作業をしています。
crane_plus_controlにタイムアウトを追加
USBケーブルや電源ケーブルを抜き差ししたときに、CRANE+ V2が暴れると怖いです。
そうならないために通信タイムアウトの処理をcrane_plus_controlに追加しました。
関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/16
crane_plus_moveit_configのkinematics_solverを変更
clane_plus_examplesにはpick_and_placeという、手先の目標座標・姿勢を指定して動かすサンプルがあります。
このサンプルを実現するには、手先の目標座標・姿勢から各サーボの目標角度を計算する逆運動学問題を解かなければなりません。
人間が計算するのは大変なので、代わりに解いてくれるのがMoveIt 2です。MoveIt 2のkinematics_solverプラグインが問題を解いてくれます。
kinematics_solverにはデフォルトでKDLKinematicsPluginがセットされているのですが、どうやら自由度が低いロボットでは計算に失敗しやすいようです。
そのため、プラグインをLMAKinematicsPluginに変更しました。
関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/15
CIの設定
作成したパッケージが、自分のPC以外でもビルドできるかを確認するためにCIを設定しました。
GitHub Actionsを使用しています。
ビルドが成功するか、linter(コードの見た目を整えるツール)のチェックが通るのかを確認しています。
GitHubにコードをPushするたびにチェックしてくれるので便利です。
関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/8、https://github.com/rt-net/crane_plus/pull/9
今後の展開
clane_plusは一旦完成しましたが、これからも作業は続きます。
まず、ros2_controlとMoveIt2のリリースに合わせた更新作業をします。
特にros2_controlのAPIが変わるので、crane_plus_controlの中身が大きく変わると思います。
次に、crane_plus_examplesにサンプルを追加します。
MoveIt 2の新機能であるMoveIt Servoを試してみたいです。
そして、最終的にはCRANE-X7のROS 2対応を実施する予定です。
まとめ
CRANE+ V2のROS 2パッケージを開発して公開するまでの裏事情をお話しました。
ROS 2ユーザやコントリビュータの皆様に、何か一つでも参考にしていただけると嬉しいです。
また、この記事をキッカケにCRANE+ V2を購入してくれたり、リポジトリにスターを付けてくれたりすると、
明日も仕事を頑張ろうという気持ちになれるのでご検討よろしくお願い致します。