ROS在RViz与Gazebo仿真环境下的控制器（ArbotiX、ros_control）配置

本文主要是看了古月老师《ROS机器人开发实践》做得一些学习笔记，其中的代码模板部分也均源于其中。

ArbotiX与ros_control简介

从个人这几天的经历看来（鄙见，可能不具有准确性，仅给大家作参考），ArbotiX与ros_control在ROS中充当的均是控制器的角色，其完成的工作都是发送路径规划信息给电机，电机完成轨迹规划并发布关节状态给RViz或Gazebo用以显示动画。

ArbotiX

《ROS by Example》中把Arbotix称作一种仿真环境，古月老师的《ROS机器人开发实践》中介绍的更为清晰一些：

ArbotiX是一款控制电机、舵机的控制板，并提供相应的ROS包，但是这个功能包的功能不仅可以驱动真实的ArbotiX控制板，它还提供一个差速控制器，通过接收速度控制指令更新机器人的joint状态，从而帮助我们实现机器人在RViz中的运动。

同时，ArbotiX还提供了Joint Trajectory Action Controllers插件，该插件可以完成机械臂的相关电机控制。所以，ArbotiX控制器既能实现移动机器人的差速控制，亦能完成机械臂的关节控制。（本文主要记录控制机械臂的过程，差速轮相对比较容易理解）
ArbotiX主要使用方法如下：

• 创建yaml关节配置文件
• 创建ArbotiX相关节点加载配置文件

ros_control

官方wiki简介如下：

The ros_control packages are a rewrite of the pr2_mechanism packages to make controllers generic to all robots beyond just the PR2.

也就是说ros_control这个包的前身是PR2的一个包，后来将其改为更通用的框架。框架图示如下：

具体的每部分细节可以参见官方wiki或者古月老师博客，我只简单的说下自己的一些思路。ros_control的主要使用方法与ArbotiX套路基本一致，只需记住两点：

• 创建yaml配置文件
• 创建controller_manager控制器节点加载配置文件

控制器的具体配置细节

本文主要代码模板来源于古月老师的《ROS机器人开发实践》一书。

Moveit+RViz+ArbotiX

创建配置文件

ArbotiX提供了Joint Trajectory Action Controllers插件，可以用来驱动机械臂的关节运动，故需要进行机械臂的关节参数配置，便有了如下的配置文件，一般放在模型包的config文件夹下，设取名为arm.yaml：

joints: {
    base_to_armA: {id: 1, ticks: 4096, neutral: 2048, max_angle: 180, min_angle: -180, max_speed: 90},
    armA_to_armB: {id: 2, max_angle: 28, min_angle: -87, max_speed: 90},
    armB_to_armC: {id: 3, ticks: 4096, neutral: 2048, max_angle: 178, min_angle: -58, max_speed: 90},
    armC_to_armD: {id: 4, ticks: 4096, neutral: 2048, max_angle: 180, min_angle: -180, max_speed: 90},
}

controllers: {
    arm_controller: {type: follow_controller, joints: [base_to_armA, armA_to_armB, armB_to_armC, armC_to_armD], action_name: arm_controller/follow_joint_trajectory, onboard: False }
}

文件主要分为两部分，第一部分用joints字典来指定关节名称和电机参数，每个电机参数同样以字典格式指定。joints的名字必须与URDF中设定的名字一致。关于具体电机参数可以参见《ROS by example vol2》的5.3节，在这里简单做个说明：

• id 电机的硬件编号
• ticks 默认值为1024。（个人理解就是类似于编码器的位数）书中提到两种电机，一种最大转动范围为300°，对应ticks为1024，另一种则全周范围转动即360°，对应ticks为4096。
• neutral 默认值为512（1024的一半）。arbotix电机映射到0弧度位置的ticks值，也即在中间位置设为0弧度位（我是这么理解的，可能有误，仅供参考），所以对应上述两种不同电机一般设为512或者2048。书中也有提到neutral设置为其他非中间值的情况。
• max_speed 最大电机允许速度。
• min_angle max_angle 最大最小电机允许角度。

第二部分是控制器的参数设置。

• arm_controller 控制器名字，同样以字典结构存储参数。
• type arbotix节点包含两种类型的控制器，一种是控制差速轮的diff_controller，另一种就是控制关节轨迹（机械臂）的follow_controller。
• joints 以列表形式存储关节名，注意与前面关节名对应。
• action_name 指定arbotix节点接收的消息名称。即要想控制电机运转，必须发送action_name指定的topic名称。
• onboard: 个人理解就是只有在使用真正的ArbotiX控制板的时候该选项为True。
  注：古月老师文件中还配置了末端控制器，用以控制末端张合，由于本文暂未研究末端控制，故未设置末端控制器相关内容。
  看文件可能仍旧一脸懵逼，具体的参数作用表现如下图:
  

