ROS講座104 moveitでオリジナルのアームを使う

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |
| Gazebo | 7.14.0 |
| Qt | 5.5.1 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
オリジナルで作ったロボットアームをmoveitで動かします。

# moveitの設定
moveitで設定ファイルを作るためのアプリとして`roslaunch moveit_setup_assistant setup_assistant.launch`というものがありますが、これでは完全な設定はできません。追加のファイルを作る必要があります。

## setup_assistantでの設定ファイルの作成
`roslaunch moveit_setup_assistant setup_assistant.launch`を実行します。
ここで「Create New MoveIt Configuration Package」を選択します。

![moveit_setup1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/56d5c31c-8982-efe3-4dbd-06d5442a2d1a.png)

「Browsw」を押してurdfファイルを選びます。

![moveit_setup2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/2a495746-aaf9-d81f-c3d4-67840ef2815b.png)

この後に「Add Joints」押して次のページでアームの動作に関するすべての軸を選択します。

「Self-Colisions」で「Generate Collision matrix」を選びます。これは軌道計画をするときにどのリンク同士は衝突計算を行うのい、どの組み合わせは無視するのかを決定します。

![moveit_setup3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/05f753f3-5cdf-e8e9-6328-94964d3d031a.png)

「Planning Groups」では

* Kinematics
    * Group Name：アームのグループの名前を自由に決定します。
    * Kinematic Solver：「kdl_kinematics_plugin/KDLKinematicsPlugin」を選択します。
* OMPL Planning
    * Group Default Planner：「RRTConnect」を選択します。

![moveit_setup4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/23ce8d6d-8946-2276-0b75-6c7c74cb8c17.png)

上記の選択をすると以下のような画面になります。

![moveit_setup5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/38b36d5c-bba3-915b-8baa-e77848652ceb.png)

コントローラーの設定をします。「Add Controller」を押します。

![moveit_setup6.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/bdb54621-882e-9563-1823-78a34d2e0d55.png)

* Controller NameではTrajectory controllerのtopic名になる名前を入れます。
* Controller Typeではeffort_controller/JointTrajectoryControllerを選択します。

この後に「Add Planning Group Joints」を選択してgroup名を選択します。

![moveit_setup7.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/40256216-8041-1317-e8e6-50514b4e04c2.png)

「Auther Information」で項目を埋めます。

![moveit_setup8.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/d2358c15-e17c-ebf5-a14c-58e595c66646.png)

最後に「Configuration Files」で「Generate Package」を選択します。

## ファイルのコピー

以下のファイルを`arm_lecture_moveit_config/config`から`arm_lecture/config/moveit`にコピーします。

```txt:コピーするファイルのリスト
arm_robot.srdf  # アームのメタ情報
joint_limits.yaml　# jointの速度、加速度の上限
kinematics.yaml  #逆運動学のソルバーの設定
ompl_planning.yaml  # 軌道計画のソルバーの設定
```

## 追加のファイル

moveitに出力側のコントローラーの設定をするためのファイルです。

```yaml:arm_lecture/config/moveit/controllers.yaml
controller_manager_ns: controller_manager
controller_list:
  - name: arm_robot_trajectory_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - arm1_joint
      - arm2_joint
      - arm3_joint
      - arm4_joint
      - arm5_joint
      - arm6_joint
```



## launchファイル

```xml:arm_lecture/launch/arm_robot_moveit.launch
<?xml version="1.0" ?>
<launch>
  <arg name="model" default="$(find arm_lecture)/urdf/arm_robot_sim.urdf" />
  <param name="robot_description" textfile="$(arg model)" />

  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>

  <!-- gazebo model -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model arm_robot" />
  <rosparam file="$(find arm_lecture)/config/trajectory_controller.yaml" command="load"/>
  <node name="arm_robot_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="arm_robot_trajectory_controller joint_state_controller"/>

  <!-- common -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- moveit setting -->
  <param name="robot_description_semantic" textfile="$(find arm_lecture)/config/moveit/arm_robot.srdf" />
  <group ns="robot_description_planning">
    <rosparam command="load" file="$(find arm_lecture)/config/moveit/joint_limits.yaml"/>
  </group>
  <group ns="robot_description_kinematics">
    <rosparam command="load" file="$(find arm_lecture)/config/moveit/kinematics.yaml"/>
  </group>

  <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
				       default_planner_request_adapters/FixWorkspaceBounds
				       default_planner_request_adapters/FixStartStateBounds
				       default_planner_request_adapters/FixStartStateCollision
				       default_planner_request_adapters/FixStartStatePathConstraints" />

  <node name="move_group"  pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="">
    <!-- Set the display variable, in case OpenGL code is used internally -->
    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />

    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <param name="planning_scene_monitor/publish_planning_scene" value="true" />
    <param name="planning_scene_monitor/publish_geometry_updates" value="true" />
    <param name="planning_scene_monitor/publish_state_updates" value="true" />
    <param name="planning_scene_monitor/publish_transforms_updates" value="true" />

    <!-- controller config -->
    <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
    <rosparam file="$(find arm_lecture)/config/moveit/controllers.yaml"/>

    <!-- planning config -->
    <param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
    <param name="request_adapters" value="$(arg planning_adapters)" />
    <param name="start_state_max_bounds_error" value="0.1" />
    <rosparam command="load" file="$(find arm_lecture)/config/moveit/ompl_planning.yaml"/>
  </node>

  <!-- Rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find arm_lecture)/config/display.rviz"/>
</launch>
```

