在Gazebo中获取驱动轮的角度和角速度

在gazebo存在完美里程计（无论对机器人进行绑架还是机器人原地打滑，/odom都会精确给出机器人所在位置），我们拥有了激光雷达数据/scan和里程计数据，并且保证传感器laser和base_footprint存在tf关系就能进行2d建图。

    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <commandTopic>cmd_vel</commandTopic>
            <odometryTopic>odom</odometryTopic>
            <odometryFrame>odom</odometryFrame>
            <odometrySource>world</odometrySource>
            <publishOdomTF>true</publishOdomTF>
            <robotBaseFrame>base_footprint</robotBaseFrame>
            <publishWheelTF>true</publishWheelTF>
            <publishTf>true</publishTf>
            <publishWheelJointState>false</publishWheelJointState>
            <legacyMode>false</legacyMode>
            <updateRate>30</updateRate>
            <leftJoint>wheel_left_joint</leftJoint>
            <rightJoint>wheel_right_joint</rightJoint>
            <wheelSeparation>${wheelSeparation}</wheelSeparation>
            <wheelDiameter>${wheelDiameter}</wheelDiameter>
            <wheelAcceleration>100</wheelAcceleration>
            <wheelTorque>100</wheelTorque>
            <rosDebugLevel>na</rosDebugLevel>
        </plugin>
    </gazebo>

 

然而，现实中没有gazebo中的完美里程计，我们需要获取机器人的线速度（通常通过电机的编码器推算获得）、角速度（通常通过获取IMU数据推算获得）、激光雷达数据、深度相机的数据、GPS以及其他传感器数据，并进行融合获得里程计。

上面对urdf的修改只能控制机器人的驱动轮，不能获取机器人驱动轮的角度、角速度、加速度（可以读取/joint_states,但读出来的全是无效数据如下图所示）

因此需要用到ros_controller

例如这份仿真用的机器人用到了两轮差分运动的运动学模型，为了在gazebo中能够读取驱动轮的角速度和加速度等信息，还需要使用controller_manager中的joint_state_controller，以及velocity_controllers

config_yaml:

joint_state_controller:

    type: joint_state_controller/JointStateController

    publish_rate: 50  

left_rear_wheel_velocity_controller:

    type: velocity_controllers/JointVelocityController

    joint: wheel_left_joint

    pid: {p: 100, i: 0.01, d: 10.0, i_clamp: 0.0}

right_rear_wheel_velocity_controller:

    type: velocity_controllers/JointVelocityController

    joint: wheel_right_joint

    pid: {p: 100, i: 0.01, d: 10.0, i_clamp: 0.0}

 1在对应的关节中加入transsmion：

        <transmission name="simple_trans_left">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="wheel_left_joint">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="foo_motor_left">
                <mechanicalReduction>50</mechanicalReduction>
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </actuator>
        </transmission>
        <transmission name="simple_trans_right">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="wheel_right_joint">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="foo_motor_right">
                <mechanicalReduction>50</mechanicalReduction>
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </actuator>
        </transmission>

2在urdf的最后加上gazebo_ros_control使其在仿真中生效：

<gazebo>

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">

</plugin>

</gazebo>

3在launch中加载controller

  <rosparam file="$(find clean_ct06a)/config/complex.yaml" command="load"/>
  <node name="controller_spawner" pkg="controller_manager" 
  type="spawner" respawn="false" output="screen" 
  args="joint_state_controller left_rear_wheel_velocity_controller right_rear_wheel_velocity_controller"/>

   

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
  </node>

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

最终能够成功获取驱动轮的角速度：