joints 字典配置的关节名称将来会对应到topic中的/JOINT_NAME/command这个话题，只需给其发送对应的指令
rostopic pub /armC_to_armD/command std_msgs/Float64 1.0
即可控制对应关节的运动指定弧度。但是这样做是枯燥而乏味的，我们的目的在于结合Moveit的输出开控制整个关节的运动轨迹。所以我们会用到 /arm_controller为前缀的action接口。那么问题来了，如何将Moveit与ArbotiX提供的action对接呢？我们需要一个桥梁，能够将规划轨迹以action的形式发布到ArbotiX的action接口上，问题就解决了。而这个桥梁，便是moveit_simple_controller_manager。下面说这个桥梁怎么使用：
在setup_assistant自动生成的moveit_config包中launch文件夹下有这么一个文件ROBOT_NAME_moveit_controller_manager.launch.xml，该文件默认生成的时候是空的，在这里面可以加载我们的桥梁moveit_simple_controller_manager相关的参数。具体内容如下：

<launch>
    
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" /> 
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />

    
      
    <rosparam file="$(find ROBOT_PACKAGE_NAME_moveit_config)/config/controllers.yaml" />
launch>

文件中出现了一个controllers.yaml配置文件，好烦呀，怎么又有一个配置文件。试想，在这里我们仅仅是加载了配置文件，设置了moveit_controller_manager，貌似确实少了点东西。Moveit的桥梁跑起来了，但是我怎么知道我要把我的轨迹信息发给arbotix节点呢？自然会想到，Moveit发布话题，arbotix订阅同样的话题不就ok了么！确实是这样的，开始我们配置了arbotix的yaml文件，目的是告诉他要接收那些/arm_controller相关的话题，类似地我们应该设置一个yaml文件告诉Moveit要发布跟这些话题相关的信息。所以这个yaml文件的作用确实是这样的。一般在ROBOT_NAME_moveit_config/config文件夹中配置一个controllers.yaml文件来完成这项工作，具体如下：

controller_list:
    - name: arm_controller
      action_ns: follow_joint_trajectory
      type: FollowJointTrajectory
      default: true
      joints:
            - base_to_armA
            - armA_to_armB
            - armB_to_armC
            - armC_to_armD

该文件列出了对应于setup_assistant中每个规划组对应的控制器。name与aciton_ns组成控制器action的接口名，即/arm_controller/follow_joint_trajectory，与前边arbotix订阅的消息接口是一致的，所以桥梁就能发挥作用了。default设置该控制器是否为该规划组的默认控制器，而joints则指明规划组中的关节。
配置文件搞定了，桥梁搭起来了。想想最开始的使用方法：1 配置文件，2 运行节点加载文件，所以下一步该好好利用这些配置了。

加载配置文件，启动节点

ROBOT_PACKAGE_NAME/launch/arbotix_arm_planning.launch

<launch>
    <param name="/use_sim_time" value="false" />

    <arg name="sim" default="true" />

    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ROBOT_PACKAGE_NAME)/urdf/ROBOT_PACKAGE_NAME.xacro' " />

    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen" >
        <rosparam file="$(find ROBOT_PACKAGE_NAME)/config/arbotix_arm.yaml" command="load" />
        <param name="sim" value="true" />
    node>

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
    <include file="$(find ROBOT_PACKAGE_NAME_moveit_config)/launch/move_group.launch" />

   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ROBOT_PACKAGE_NAME)/config/pick_and_place.rviz" required="true" /> 

launch>

如此便可以愉快地跑自己的Moveit相关例程了，当然也可以直接使用RViz的Motion Planning插件直接以GUI方式进行运动规划。

---

Moveit+Gazebo+ros_control

配置完ArbotiX，Gazebo的配置就很容易理解了。套路是一样一样的～～
创建配置文件
ros_control提供了JointTrajectoryController的控制器插件，用接收Moveit发送过来的轨迹信息，然后转换为控制Gazebo关节所需要的信息。
对应于前述的arm.yaml，我们需要配置一个文件告诉ros_control的controller_manager我们要发布什么样类型的信息，关节列表，控制参数等。

ROBOT_PACKAGE_NAME/config/trajectory_control.yaml

