使用MoveIt！+Arbotix控制六自由度机械臂

写在前面

目前还是在ros基础学习阶段，进度比较慢，控制对象是在某宝上买的一款六自由度机械臂，现在只是搭建了模型，按着参考书上的步骤一步一步走，还没到控制实物的部分。
现在发现基础真是非常重要，刚开始学习ros时，被一开始的各种通信机制topic、message、action等等啊弄得一头雾水，到底是书读百遍其义自见啊，重复看参考书源代码、ros.wiki和各位大神的文章，至今还是略懂皮毛，距离我能控制实物还有较长一段路要走，还有很多坑要跳。不管怎么说，知识和能力都是在跳坑和解决问题中长进的，加油坚持吧！

书接上回，六自由度机械臂ROS+Rviz+Arbotix控制器仿真中提到了arbotix虚拟控制器，这个控制器实现joint trajectory action server，通过手写client创建一个action客户端和arbotix中的服务端进行连接，action消息类型为FollowJointTrajectoryAction。

MoveIt！搭配Arbotix

如果能够通过MoveIt！实现运动规划，并且将规划结果通过FollowJointTrajectoryAction发送给机器人的Arbotix关节控制器……
所以在MoveIt！上层规划和Arbotix关节控制器之间需要一个将两者结合的接口，这个接口在MoveIt！中同样以插件的形式提供，成为moveit_simple_controller_manager。这个manager可以提供FollowJointTrajectoryAction接口，将规划轨迹以action的形式发布。

上面这张图真的非常重要！
关注上图英文中提到的两个信息。
/joint_state topic被move_group监听，move_group并不发布机器人的关节信息，这必须在机器人上实现。move_group只监听tf，tf信息由机器人发布，需要在机器人上运行robot_state_publisher节点。这些对理解下文的launch文件很有帮助。

使用Setup Assistant进行配置之后，会生成一个myrobot_moveit_config功能包，里面的demo.launch是演示启动文件，包括启动joint_state_publisher节点（发布虚拟机器人的关节状态）、robot_state_publisher节点（发布tf）、加载fake_controllers.yaml等。运行这个启动文件后，得到的节点图如下。
MoveIt！搭配Arbotix的做法就是将Arbotix控制器取代fake_controller，具体做法就是添加yaml配置文件，代码如下：

controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory # 要与之前arbotix中设置的一致
    type: FollowJointTrajectory
    default: true
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
  
  - name: gripper_controller
    action_ns: gripper_action
    type: GripperCommand
    default: true
    joints:
      - finger_joint1

然后修改myrobot_moveit_controller_manager.launch.xml文件如下，这个文件与fake_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 controller_list -->
  <!-- Arbotix -->
  <rosparam file="$(find myrobot_moveit_config)/config/myrobot_controllers.yaml"/>
  <!-- Gazebo -->
  <!-- <rosparam file="$(find marm_moveit_config)/config/controllers_gazebo.yaml"/> -->
</launch>

了解MoveIt!中的action

使用moveit！控制真实机械臂（4）——了解moveit！所使用的action
这篇文章写的挺好，里面挺重要一点就是，关于action接口，moveit扮演的角色是客户端，服务端才是我们需要编写的部分。搭配arbotix进行理解，moveit就是客户端，arbotix就是服务端，之间的action消息类型为FollowJointTrajectoryAction。

修改demo.launch

原自动生成的demo.launch代码如下：

<launch>

  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" />
  <!-- Allow user to specify database location -->
  <arg name="db_path" default="$(find marm_moveit_config)/default_warehouse_mongo_db" />

  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />

  <!--
  By default, hide joint_state_publisher's GUI

  MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
  The latter one maintains and publishes the current joint configuration of the simulated robot.
  It also provides a GUI to move the simulated robot around "manually".
  This corresponds to moving around the real robot without the use of MoveIt.
  -->
  <arg name="use_gui" default="false" />

  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find marm_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- If needed, broadcast static tf for robot root -->
  

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
  </node>

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find marm_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find marm_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(find marm_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  </include>

</launch>

参考这篇文章对上述代码进行修改。另外，由于原demo.launch启动了joint_states_publisher，用来发布模拟的关节状态，而arbotix也能发布joint_states，因此需要把上述代码中有关joint_states_publisher的代码注释掉，同时添加下述代码：

  <!-- we have a fake robot connected, and use the arbotix to pulish joint states -->
 <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
    <rosparam file="$(find myrobot_description_ultimate)/config/myrobot_arm.yaml" command="load"/>
    <param name="sim" value="true"/>
  </node>

  <node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller" output="screen">
    <rosparam file="$(find myrobot_description_ultimate)/config/myrobot_gripper.yaml" command="load" />
	  <param name="sim" value="true"/>
  </node>

运行修改后的demo.launch，得到的节点图如下
终端会得到下面的输出，说明arbotix控制器添加成功，与moveit也通信成功。