使用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_controlleraction_ns: follow_joint_trajectory # 要与之前arbotix中设置的一致type: FollowJointTrajectorydefault: truejoints:- joint1- joint2- joint3- joint4- joint5- name: gripper_controlleraction_ns: gripper_actiontype: GripperCommanddefault: truejoints:- 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 GUIMoveIt!'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也通信成功。