写在前面

通过上两节的博客，相信朋友们已经对gazebo的一些标签有了较为深入的认识，但是特别是上一节，程序写完了之后发现并没有什么特别的变化。着实，到目前为止，我们只是很执拗的在rviz和gazebo之间进行显示，但是由于两个模块是完全不一样的，内部的消息机制以及驱动机制都是差的比较多的，所以要想把两者嫁接起来，中间还必须有一个转换这的角色，这就是这个博客要讲的ros_control和plugin。

整体流程框架

依旧祭出官方的框架图，今天其实我们会逐渐的进入到这个框架图的内部，感受这个框架的魅力
整体框架图

提前声明一下，在整张图里面我们基本上不用关注右上角的“Reality”，因为这个框框里面显示的都是真实的硬件设备，由于我们使用的是硬件仿真（也就是gazebo），所以关注点放在下面和左上就可以了。

把大象装进冰箱里的第三步——连接gazebo和ros

回顾上一节的内容，我们在urdf文件中，对一些关节（joint）打上了给gazebo使用的transmission标签，我个人的理解是：transmission把TF（ros）的连接关系与仿真平台上的驱动设备（gazebo）联系在了一起，这样，当物理引擎下，这个驱动设备进行了动作的时候，ros就能知道是哪个TF需要变化；但是同理，我们要明白的一点是：transmission只是将两个标签联系在了一起，但是并不代表ros发送一个消息，gazebo中的该驱动设备就能收到！还是那句老话，ros中机器人形态的变化只需修改对应的TF就可以了，但是gezebo中必须要有物理层面的输入。

那么，为了将两者狠狠的联系起来，勤劳的程序员们就创造了一个中间者——ros_control和gazebo plugin

gazebo plugin

gazebo plugin就是在ros和gazebo之间的一座桥梁了，下面是官方的原话：

Gazebo plugins give your URDF models greater functionality and can tie in ROS messages and service calls for sensor output and motor input. In this tutorial we explain both how to setup preexisting plugins and how to create your own custom plugins that can work with ROS.

可见当你的urdf中有了plugin之后，你的gazebo就与ros之间有了通信的可能，这时候，很多朋友估计想起了上一节中的一个plugin——gazebo_ros_control。其实这个plugin有些特殊，私认为这个plugin其实就是让所有transmission标签生效的一个插件，我们可以看看官方的原话：

In addition to the transmission tags, a Gazebo plugin needs to be added to your URDF that actually parses the transmission tags and loads the appropriate hardware interfaces and controller manager. By default the gazebo_ros_control plugin is very simple, though it is also extensible via an additional plugin architecture to allow power users to create their own custom robot hardware interfaces between ros_control and Gazebo.

可以看到这个插件的作用一方面是要解释transmission标签，另一方面，它要载入合适的hardware interface和controller manager。回到最开始的整体框架图中，我们可以清晰的看到官方将这个plugin与其他的Custom plugins也做了区分，表明说gazebo_ros_control plugin是与其他plugin不同的，而这个plugin可以看到，它专门接管的就是具有transmission的joint，也就是执行器这个部分。因此，私以为这个plugin的作用其实就是控制关节中的执行器运动，同时将运动后的信息传递出来。那么这里需要澄清的一点就是，这个plugin其实并没有将gazebo和ros连接起来（笔者这里所说的连接是指在ros中可以通过话题topic或者服务service来控制），其作用应该是提供了一个能控制gazebo中执行器运动的途径。

以上只是想打消一下各位朋友的疑问，同时也是怕之后在讲真正的plugin（图中的Custom Plugins）的时候，大家心中会一直想着这个plugin。

————————手动分割一下——————————

以下内容就是针对图中的Custom Plugins了，这些plugin就具有将ros和gazebo连接起来的能力，也就是使用了这样的插件之后，你就可以在ros中直接的订阅相应的话题并控制你的机器人了～

让机器人动起来

做了这么长时间的机器人了，一直就是看，这次终于要动起来了，其实只要在urdf中加入能让机器人动作的插件就可以啦，例如官方推荐的这个driver

    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <alwaysOn>true</alwaysOn>
            <updateRate>50.0</updateRate>
            <leftJoint>car_base_front_left_wheel</leftJoint>
            <rightJoint>car_base_front_right_wheel</rightJoint>
            <wheelSeparation>1.0</wheelSeparation>
            <wheelDiameter>0.5</wheelDiameter>
            <torque>1.0</torque>
            <commandTopic>cmd_vel</commandTopic>
            <odometryTopic>odom</odometryTopic>
            <odometryFrame>odom</odometryFrame>
            <robotBaseFrame>base_link</robotBaseFrame>
            <publishWheelTF>true</publishWheelTF>
            <publishWheelJointState>true</publishWheelJointState>
            <legecyMode>false</legecyMode>
            <wheelAcceleration>1</wheelAcceleration>
        </plugin>
    </gazebo>