arm:
    arm_joint_controller:
        type: "position_controllers/JointTrajectoryController"
        joints: 
            - base_to_armA
            - armA_to_armB
            - armB_to_armC
            - armC_to_armD
        
        gains:
            base_to_armA: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
            armA_to_armB: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
            armB_to_armC: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
            armC_to_armD: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}

同样类比于arm.yaml，通过列出topic我们可以发现，该文件其实指定了发布消息的命名空间与话题名称，即/arm/arm_joint_controller/...。如下图：

同理，对于moveit_simple_controller_manager这个桥梁，我们应该将轨迹信息发布到与控制器接收一致的话题上，自然想到需要对其配置文件进行修改，为了方便运行Arbotix或Gazebo，我们新建配置文件controllers_gazebo.yaml并在ROBOT_PACKAGE_NAME_moveit_controller_manager.launch.xml文件中添加加载Gazebo配置文件的内容。
ROBOT_PACKAGE_NAME_moveit_config/config/controllers_gazebo.yaml

controller_manager_ns: controller_manager
controller_list:
    - name: arm/arm_joint_controller
      action_ns: follow_joint_trajectory
      type: FollowJointTrajectory
      default: true
      joints:
        - base_to_armA
        - armA_to_armB
        - armB_to_armC
        - armC_to_armD

ROBOT_PACKAGE_NAME_moveit_controller_manager.launch.xml

<launch>
    
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" /> 
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />

    
      
    

    
    <rosparam file="$(find ROBOT_PACKAGE_NAME_moveit_config)/config/controllers_gazebo.yaml" />
launch>

加载配置文件，启动节点
ros_control控制器配置完成，自然要进行配置加载
ROBOT_PACKAGE_NAME/config/arm_trajectory_controller.launch

<launch>
    <rosparam file="$(find ROBOT_PACKAGE_NAME)/config/trajectory_control.yaml" command="load" />

    <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/arm" args="arm_joint_controller" />

launch>

加载机器人状态发布节点
ROBOT_PACKAGE_NAME/config/arm_gazebo_states.launch

>
    <!-- 将关节控制器的配置参数加载到参数服务器中 -->
    

    <!-- > -->

    <!-- 运行robot_state_publisher节点，发布Tf -->
    
        
    
>

注：这里加载的joint_state_controller插件是一个ros_control的可选插件，用来发布机器人的关节状态和TF变换，如果不加，当在RViz里面添加RobotModel插件的时候无法显示正确的机器人模型，但是单纯添加MotionPlanning的时候是正常可以规划的。如果添加了该控制器，则要添加如下配置文件
ROBOT_PACKAGE_NAME/config/arm_gazebo_joint_states.yaml

arm:
    # 发布关节状态
        joint_state_controller:
            type: joint_state_controller/JointStateController
            publish_rate: 50

我估计是因为该插件为可选的原因，所以上述都是按古月老师书中的方式先写配置文件然后加载的。其实仔细想想，既然joint_state_controller也是ros_control的插件，在配置和加载arm_joint_control的时候就可以一块加载了。故可以省掉创建arm_gazebo_states.yaml这个配置文件了。然后合并后的trajectory_control.yaml与arm_trajectory_controller.launch，arm_gazebo_states.launch便成了如下的样子：

ROBOT_PACKAGE_NAME/config/trajectory_control.yaml

arm:
    arm_joint_controller:
        type: "position_controllers/JointTrajectoryController"
        joints: 
            - base_to_armA
            - armA_to_armB
            - armB_to_armC
            - armC_to_armD
        
        gains:
            base_to_armA: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
            armA_to_armB: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
            armB_to_armC: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
            armC_to_armD: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    
    # 发布关节状态
    joint_state_controller:
        type: joint_state_controller/JointStateController
        publish_rate: 50

ROBOT_PACKAGE_NAME/config/arm_trajectory_controller.launch

>
    >

    >

>

ROBOT_PACKAGE_NAME/config/arm_gazebo_states.launch

>
    <!-- 将关节控制器的配置参数加载到参数服务器中 -->
    
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/> 
    <rosparam param="/source_list">[/arm/joint_states]rosparam>
  node>
launch>

注意到最后加载了joint_state_publisher节点，关键问题是其/source_list参数，这是一种设置joint_state_publisher话题来源的方式，也即它订阅了/arm/joint_states这个话题，然后发布了/joint_states话题，而/joints_states被move_group订阅，从而Moveit获取到了来自于Gazebo控制器的关节状态信息。

以上看法仅为个人看法，可能存在不妥甚至错误，希望大家不吝赐教，共同学习。