注意事項を以下に書きます。

* gazenoのros_controlの名前はmoveitの物をそろえましょう。
* ロボットのurdfを格納するrosparamである`robot_description`の名前は`robot_description`で固定です。moveitのコードの中に絶対パスで読んでいるところがあるので、上位の名前を付けて区別するなどはできません。
* 同じように`robot_description_planning`や`robot_description_kinematics`も固定の名前になります。

# ソースコード（moveitへの指令を出すノード）

通信自体はactionlibなのですが、そこそこ複雑なので直接触るのは難しく、`MoveGroupInterface`というmoveitのAPI経由で使います。

```c++:arm_lecture/src/moveit_commander.cpp
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv) {
  ros::init(argc, argv, "moveit_commander");
  ros::NodeHandle nh;

  ros::AsyncSpinner spinner(2);
  spinner.start();

  // Set up the arm planning interface
  moveit::planning_interface::MoveGroupInterface arm("arm0");

  // Prepare
  ROS_INFO("Moving to prepare pose");
  ROS_INFO_NAMED("tutorial", "Reference frame: %s", arm.getPlanningFrame().c_str());
  ROS_INFO_NAMED("tutorial", "End effector link: %s", arm.getEndEffectorLink().c_str());
  arm.setPlanningTime(0.050);
  arm.setPlannerId("RRTConnect");
  arm.setGoalTolerance(0.01);

  // pose1
  geometry_msgs::PoseStamped pose1;
  pose1.header.frame_id = "base_link";
  pose1.pose.position.x = 0.20;
  pose1.pose.position.y = 0.00;
  pose1.pose.position.z = 0.35;
  pose1.pose.orientation.w = 1.0;

  // pose2
  geometry_msgs::PoseStamped pose2;
  pose2.header.frame_id = "base_link";
  pose2.pose.position.x = 0.30;
  pose2.pose.position.y = 0.10;
  pose2.pose.position.z = 0.35;
  pose2.pose.orientation.w = 1.0;

  // pose3
  geometry_msgs::PoseStamped pose3;
  pose3.header.frame_id = "base_link";
  pose3.pose.position.x = 0.40;
  pose3.pose.position.y = 0.00;
  pose3.pose.position.z = 0.35;
  pose3.pose.orientation.w = 1.0;

  // pose4
  geometry_msgs::PoseStamped pose4;
  pose4.header.frame_id = "base_link";
  pose4.pose.position.x = 0.30;
  pose4.pose.position.y = -0.10;
  pose4.pose.position.z = 0.35;
  pose4.pose.orientation.w = 1.0;

  moveit::planning_interface::MoveItErrorCode ret;

  ROS_INFO("move to WP1");
  arm.setPoseTarget(pose1);
  ret = arm.move();
  if (!ret) {
    ROS_WARN("Fail: %i", ret.val);
  }
  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP2");
  arm.setPoseTarget(pose2);
  ret = arm.move();
  if (!ret) {
    ROS_WARN("Fail: %i", ret.val);
  }
  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP3");
  arm.setPoseTarget(pose3);
  ret = arm.move();
  if (!ret) {
    ROS_WARN("Fail: %i", ret.val);
  }
  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP4");
  arm.setPoseTarget(pose4);
  ret = arm.move();
  if (!ret) {
    ROS_WARN("Fail: %i", ret.val);
  }
  ros::Duration(0.5).sleep();

  ros::shutdown();
  return 0;
}
```

初めのほうはいろいろな設定で、`arm.setPoseTarget(pose)`で対象の位置姿勢を決定します。`ret = arm.move()`で設定に従って問題を解き行動が開始されます。この`move()`はブロッキング関数で、移動が終わるまでブロックされます。返り値は0なら成功で、失敗ならエラーコードが返ってきます。エラーの詳細は[rosmgsのサイト](http://docs.ros.org/jade/api/moveit_msgs/html/msg/MoveItErrorCodes.html)を参照して下しさい。このデモだと-4(CONTROL_FAILED)がまれに出ます。これはgazeboのros_controlのパラメーターがうまく調整されていないので、追従性が悪いために出ます。

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行

```shell:1つ目のターミナル
roslaunch arm_lecture arm_robot_moveit.launch
```

```shell:1つ目のターミナル
rosrun arm_lecture moveit_commander
```

![arm_sim_move.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/183d3edb-5b71-c3c3-9dbb-aea48287c17c.gif)


# 参考
[https://sites.google.com/site/robotlabo/time-tracker/ros/gazebo_mani](https://sites.google.com/site/robotlabo/time-tracker/ros/gazebo_mani)
[MoveGroupInterfaceリファレンス](http://docs.ros.org/melodic/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroupInterface.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)