moveitとの連携

[yasuohayashibara]

moveitとの接続を行う．
Gazebo上ではアームの制御ができているため実機で行えるようにKondo_driver側の整備を行う．

[jsupratman13]

moveitと連携するためにkondo_driverからFollowJointTrajectoryインターフェースを使用す。
以下の通りを行う：

Moveit側

1. 以下のようにcontroller.yamlのファイルを作成する.ここでnameやjointsのはkondo_driver に書いてあるyamlファイルと実効するlaunchファイルのcontroller_spawnerの設定が同じであること

controller_manager_ns: ''
controller_list:
  - name: manipulator/left_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - l_sholder_pitch_joint
      - l_sholder_roll_joint
      - l_elbow_roll_joint
      - l_wrist_roll_joint
      - l_wrist_pitch_joint
      - l_wrist_yaw_joint

2. moveit_config/launchファルダにある<ロボット名>_moveit_controller_manager.launch.xmlが空っぽなので以下を記入する

<launch>
	<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
	<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
	<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

	<!-- Load joint controller configurations from YAML file to parameter server -->
	<rosparam file="$(find cit_moveit_config)/config/controllers.yaml"/>
</launch>

3. 以下の通りに新たなlaunchファイルを作成する。実際moveitを実行うするファイル

<launch>
  <include file="$(find <パッケージ名＞_moveit_config)/launch/planning_context.launch" >
    <arg name="load_robot_description" value="true" />
  </include>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

  <include file="$(find <パッケージ名＞_moveit_config)/launch/move_group.launch">
    <arg name="publish_monitored_planning_scene" value="true" />
    <remap from="joint_states" to="manipulator/joint_states" />
  </include>

  <include file="$(find <パッケージ名＞_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
  </include>
  
  <include file="$(find <パッケージ名＞_moveit_config)/launch/default_warehouse_db.launch" />

</launch>

Kondo_driver側

サンプルでは各jointをコントローラとして設定していますが、これ別のJointTrajectoryコントローラに変えます

4. controller_sample.yamlを以下の通りに改造します。driverの形はそのままで大丈夫です。

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 10

manipulator:
  left_arm_controller:
    type: position_controllers/JointTrajectoryController
    joints:
      - l_sholder_pitch_joint
      - l_sholder_roll_joint
      - l_elbow_roll_joint
      - l_wrist_roll_joint
      - l_wrist_pitch_joint
      - l_wrist_yaw_joint
    action_monitor_rate: 10
    constraints:
      goal_time: 1.0
      stopped_velocity_tolerance: 1.0
      l_sholder_pitch_joint:
        trajectory: 1.0
        goal: 1.0
      l_sholder_roll_joint:
        trajectory: 1.0
        goal: 1.0
      l_elbow_roll_joint:
        trajectory: 1.0
        goal: 1.0
      l_wrist_roll_joint:
        trajectory: 1.0
        goal: 1.0
      l_wrist_pitch_joint:
        trajectory: 1.0
        goal: 1.0
      l_wrist_yaw_joint:
        trajectory: 1.0
        goal: 1.0

5. B3Mを実行するlaunchファイルは基本kondo_driver.launchと同じ形であるが以下のところを改善するだけ
   

ここの部分を

<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--shutdown-timeout 0.1 joint_state_controller joint_0_controller"/>

これに変える

<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--shutdown-timeout 0.1 joint_state_controller /manipulator/left_arm_controller"/>
<param name="trajectory_execution/execution_duration_monitoring" value="false"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

/manipulator/left_arm_controllerは4)で変更したyamlの名前が同じにする。

実行

実行する時に初めに5)で改造したkondo_driverを実行するlaunchファイルを実行してから後に3)で作ったmoveitを実行するlaunchファイルを実行する

roslaunch kondo_driver <5.で作ったファイル>.launch
roslaunch <パッケージ名>_moveit_config <3.で作ったファイル>.launch

参考：
http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/controller_configuration.html
http://wiki.ros.org/Industrial/Tutorials/Create_a_MoveIt_Pkg_for_an_Industrial_Robot