ROS仿真：ros_control+gazebo

用ROS仿真机器人时，无外乎传感器和机器人，而这些硬件在gazebo中都集成了很好的插件。

传感器仿真

传感器仿真分为两步，首先在urdf中建立传感器的模型，然后在urdf中指定传感器对应的gazebo插件即可（注意在gazebo中传感器是依附在某一个连杆上的）。

基本上常见的传感器都可以在gazebo中找到对应的插件，使用的方法也很简单。配置如下

<robot>
  ... robot description ...
  <link name="sensor_link">
    ... link description ...
  </link>

  <gazebo reference="sensor_link">
    <sensor type="camera" name="camera1">
      ... sensor parameters ...
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        ... plugin parameters ..
      </plugin>
    </sensor>
  </gazebo>

</robot>

Gazebo中提供的常见传感器列表如下：

Camera： provides ROS interface for simulating cameras such as wge100_camera by publishing the CameraInfo and Image ROS messages as described in sensor_msgs.
Multicamera： synchronizes multiple camera’s shutters such that they publish their images together. Typically used for stereo cameras, uses a very similar interface as the plain Camera plugin and currently only supports stereo cameras.
Depth Camera: simulates a sensor like a Kinect, which is duplicated in the Kinect plugin.
GPU Laser: simulates laser range sensor by broadcasting LaserScan message as described in sensor_msgs.
Laser: the non-GPU version of GPU Laser, but essentially uses the same code.
Block Laser: provides grid style laser range scanner simulation (e.g. Velodyne).
Force: ROS interface for applying Wrench (geometry_msgs) on a body in simulation.
IMU: simulates IMU sensor. Measurements are computed by the ROS plugin or by Gazebo.

机器人仿真

讲机器人仿真之前，先来介绍一下ros_control，因为这个模块和机器人仿真有非常大的关系。

ROS_CONTROL

ros_control是ros为机器人开发者提供的机器人控制中间件，包含一系列控制器接口，传动装置接口，硬件接口，控制器工具等，帮助机器人应用快速落地，提高开发效率。整体框架如下：

可以看到整个ros_control分成了五个部分，每个部分的功能解释如下。

controller magager控制器管理器: 作用就是加载控制器以及控制器对应的参数；比如通过yaml和launch文件可以加载一个二连杆机械臂的控制器。其中yaml文件指定了控制器的名称和参数，launch文件中通过node name="controller_spawner" pkg="controller_manager" type="spawner" args="..."/>来加载yaml中指定的控制器。

# MYROBOT_control/config/rrbot_control.yaml
rrbot:
    # Publish all joint states -----------------------------------
    joint_state_controller:
        type: joint_state_controller/JointStateController
        publish_rate: 50  

    # Position Controllers ---------------------------------------
    joint1_position_controller:
        type: effort_controllers/JointPositionController
        joint: joint1
        pid: {p: 100.0, i: 0.01, d: 10.0}
    joint2_position_controller:
        type: effort_controllers/JointPositionController
        joint: joint2
        pid: {p: 100.0, i: 0.01, d: 10.0}

# MYROBOT_control/launch/MYROBOT_control.launch
<launch>
    <!-- Load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find rrbot_control)/config/rrbot_control.yaml" command="load"/>
    <!-- load the controllers -->
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
        output="screen" ns="/rrbot" args="joint1_position_controller joint2_position_controller joint_state_controller"/>
    <!-- convert joint states to TF transforms for rviz, etc -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
        respawn="false" output="screen">
        <remap from="/joint_states" to="/rrbot/joint_states" />
    </node>
</launch>

controller控制器: 就是上面yaml文件中type标签对应的类型，ros提供的控制器列表如下

