C++ を用いた動作計画¶
ロボットをプログラムから動かすための MoveIt! の C++ インタフェースについて見ていきます.
モチベーション¶
前回の演習では MoveIt! の作業セル設定を行い, 動作計画ツールを用いて RViz でロボットを少し動かしました. 今度は動作計画とその実行をプログラムコードから行ってみましょう.
この演習では自身のプログラムから MoveIt! ノードとやり取りするための 基本的な C++ インターフェイスを紹介します. MoveIt! を使用する方法はいろいろとありますが, シンプルなアプリケーションの場合はこれが最も分かりやすい方法です.
追加情報とリソース¶
Scan-N-Plan アプリケーション: 演習問題¶
本演習の目標は
myworkcell_core
ノードを次のように変更することです.
- 視覚ノードへのサービスコールによって通知された部品位置の中央に ロボットのツールフレームを移動します.
Scan-N-Plan アプリケーション: ガイダンス¶
myworkcell_node.cpp
ファイルを編集します.#include <tf/tf.h>
を追加して, フレーム座標変換/ユーティリティのための tf ライブラリにアクセスできるようにします.- 既にこれまでの演習で
tf
パッケージの依存関係を 追加していることを思い出してください.
- 既にこれまでの演習で
ScanNPlan
クラスのstart
メソッドでLocalizePart
サービスからの応答を用いて 新しいmove_target
変数を初期化します.geometry_msgs::Pose move_target = srv.response.pose;
- vision_node のサービスへの呼び出しの 後に このコードを配置してください.
MoveGroupInterface
を用いてmove_target
位置への移動の動作計画・実行を行います.MoveGroupInterface
クラスを使用するにはmoveit_ros_planning_interface
パッケージをmyworkcell_core
パッケージの依存関係として追加する必要があります.これまでの演習同様にパッケージの
CMakeLists.txt
(2行)とpackage.xml
(1行)を変更してmoveit_ros_planning_interface
依存関係を追加してください.適切な「インクルード」参照を追加して
MoveGroupInterface
を使用できるようにします.#include <moveit/move_group_interface/move_group_interface.h>
ScanNPlan
クラスのstart()
メソッドでmoveit::planning_interface::MoveGroupInterface
オブジェクトを作成します.作業セル MoveIt! パッケージ( "manipulator" )の作成時に定義した プランニング・グループの名前を持つ単一のコンストラクタがあります.
moveit::planning_interface::MoveGroupInterface move_group("manipulator");
move_group
オブジェクトのsetPoseTarget
関数を用いて 直交座標系上の意図する目標位置を設定します.オブジェクトの
move()
関数を呼び出して 目標位置への移動の動作計画を行い,実行します.// Plan for robot to move to part move_group.setPoseReferenceFrame(base_frame); move_group.setPoseTarget(move_target); move_group.move();
move()
コマンドのブロック中に ROS メッセージを処理できるようにするためには, ここ に記述があるように,move_group.move()
コマンドに 「非同期の」スピナーを使用する必要があります.下記のように
main()
ルーチンのはじめのあたりでros::init(argc, argv, "myworkcell_node")
の後でスピナーを初期化し, 既存のros::spin()
コマンドをros::waitForShutdown()
に 置き換え ます.ros::AsyncSpinner async_spinner(1); async_spinner.start(); ... ros::waitForShutdown();
システムをテストします.
catkin build roslaunch myworkcell_moveit_config myworkcell_planning_execution.launch roslaunch myworkcell_support workcell.launch
より詳しく見ていきます.
- RViz でトピック "/ar_pose_visual" の "Marker" 表示を追加して,
最終的なロボット位置が
fake_ar_publisher
によって発行された位置 と一致することを確認します. - 動作計画シーケンスを繰り返してみてください.
- MoveIt! RViz インタフェースを用いてアームを "allZeros" の位置に戻します.
- Ctrl+C で
workcell.launch
を停止した後に 再実行します.
workcell_node
のstart
メソッドを更新して AR_target 位置に移動した後,自動的にallZeros
位置に戻るようにしてみてください.move_group
の利用可能なメソッドのリストについては ここ を参照してください.
- 最終的なターゲットへの移動の前に, 目標位置から数インチ離れた「アプローチ位置」に移動してみてください.
- RViz でトピック "/ar_pose_visual" の "Marker" 表示を追加して,
最終的なロボット位置が