MoveIt2のビルドとマニピュレータ「MARA」による動作確認（WMD 2019 in Tokyo 準備編）

 cd ~/moveit2/src

 git clone https://github.com/RyuYamamoto/mara_moveit_tutorials -b wmd_dashing

cd ~/moveit2 && colcon build --merge-install

 cd ~/moveit2

 wget https://raw.githubusercontent.com/AcutronicRobotics/gym-gazebo2/dashing/provision/additional-repos.repos

 vcs import src < additional-repos.repos

 cd ~ && git clone -b dashing https://github.com/AcutronicRobotics/gym-gazebo2

 cd ~/gym-gazebo2 && pip3 install -e .

 git clone https://github.com/openai/gym

 cd gym && pip3 install -e .

3.3 動作確認

無事に環境が構築できたら実際に起動して動作を確認してみます．

まずはじめにGazeboを起動します．

ros2 launch mara_gazebo mara.launch.py

次にmove_groupを立ち上げます．こちらを立ち上げることでMoveItの独立した各モジュールを呼び出して統合し，またユーザーインターフェース側とロボットインターフェース側を繋ぎます．MoveIt1と大きな違いはありません．

ros2 run moveit_ros_move_group move_group

続いてmove_groupに対してプランニングに必要なパラメータを渡すためのlaunchファイルを起動します．

ここで渡すのはロボットのurdfファイルとmoveit_setup_assistantで生成されるsrdfファイルです．urdfはrobot_state_publisherに，srdfはtest_publish_dummy_joint_statesというノードに渡されます．srdfですが，各自の環境でmoveit_setup_assistantにより生成しても良いですが，こちらのレポジトリにmoveit_configがありますので，config内にあるものを使用すると良いと思います．

ros2 launch move_group_interface test_app.launch.py

立ち上げたあとにmove_groupが立ち上がっているターミナルでyou can start planning nowとでていれば成功です．

最後にmove_group_interfaceノードを立ち上げます．こちらのノードはユーザーインターフェース側となり，move_group_interface(C++)を用いて目標姿勢を設定し，プランニングとプランニング結果の実行指令をMoveIt側に送ります．(ちなみに，move_commander(Python)とRvizプラグイン(GUI)はまだROS2対応されていないため，プランニング結果の可視化は出来ません)．

ros2 run move_group_interface move_group_interface

Gazebo上のロボットがプランニング結果に基づき動作すれば成功です．

move_group_interfaceノードの中身のコードについて軽く解説します．

まず以下でMoveGroupクラスのコンストラクタを宣言します．事前に設定したplanning_groupを指定することでMoveGroupの登録を行います．

moveit::planning_interface::MoveGroupInterface::Options options(PLANNING_GROUP, "robot_description", node);
moveit::planning_interface::MoveGroupInterface move_group(options);

MoveGroupに対して目標姿勢を渡すには以下のようにします．

geometry_msgs::msg::Pose target_pose1;
target_pose1.orientation.w = 0.5;
target_pose1.orientation.x = -0.5;
target_pose1.orientation.y = -0.5;
target_pose1.orientation.z = -0.5;
target_pose1.position.x = -0.039;
target_pose1.position.y = -0.156;
target_pose1.position.z = 1.5;
move_group.setPoseTarget(target_pose1);

以下でプランニングとプランニング結果を実行します．

moveit::planning_interface::MoveGroupInterface::Plan my_plan;
move_group.plan(my_plan);
move_group.execute(my_plan);

最後に