ROS 2でCRANE+V2を動かしてみた 技術情報・開発日誌

CRANE+V2のROS 2パッケージ開発裏話

CRANE+V2のROS 2パッケージ開発裏話 ROS 2でCRANE+V2を動かしてみた

こんにちは、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と呼びます。)

rt-net/crane_plus GitHubページ

rt-net/crane_plus GitHubページ

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_plusの構造

後ほど、これらのパッケージを開発時系列順に紹介していきます。

パッケージのコンセプト

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に主に依存するパッケージはMoveItros_controlでした。
そのため、crane_plusにも同じようにMoveItとros_controlのROS 2バージョンのパッケージが依存するということです。

CRANE-X7とCRANE+V2にはROBOTIS社のDynamixelというサーボモータが使われています。DynamixelはDyanamixel SDKというソフトウェアを用いて動かせます。Dyanmixel SDKはROS 2 Foxyに対応しています。

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はこちら

MoveIt 2とros2_controlはFoxy向けにリリースされていませんが、開発は進んでいます。
rosdistroリポジトリの情報ですが、MoveIt 2はまもなくFoxy 向けにリリースされそうです。(関連プルリクエスト
ros2_controlはリリースの準備が進んでいます。(関連プルリクエスト
来年(早くて1月)からはリリース版のパッケージを使えそうですね。

パッケージの使いかた

crane_plusのインストール方法、使い方については過去のブログ記事を参照してください。

crane_plus開発の裏事情

それではいよいよ本題です。

crane_plus開発時には、ログが残るようにGitHubのプルリクエスト機能を使っていました。

crane_plusのプルリクエスト

crane_plusのプルリクエスト

このログを見ながらお話します。

CRANE+V2のxacroファイルを作成する【crane_plus_description】

最初に作成したパッケージはcrane_plus_descriptionです。
メインの作業はロボットの構造を定義したファイル(URDFxacro)を作成することです。
URDFを作成する際に変数や計算機能を使いたかったので、URDFの拡張バージョンであるxacroを使って記述しました。

xacroファイルを作成できると、このようにCRANE+V2をRViz2に表示させることが出来ます。

crane_plus_descriptionのdisplay.launch.pyを実行

crane_plus_descriptionのdisplay.launch.pyを実行

xacroファイルを作成するには、CRANE+V2の各リンクの長さジョイントの位置ジョイントの回転リミットメッシュファイル(STL)等のデータが必要です。
これらのデータはCRANE+V2の製品ページからダウンロードしました。(正確に言うと、私は社員なので製品を買わなくてもデータを取得できます

最終的に、crane_plus_descriptionパッケージはxacroファイルと、メッシュファイル、モデル描画用のlaunchファイルで構成されています。

関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/1

プルリクエスト:crane_plus_descriptionの作成

プルリクエスト:crane_plus_descriptionの作成

CRANE+V2のモデルをMoveIt 2で動かす【crane_plus_moveit_config】

ROS 2の空間上に(RViz2上に)CRANE+V2を描画できたので、次はこれをMoveIt 2で動かします

moveit2にはアームロボットPandaを動かすデモプログラムがあります。

moveit2 デモプログラム

moveit2 デモプログラム

このプログラムを、crane_plus_descriptionのxacroファイルを読み出すように書き換えて、CRANE+V2を動かしました。

moveit2のデモプログラムを改造してCRANE+V2を動かす

moveit2のデモプログラムを改造してCRANE+V2を動かす

最終的にcrane_plus_moveit_configパッケージは、MoveIt 2のノードを呼び出すlaunchファイルと、moveit2用のパラメータを定義したコンフィグファイルで構成されています。

関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/2

プルリクエスト:crane_plus_moveit_configの作成

プルリクエスト:crane_plus_moveit_configの作成

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が同じように動くかどうか確認しました。

crane_plus_controlのread部分のデバッグ

crane_plus_controlのread部分のデバッグ

その後、サーボモータに目標角度指令を送る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_plus_controlの作成

プルリクエスト:crane_plus_controlの作成

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_gazeboの作成

プルリクエスト:crane_plus_gazeboの作成

サンプルコードの作成【crane_plus_examples】

一通りパッケージが揃ったのでサンプルコードを作成しました。
どうやってmoveit2を扱うのかを理解しやすくするために、C++のサンプルコードを用意しています。

こちらもmoveit2のデモプログラムを参考にして作成しています。

関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/13

プルリクエスト:crane_plus_examples

プルリクエスト:crane_plus_examples

その他の微修正

CRANE+V2をROS 2で動かすところまでは出来ました。しかし、公開するまでには他にも作業をしています。

crane_plus_controlにタイムアウトを追加

USBケーブルや電源ケーブルを抜き差ししたときに、CRANE+V2が暴れると怖いです。
そうならないために通信タイムアウトの処理をcrane_plus_controlに追加しました。

関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/16

crane_plus_controlにタイムアウトを追加

crane_plus_controlにタイムアウトを追加

crane_plus_moveit_configのkinematics_solverを変更

clane_plus_examplesにはpick_and_placeという、手先の目標座標・姿勢を指定して動かすサンプルがあります。

pick_and_placeサンプル

pick_and_placeサンプル

このサンプルを実現するには、手先の目標座標・姿勢から各サーボの目標角度を計算する逆運動学問題を解かなければなりません。
人間が計算するのは大変なので、代わりに解いてくれるのがMoveIt 2です。MoveIt 2のkinematics_solverプラグインが問題を解いてくれます。

kinematics_solverにはデフォルトでKDLKinematicsPluginがセットされているのですが、どうやら自由度が低いロボットでは計算に失敗しやすいようです。

そのため、プラグインをLMAKinematicsPluginに変更しました。

関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/15

プルリクエスト:pick_and_placeサンプルの追加とパラメータ調整

プルリクエスト:pick_and_placeサンプルの追加とパラメータ調整

CIの設定

作成したパッケージが、自分のPC以外でもビルドできるかを確認するためにCIを設定しました。
GitHub Actionsを使用しています。

ビルドが成功するか、linter(コードの見た目を整えるツール)のチェックが通るのかを確認しています。
GitHubにコードをPushするたびにチェックしてくれるので便利です。

GitHub Actionsでのビルド・lintテスト

GitHub Actionsでのビルド・lintテスト

関連プルリクエスト:https://github.com/rt-net/crane_plus/pull/8https://github.com/rt-net/crane_plus/pull/9

プルリクエスト:GitHub ActionsによるCI

プルリクエスト:GitHub ActionsによるCI

プルリクエスト:linterによるコードの検査

プルリクエスト:linterによるコードの検査

今後の展開

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を購入してくれたり、リポジトリにスターを付けてくれたりすると、
明日も仕事を頑張ろうという気持ちになれるのでご検討よろしくお願い致します。

タイトルとURLをコピーしました