位置控制器:
- position_controller/JointPositionController: 输入关节位置，输出关节位置；
- position_controller/JointGroupPositionController: 输入关节组位置，输出关节组位置；
速度控制器:
- velocity_controller/JointVelocityController: 输入关节速度，输出关节速度；
- velocity_controller/JointGroupVelocityController: 输入关节组速度，输出关节组速度；
- velocity_controller/JointPositionController: 输入关节位置，输出关节速度，输入输出之间用PID调节；
力矩控制器:
- effort_controller/JointEffortController：输入关节力矩，输出关节力矩；
- effort_controller/JointGroupEffortController: 输入关节组力矩，输出关节组力矩；
- effort_controller/JointVelocityController：输入关节速度，输出关节力矩，输入输出之间用PID调节；
- effort_controller/JointPositionController：输入关节位置，输出关节力矩，输入输出之间用PID调节；
- effort_controller/JointGroupPositionController: 输入关节组位置，输出关节组力矩，输入输出之间用PID调节；
关节状态控制器:
- joint_state_controller/JointStateController: 关节状态控制器，作用是获取关节状态；
关节轨迹控制器：可以理解成对关节位置，速度，力矩控制器的二次封装，作用是控制整个机器人关节组，和上面的JointGroup***Controller类似，但是上面的控制器提供的接口都是topic，而关节轨迹控制器提供的接口既有action，又有topic，便于和moveit等交互
- position_controller/JointTrajectoryController: 关节组轨迹位置控制器，输入关节组轨迹位置，输出关节组位置；
- velocity_controller/JointTrajectoryController: 关节组轨迹速度控制器，输入关节组轨迹位置和速度，输出关节组速度，中间用PID+feedforward；
- effort_controller/JointTrajectoryController: 关节组力矩控制器，输入关节组轨迹位置和速度，输出关节组力矩，中间用PID+feedforward；
其他控制器：其他还有ackermann_steering_controller（专门针对汽车的），diff_drive_controller（差分移动机器人的）等，具体可以看各自的wiki介绍。
hardware resource interface layer硬件接口: 这一层是软硬件的分界线，使得软硬件解耦。一方面接收硬件的信息如关节数据，另一方面可以将控制器计算出来的关节控制指令如力矩发送给硬件，目前ros提供的hardwareInterface接口如下，其实就是对应电机的三种运动模式（这个代表电机输入的信号类型，和上面的ros_controller的输出类型要一致）：
- hardware_interface/EffortJointInterface
- hardware_interface/VelocityJointInterface
- hardware_interface/PositionJointInterface
hardware interface硬件抽象: 该层的作用是兼容硬件多样性，同时为上层控制器提供与硬件交互的统一接口，具体功能可以是控制关节约束，力矩转换和状态转换等功能；
hardware硬件: 具体的机器人硬件；

这里讲解ros_control架构的时候，是自上而下介绍的，上层的输出一般代表下层的输入。比如ros_controller的输入是用户上层应用，如moveit用FollowJointTrajectory来控制机械臂，或者用户想控制某个关节速度；ros_controller控制器的输出就是hardware_interface指定的类型，要么就是力矩，速度，位置，经过一些后处理发送给实际的机器人或者gazebo。

ROS_CONTROL + GAZEBO

有了上面对ros_control的理解之后，再来看一下其如何与gazebo仿真建立联系，两者之间的关系图如下

可以看到实际硬件和gazebo仿真的差异体现在4和5，即硬件抽象和具体硬件层。在仿真中，gazebo里面的机器人就代表硬件，左边的模块4为ros和gazebo之间沟通的桥梁。在该模块中，通过插件将ros和gazebo连接起来，具体的配置在下面会介绍。

一般机器人仿真

机器人仿真的过程也就是配置ros_control+gazebo的过程。配置过程和ros_control的讲解过程相反，是自下而上的，具体如下：

设置link标签：gazebo要求每个实际的link都要有inertial标签，同时link中的visual标签在gazebo中没用，需要单独配置零件的颜色；

<!-- Base Link -->
<link name="link1">
    <collision>
        <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
        <geometry>
            <box size="${width} ${width} ${height1}"/>
        </geometry>
    </collision>

    <visual>
        <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
        <geometry>
            <box size="${width} ${width} ${height1}"/>
        </geometry>
        <material name="orange"/>
    </visual>

    <inertial>
        <origin xyz="0 0 1" rpy="0 0 0"/>
        <mass value="1"/>
        <inertia
            ixx="1.0" ixy="0.0" ixz="0.0"
            iyy="1.0" iyz="0.0"
            izz="1.0"/>
    </inertial>
</link>

<gazebo reference="link1">
    <material>Gazebo/Orange</material>
</gazebo>

设置joint标签：设置关节的阻尼等参数，以便进行动力学仿真；同时设置transmission标签，用来表示电机和关节的关系；为每个关节和电机配置hardwareInterface硬件接口，即上图中的模块3；

<joint name="joint2" type="continuous">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 ${width} ${height2 - axel_offset*2}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.7"/>
</joint>

<transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="$link1">
        <hardwareInterface>hardwareInterface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
        <hardwareInterface>hardwareInterface/PositionJointInterface</hardwareInterface> # indigo之后的版本可以不对电机设置，但必须对joint设置
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>

加载libgazebo_ros_control.so插件：即上面的模块4的一部分，插件的作用一方面是解析在urdf设置的transmission和hardwareInterface标签，加载合适的硬件接口，另一方面是ros和gazebo通讯的桥梁；之前研究过libgazebo_ros_control.so插件的源码，里面把关节运动模式分成：EFFORT，POSITION，VELOCITY，POSITION_PID，VELOCITY_PID五种
- EFFORT：ros_controller输出的是力矩，该插件原封不动的用该力矩在gazebo中驱动关节；
- POSITION_PID和VELOCITY_PID：ros_controller输出的是关节速度或者位置，同时为libgazebo_ros_control.so插件配置了PID参数，则将输入的关节位置和速度通过PID算法转换成关节力矩，再去驱动gazebo中的关节；注意这里的PID参数不是ros_controller的yaml文件中的参数；
- POSITION和VELOCITY：ros_controller输出的是关节速度或者位置，同时没有为libgazebo_ros_control.so插件配置PID参数，此时关节会以最大扭矩驱动，迅速达到指定的速度或位置，这个最大扭矩是urdf中joint标签中设置的；
- 默认情况下没有为libgazebo_ros_control.so插件配置关节速度和关节位置PID参数，因此第二种情况是进不去的。所以在仿真中我们就会发现，如果为关节指定了速度或者位置接口，仿真效果都比较理想，那是因为无论我们输入什么速度或位置，关节都以最大扭矩驱动（在urdf中往往也会为关节设置一个很大的扭矩），效果当然也就比较好；但是当为关节设置力矩接口时，机器人响应可能就不理想，那是因为ros_controller输出什么力矩，gazebo就以该扭矩驱动关节，所以压力就到ros_controller了。因此在这种情况下，需要花费精力调整ros_controller中的PID参数。
  1
  2
  3
  4
  5
  <gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
  <robotNamespace>/arm</robotNamespace>
  </plugin>
  </gazebo>
加载ros_controller：最后就是通过配置yaml和launch文件来加载ros_controller，注意加载的控制器需要和hardwareInterface适配，即如果hardwareInterface中配置的是位置控制接口，那么ros_controller也要加载位置控制器，具体可以参考官方例程。

通过配置不同控制器，就可以通过ros接口来控制gazebo中的机器人，比如通过话题控制某个关节，通过action控制整个机器人等。

注意在上述ros_control的配置，我们并没有涉及到机器人具体构型，而重点关注每个关节的运动，为每个关节提供了控制接口。因此上述配置方式理论上对任何构型的机器人都适用。

移动机器人仿真

我们上面讲了一般机器人的仿真，最核心的就是libgazebo_ros_control.so插件和关节控制器，其中插件负责ros和gazebo的通信并解析硬件接口，控制器负责实际的关节控制。然而插件和控制器都不是一成不变的，下面我们通过移动机器人的例子看一下通过对插件和控制器的更改，来实现不同的仿真模式。

移动机器人的仿真有三种不同的方式：

标准模式：使用标准的libgazebo_ros_control.so插件管理通信和解析硬件接口，然后为每个轮子加载一个速度控制器，此时用户需要手动管理移动机器人正逆运动学，里程计等信息（因为这个时候只能控制轮子）；
定制化控制器：通信和硬件接口解析，仍然使用标准的libgazebo_ros_control.so，但是在每个轮子的关节速度控制的基础上做了二次集成，开发了新的控制器。比如ros对移动机器人提供了ackermann_steering_controller和diff_drive_controller（简单测试了一下，不好用），这样就可以直接管理车子的速度和里程计信息了，而不是只控制轮子运动；
定制化插件：在这种模式下，通信和硬件接口解析以及机器人控制器都在一个插件中完成了，这种方式集成度最高。此时，我们就不需要在urdf中加载libgazebo_ros_control.so，也不需要启动控制器管理器了，只需要在urdf中加载新的插件并进行相应的配置即可。目前ros对移动机器人提供了三个插件，介绍如下：

移动机器人gazebo插件

差分移动机器人

<gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
        <rosDebugLevel>Debug</rosDebugLevel>
        <publishWheelTF>true</publishWheelTF>
        <robotNamespace>/</robotNamespace>
        <publishTf>1</publishTf>
        <publishWheelJointState>true</publishWheelJointState>
        <alwaysOn>true</alwaysOn>
        <updateRate>100.0</updateRate>
        <legacyMode>true</legacyMode>
        <leftJoint>base_to_wheel_left_joint</leftJoint>
        <rightJoint>base_to_wheel_right_joint</rightJoint>
        <wheelSeparation>${base_link_radius*2}</wheelSeparation>
        <wheelDiameter>${2*wheel_radius}</wheelDiameter>
        <broadcastTF>1</broadcastTF>
        <wheelTorque>30</wheelTorque>
        <wheelAcceleration>1.8</wheelAcceleration>
        <commandTopic>cmd_vel</commandTopic>
        <odometryFrame>odom</odometryFrame> 
        <odometryTopic>odom</odometryTopic> 
        <robotBaseFrame>base_footprint</robotBaseFrame>
    </plugin>
</gazebo>

该插件的源码可以看这里，可以看到插件的主要逻辑是：

配置移动机器人基本参数，如轮子直径，轮距，坐标发布等；
接收/cmd_vel指令，即控制机器人的速度；
根据/cmd_vel的信息，更新/odom，然后将里程计信息发布出去；
根据小车的集合信息，计算出每个轮子的状态，更新/joint_states;

四驱移动机器人

<gazebo>
    <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
        <updateRate>100.0</updateRate>
        <robotNamespace>/</robotNamespace>
        <leftFrontJoint>front_left_wheel_joint</leftFrontJoint>
        <rightFrontJoint>front_right_wheel_joint</rightFrontJoint>
        <leftRearJoint>back_left_wheel_joint</leftRearJoint>
        <rightRearJoint>back_right_wheel_joint</rightRearJoint>
        <wheelSeparation>0.4</wheelSeparation>
        <wheelDiameter>0.215</wheelDiameter>
        <robotBaseFrame>base_link</robotBaseFrame>
        <torque>20</torque>
        <topicName>cmd_vel</topicName>
        <broadcastTF>false</broadcastTF>
    </plugin>
</gazebo>

全向移动机器人： model plugin that allows arbitrary objects (for instance cubes, spheres and cylinders) to be moved along a horizontal plane using a geometry_msgs/Twist message. The plugin works by imparting a linear velocity (XY) and an angular velocity (Z) to the object every cycle.
Here is a full URDF example that demonstrates how to control a floating box inside gazebo using this plugin, using different visual and collision elements. Note: The object needs to have sufficient inertia to prevent undesirable motions - which can occur as a reaction to the supplied velocity. You can try increasing inertia until the object moves as desired. It is also good to have the center of mass close to the ground. （因为里面没有活动关节，所以不需要对关节设置传动）

<robot name="test_model">

<!-- root link, on the ground just below the model origin -->
<link name="base_footprint">
<visual>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <geometry>
        <box size="0.001 0.001 0.001" />
    </geometry>
    </visual>
</link>

<joint name="base_link_joint" type="fixed">
    <origin xyz="0.0 0 1.25" rpy="0 0 0" />
    <parent link="base_footprint"/>
    <child link="base_link" />
</joint>

<!-- the model -->
<link name="base_link">
    <inertial>
    <mass value="50" />
    <origin xyz="0 0 -1.25" />
    <inertia ixx="50.0" ixy="0.0" ixz="0.0"
        iyy="50.0" iyz="0.0"
        izz="50.0" />
    </inertial>
    <visual>
    <geometry>
        <box size="0.5 0.5 1.0" /> <!-- does not need to match collision -->
    </geometry>
    </visual>
    <collision>
    <origin xyz="0 0 -1.0" />
    <geometry>
        <cylinder length="0.5" radius="0.25" />
    </geometry>
    </collision>
</link>

<gazebo>
    <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>
    <odometryRate>20.0</odometryRate>
    <robotBaseFrame>base_footprint</robotBaseFrame>
    </plugin>
</gazebo>

</robot>

总结

传感器仿真比较简单，先为传感器建立urdf，然后再增加传感器插件和对应的参数即可。

一般机器人的仿真是建立在对ros_control理解的基础上的，通过对ros_control的配置，可以实现一般机器人的仿真。

移动机器人作为一种特殊的机器人，控制模式较为单一，构型也不复杂，因此ros提供了几种常见的插件来对其控制，相比ros_control的方式，使用定制化的插件配置简单，但劣势是一个插件只对一个固定构型的移动机器人有用。

张恒的博客