这里面leftJoint和rightJoint要填写你模型中的名称，然后下面的两个wheel separation和wheel diameter也要根据你的轮子相距的距离和轮子的直径，此时如果你再使用urdf_sim_tutorial包进行显示的时候，在另一个终端下查看ros的话题时，你就能发现多了一个cmd_vel的话题了
rostopic

此时使用下面的指令，你就可以看到小车动起来了～特别提醒，点击view->Wireframe和view->Link Frames能看的更清晰哦：D

rostopic pub -1 /cmd_vel geometry_msgs/Twist -- '[1.0,0,0]' '[0,0,0.0]'

这里要提示一下，因为这个plugin是属于Custom Plugins中的，因此使用的时候和上一节中的gazebo_ros_control那个plugin会冲突，因此在使用前请将gazebo_ros_control的代码注释掉，当然这个plugin也不是没有用的，它结合ros_control照样能让小车动起来，后面会介绍一下。

如果此时朋友们在rviz中观察小车的TF的时候，会发现只有两个轮子的TF是不停更新的，另外两个轮子是不动的，但是gazebo中的轮子确实是四个都在动，原因主要是因为你在这个plugin中只绑定了两个轮子（确切的说是你将publishWheelTF和publishWheelJointState声明为true了，如果是false的话，rviz中轮子的TF也是不会动的哦），gazebo中另外两个轮子动是因为轮子与地面有摩擦力，而ros单纯检测TF，你不发他们的信息，我死活就是不动。

上面的rostopic指令每次只发一次就可以了，因为这个plugin接到之后会一直让joint以一定速度旋转，想让小车停下的话就发送全0的指令就可以啦

————————又手动分割一下——————————

让机器感受到自己的位姿——IMU插件

其实经过上面的这个例子，想必朋友们对plugin已经有一些了解了，想用它很简单，就直接将它“插入”到某个link或者joint中就可以了，刚才我们将diff_drive插入到了joint中，这次我们就将插件“插入”一个link中去。
1. 首先在urdf中创建一个imu的link

    <link name="imu_link">
        <visual>
            <xacro:box_geometry width="${imu_size}" length="${imu_size}" height="${imu_height}"/>
            <material name="yellow"/>
        </visual>
        <collision>
            <xacro:box_geometry width="${imu_size}" length="${imu_size}" height="${imu_height}"/>
        </collision>
        <xacro:default_inertial mass="0.05"/>
    </link>

这里面的参数大家自己定义就好了，这里不再赘述。
2. 随后对这个link插入一个IMU的插件

<gazebo reference="imu_link">
        <gravity>true</gravity>
        <sensor name="imu_sensor" type="imu">
            <always_on>true</always_on>
            <update_rate>100</update_rate>
            <visualize>true</visualize>
            <topic>__default_topic__</topic>
            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
                <topicName>imu</topicName>
                <bodyName>imu_link</bodyName>
                <updateRateHZ>100.0</updateRateHZ>
                <gaussianNoise>0.0</gaussianNoise>
                <xyzOffset>0 0 0</xyzOffset>
                <rpyOffset>0 0 0</rpyOffset>
                <frameName>imu_link</frameName>
            </plugin>
            <pose>0 0 0 0 0 0</pose>
        </sensor>
    </gazebo>

gazebo标签的reference属性就是让gazebo知道随后的代码是依附于谁的，这个reference的内容可以是一个link，也可以是一个joint。特别强调的是，plugin标签一定是要作为sensor标签的一部分，在sensor标签内部出现，否则是不起作用的！！
3. 此时使用urdf_sim_tutorial启动模型，同时使用rostopic查看的时候，就会在topic中发现先一个名为/imu的话题，当然你有100种方法去查看这个值，这里我使用gazebo中的window->Topic Visulization中的IMU消息进行查看，效果如下图所示：
IMU数据
可以看到这个信息给的是很全的了，不仅给除了角速度和加速度，同时连四元数都给你了，简直不能再壕了，不过美中不足的就是这个IMU可谓是一点儿温飘都没有，yaw轴就不会像我们结算出来的那样存在漂移。

tips：当然gazebo还有很多插件提供给开发者，一般情况下我觉得这些插件是完全够用了，具体的插件使用大家可以到这里查看

ros_control

说完gazebo的plugin机制了，我们再来讲一下ros control这个东东，流程图中其实也给这个模块了很大的份量，但是到现在，我们似乎是没有理由在去折腾这个东西了，原因也很简单，上面的两个plugin基本上已经能覆盖我们使用的情况了，机器人也能动了，官方也提供了很多传感器插件了，我们的大象已经装到冰箱里面了呀。粗略的想一下，emmm，是这样的，但是仔细想想，我现在只是能控制差分旋转了，不是差分的呢？像舵机这样的位置控制电机又要怎么办呢？

所以，我们的装大象之旅还没有结束，还需要把这个小尾巴给实实在在的装进去。

————————又双手动分割一下——————————

回想一下，我们在程序中使用transmission标签把joint关节转换成了各式各样的在gazebo下的电机，随后通过对于整个机器人插入了一个叫做gazebo_ros_control的插件使的这些transmission生效，让这些电机与关节对应了起来，但是电机是gazebo中的啊，它即不能发送ros的消息，也不能订阅ros的消息，因此对于ros而言，这些电机是毫无用处的，因为不可读，也不可写；同理，对于gazebo，不管因为什么因素，我的电机转起来了之后，我只管仿真，没有必要每个周期再去计算一下TF变换了多少，整个机器人相对于原点移动了多少。

那么ros_control的作用就呼之欲出了：担任gazebo与ros的翻译者（大神们如果觉得不妥的话，请一定指出），让两个世界彼此可以了解对方。特别需要说明的是，这个翻译者仅仅对有transmission标签的关节有效，如果没有的话，那肯定是不行的。

回到框架图中，除去上面的两个大方框，我们可以看到，当ros的消息传递过来的时候(绿色的方框)，它并没有给gazebo，而是给了一个controller，经过controller中的控制器计算出来相应的值之后，数据流入了JointCommand接口(图下面有一个eg：EffortJointInterface的字样，说明该接口是有许多种的)中，然后由这个接口转交给gazebo模块；当gazebo计算数据完成之后，数据也是先流入一个叫做JointState的接口中，随后又流入了一个controller中，不过这个controller的目的就很明显了，就是计算整个机器人的TF，之后把TF发布出去。

其中不得不说一下最耀眼的Controller Manager，这里先剧透一下说这些controller其实都是通过配置文件的形式载入到ros的参数服务器中（param service），之后通过ros的服务（service）来启动它们，如果每次都这么干的话，势必很浪费时间和精力，因此ros_control就创建一了Controller Manager的东西来帮助我们管理这些服务。

那下面就介绍一下如何使用ros_control让机器人动起来

配置文件

作用

主要就是把controller的配置参数给记录下来，下面的文件就是我工程中的配置文件：

/:
  joint_state_controller:
    type: "joint_state_controller/JointStateController"
    publish_rate: 50

  mobile_base_controller:
    type: "diff_drive_controller/DiffDriveController"
    left_wheel  : [ 'front_left_wheel', 'end_left_wheel' ]
    right_wheel : [ 'front_right_wheel', 'end_right_wheel' ]
    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]

    base_frame_id: base_link

    linear:
      x:
        has_velocity_limits    : true
        max_velocity           : 0.2   # m/s
        has_acceleration_limits: true
        max_acceleration       : 0.6   # m/s^2
    angular:
      z:
        has_velocity_limits    : true
        max_velocity           : 2.0   # rad/s
        has_acceleration_limits: true
        max_acceleration       : 6.0   # rad/s^2

需要注意的就是left_wheel和right_wheel中填写的一定是wheel joint的名称，不要填写为link的名称了。

最后我们再来把所有的命令写为一个launch文件，如下：

<launch>
    <arg name="model" default="$(find ros_gazebo_learn)/urdf/rbo.urdf.xacro"/>
    <arg name="rviz_config" default="$(find urdf_tutorial)/rviz/urdf.rviz"/>
    <arg name="use_gui" default="True"/>

    <!-- rosparam -->
    <param name="robot_description" command="$(find xacro)/xacro.py --inorder $(arg model)"/>
    <!-- joint state publisher -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>

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

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

    <!-- spawn model -->
    <node name="robot_gazebo_spawner" pkg="gazebo_ros" type="spawn_model" args="-urdf -model rbo -param robot_description">
    </node>

    <!-- load controller yaml -->
    <rosparam command="load" file="$(find ros_gazebo_learn)/config/joints.yaml"/>
    <!-- controller manager -->
    <node name="controller_manager" pkg="controller_manager" type="spawner" ns="/"
        args="joint_state_controller mobile_base_controller"/>

    <!-- rviz -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" />

</launch>

这里面每个节点什么含义这里就不做解释了，大家应该也都能看懂，执行起来之后在rostopic下就能看到这样一个话题：
这里写图片描述

其中的/mobile_base_controller/cmd_vel就是驱动所要订阅的话题，我们用rostopic的形式发送命令，就可以看到小车动起来了：

rostopic pub -r 30 /mobile_base_controller/cmd_vel geometry_msgs/Twist -- '[1.0,0,0]' '[0,0,0.0]'

tips：这里我也是介绍的速度控制电机的例子，希望朋友们自己下去可以试一下位置控制电机或者功率控制电机，主要就是熟悉一下需要改什么，怎么改

总结

那其实到这里，gazebo和ros之间就已经联系起来了，这时候你就可以用gazebo做不少事情啦～slam也好，路径规划也好，其实都可以在这上面试一下～

ROS与GAZEBO实时硬件仿真（3）——将gazebo和ros连接起来