第四章、用xacro优化URDF并配置gazebo仿真插件

1►前言

上节用简易模型写了一个小车的URDF代码，这一节将用xacro对其进行优化，这里我并不打算用宏对参数进行封装，因为我个人觉得这样看起来会比较直观，方便读者阅读。

2►配置主xacro文件

新建racecar.xacro文件，将上一节racebot.urdf中的代码复制过来并进行修改，整体代码如下：

<?xml version="1.0" encoding="utf-8"?>
<robot name="racebot" xmlns:xacro="http://www.ros.org/wiki/xacro"><xacro:include filename="$(find racebot_description)/urdf/ackermann/macros.xacro" />  <link name="base_footprint"></link><link name="base_link"><visual><geometry><box size="0.28 0.1 0.03"/><!-- <mesh filename="package://tianracer_description/meshes/base_link.STL" /> --></geometry><!-- <origin xyz="0 0 -0.023" rpy="0 0 0" /> --><origin xyz="0 0 0" rpy="0 0 0" /><material name="yellow"><color rgba="0.8 0.3 0.1 0.5" /></material></visual><collision><geometry><box size="0.28 0.1 0.03" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision></link><joint name="base_link2base_footprint" type="fixed"><parent link="base_footprint" /><child link="base_link"/><origin xyz="0 0 0.032" rpy="0 0 0" /></joint><link name="base_inertia"><inertial><origin xyz="0 0 0" /><mass value="4" /><inertia ixx="0.0264" ixy="0" ixz="0" iyy="0.0294" iyz="0" izz="0.00364" /></inertial></link>    <joint name="chassis_inertia_joint" type="fixed"><origin xyz="0 0 0" rpy="0 0 0" /><parent link="base_link" /><child link="base_inertia" /></joint><link name="left_steering_hinge"><visual><geometry><cylinder radius="0.01" length="0.005" /><!-- <sphere radius="0.015" /> --></geometry><origin xyz="0 0 0" rpy="1.57 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><cylinder radius="0.01" length="0.005" /><!-- <sphere radius="0.015" /> --></geometry><origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="0.5" /><inertia ixx="1.35E-05" ixy="0" ixz="0" iyy="1.35E-05" iyz="0" izz="2.5E-05" /></inertial></link><joint name="left_steering_hinge_joint" type="revolute"><parent link="base_link" /><child link="left_steering_hinge" /><origin xyz="0.13 0.065 0" /><axis xyz="0 0 1" /><limit lower="-0.6" upper="0.6" effort="5.0" velocity="1000.0"/></joint><xacro:steering_hinge_transmission name="left_steering_hinge" /><link name="left_front_wheel"><visual><geometry><cylinder radius="0.033" length="0.02" /></geometry><origin xyz="0 0 0" rpy="1.57 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><cylinder radius="0.032" length="0.02" /></geometry><origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="2.0" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="left_front_wheel_joint" type="continuous"><parent link="left_steering_hinge" /><child link="left_front_wheel" /><origin xyz="0 0.025 0" /><axis xyz="0 1 0" /><limit effort="10" velocity="1000" /></joint><xacro:wheel_transmission name="left_front_wheel" />
<link name="right_steering_hinge"><visual><geometry><cylinder radius="0.01" length="0.005" /></geometry><origin xyz="0 0 0" rpy="1.57 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><cylinder radius="0.01" length="0.005" /></geometry><origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="0.5" /><inertia ixx="1.35E-05" ixy="0" ixz="0" iyy="1.35E-05" iyz="0" izz="2.5E-05" /></inertial></link><joint name="right_steering_hinge_joint" type="revolute"><parent link="base_link" /><child link="right_steering_hinge" /><origin xyz="0.13 -0.065 0" /><axis xyz="0 0 1" /><limit lower="-0.6" upper="0.6" effort="5.0" velocity="1000.0"/></joint><xacro:steering_hinge_transmission name="right_steering_hinge" /><link name="right_front_wheel"><visual><geometry><cylinder radius="0.033" length="0.02" /></geometry><origin xyz="0 0 0" rpy="1.57 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><cylinder radius="0.032" length="0.02" /></geometry><origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="2.0" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="right_front_wheel_joint" type="continuous"><parent link="right_steering_hinge" /><child link="right_front_wheel" /><origin xyz="0 -0.025 0" /><axis xyz="0 1 0" /><limit effort="10" velocity="1000" /></joint><xacro:wheel_transmission name="right_front_wheel" /><link name="left_rear_wheel"><visual><geometry><cylinder radius="0.032" length="0.02" /></geometry><origin xyz="0 0 0" rpy="1.57 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><cylinder radius="0.032" length="0.02" /></geometry><origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="2.0" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="left_rear_wheel_joint" type="continuous"><parent link="base_link" /><child link="left_rear_wheel" /><origin xyz="-0.13 0.09 0" /><axis xyz="0 1 0" /><limit effort="10" velocity="1000" /></joint><xacro:wheel_transmission name="left_rear_wheel" /><link name="right_rear_wheel"><visual><geometry><cylinder radius="0.032" length="0.02" /></geometry><origin xyz="0 0 0" rpy="1.57 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><cylinder radius="0.032" length="0.02" /></geometry><origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="2.0" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="right_rear_wheel_joint" type="continuous"><parent link="base_link" /><child link="right_rear_wheel" /><origin xyz="-0.13 -0.09 0" /><axis xyz="0 1 0" /><limit effort="10" velocity="1000" /></joint><xacro:wheel_transmission name="right_rear_wheel" /><link name="camera"><visual><geometry><box size="0.005 0.03 0.03"/></geometry><origin xyz="0 0 0" rpy="0 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><box size="0.005 0.03 0.03"/></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="0.05" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="camera_joint" type="fixed"><parent link="base_link" /><child link="camera" /><origin xyz="0.14851 0.0022137 0.0975" /><axis xyz="0 0 1" /></joint><link name="real_sense"><visual><geometry><box size="0.01 0.1 0.02"/></geometry><origin xyz="0 0 0" rpy="0 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><box size="0.01 0.1 0.02"/></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="0.05" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="realsense_joint" type="fixed"><parent link="base_link" /><child link="real_sense" /><origin xyz="0.19864 0.0038046 0.052021" /><axis xyz="0 0 1" /></joint><link name="lidar"><visual><geometry><cylinder radius="0.03" length="0.06" /></geometry><origin xyz="0 0 0" rpy="0 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><cylinder radius="0.03" length="0.06" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="0.1" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="lidar_joint" type="fixed"><parent link="base_link" /><child link="lidar" /><origin xyz="0.093603 0 0.12377" /><axis xyz="0 0 1" /></joint><link name="imu"><visual><geometry><box size="0.01 0.01 0.005"/></geometry><origin xyz="0 0 0" rpy="0 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><box size="0.01 0.01 0.005"/></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="0.05" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="imu_joint" type="fixed"><parent link="base_link" /><child link="imu" /><origin xyz="0 0 0.02" /><axis xyz="0 0 1" /></joint><xacro:include filename="$(find racebot_description)/urdf/ackermann/racecar.gazebo" />
</robot>

代码起始和末尾添加了两个文件，通过xacro封装引用进来。这两个xacro文件是接下去要配置的传动文件macros.xacro以及gazebo插件racecar.gazebo。

3►添加传动transmission

创建macro.xacro文件，该代码给前后轮以及前轮摆转配置了传动transmission元素，代码如下：

<?xml version="1.0"?>
<robot name="racebot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- transmission macros -->
<xacro:macro name="wheel_transmission" params="name"><transmission name="${name}_transmission" type="SimpleTransmission"><type>transmission_interface/SimpleTransmission</type><joint name="${name}_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="${name}_motor"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission>
</xacro:macro>
<xacro:macro name="steering_hinge_transmission" params="name"><transmission name="${name}_transmission" type="SimpleTransmission"><type>transmission_interface/SimpleTransmission</type><joint name="${name}_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="${name}_motor"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission>
</xacro:macro>
</robot>

值得注意的是，引用该文件时，要将路径代码放置到racecar.xacro代码的最上端位置，如上代码所示，否则将无法引用到，并且在需要添加传动的link后面添加宏定义，以左前轮以及左前轮摆转为例：

<xacro:wheel_transmission name="left_front_wheel" />...<xacro:steering_hinge_transmission name="right_steering_hinge" />

4►配置gazebo插件

要让小车在gazebo中仿真，并且让小车能够进行建图导航，需要给小车的摄像头，激光雷达等link添加传感器插件，下面将进行配置，新建racecar.gazebo文件，开头代码：

<?xml version="1.0"?>
<robot name="racebot" xmlns:xacro="http://www.ros.org/wiki/xacro">
。
。
。
</robot>

配置各个link的颜色：

由于车轮实际上会接触地面，因此会与地面发生物理相互作用，将各个link添加部件材料的附加信息，并且定义各个link的颜色信息。参考gazebo官网：http://gazebosim.org/tutorials/?tut=ros_urdf

<!-- Gazebo references -->
<gazebo reference="base_link"><mu1 value="0.0"/><mu2 value="0.0"/><kp  value="10000000.0" /><kd  value="1.0" /><material>Gazebo/Red</material>
</gazebo>
<gazebo reference="left_rear_wheel"><mu1 value="2.0"/><mu2 value="2.0"/><kp  value="10000000.0" /><kd  value="1.0" /><fdir1 value="1 0 0"/><material>Gazebo/Black</material>
</gazebo>
<gazebo reference="right_rear_wheel"><mu1 value="2.0"/><mu2 value="2.0"/><kp  value="10000000.0" /><kd  value="1.0" /><fdir1 value="1 0 0"/><material>Gazebo/Black</material>
</gazebo>
<gazebo reference="left_front_wheel"><mu1 value="2.0"/><mu2 value="2.0"/><kp  value="10000000.0" /><kd  value="1.0" /><fdir1 value="0 0 1"/><material>Gazebo/Black</material>
</gazebo>
<gazebo reference="right_front_wheel"><mu1 value="2.0"/><mu2 value="2.0"/><kp  value="10000000.0" /><kd  value="1.0" /><fdir1 value="0 0 1"/><material>Gazebo/Black</material>
</gazebo>
<gazebo reference="lidar"><material>Gazebo/Black</material>
</gazebo>
<gazebo reference="camera"><material>Gazebo/Black</material>
</gazebo>
<gazebo reference="real_sense"><material>Gazebo/Grey</material>
</gazebo>

配置ros_control：

由于gazebo并没阿克曼车型的插件，要链接gazebo与ros，我们先添加ros_control插件，它读取所有transmission标记，以及joint_state_publisher插件。

<!-- Gazebo Plugins -->
<gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"><robotNamespace>/racebot</robotNamespace><robotParam>robot_description</robotParam><robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType><legacyModeNS>true</legacyModeNS></plugin>
</gazebo>

注意命名空间要设置为/racebot。

配置激光雷达插件：

<!-- hokuyo --><gazebo reference="lidar"><material>Gazebo/Grey</material><sensor type="ray" name="hokuyo_sensor"><pose>0 0 0.0124 0 0 0</pose><visualize>false</visualize><update_rate>40</update_rate><ray><scan><horizontal><samples>1081</samples><resolution>1</resolution><min_angle>-2.3561944902</min_angle><max_angle>2.3561944902</max_angle></horizontal></scan><range><min>0.1</min><max>10.0</max><resolution>0.01</resolution></range><noise><mean>0.0</mean><stddev>0.01</stddev></noise></ray><plugin name="gazebo_ros_hokuyo_controller" filename="libgazebo_ros_laser.so"><topicName>/scan</topicName><frameName>lidar</frameName></plugin></sensor></gazebo>

配置深度相机插件：

由于配置realsense插件过于复杂，因此这里用kinect插件来代替realsense插件。

<gazebo reference="real_sense"><sensor type="depth" name="real_sense"><always_on>true</always_on><update_rate>20.0</update_rate><camera><horizontal_fov>${60.0*3.14/180.0}</horizontal_fov><image><format>R8G8B8</format><width>640</width><height>480</height></image><clip><near>0.05</near><far>8.0</far></clip></camera><plugin name="kinect_real_sense_controller" filename="libgazebo_ros_openni_kinect.so"><cameraName>real_sense</cameraName><alwaysOn>true</alwaysOn><updateRate>10</updateRate><imageTopicName>rgb/image_raw</imageTopicName><depthImageTopicName>depth/image_raw</depthImageTopicName><pointCloudTopicName>depth/points</pointCloudTopicName><cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName><depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName><frameName>real_sense</frameName><baseline>0.1</baseline><distortion_k1>0.0</distortion_k1><distortion_k2>0.0</distortion_k2><distortion_k3>0.0</distortion_k3><distortion_t1>0.0</distortion_t1><distortion_t2>0.0</distortion_t2><pointCloudCutoff>0.4</pointCloudCutoff></plugin></sensor></gazebo>

配置单目相机插件：

<!-- camera -->
<gazebo reference="camera"><material>Gazebo/Grey</material><sensor type="camera" name="camera"><update_rate>30.0</update_rate><camera name="camera"><horizontal_fov>1.3962634</horizontal_fov><image><width>800</width><height>800</height><format>R8G8B8</format></image><clip><near>0.02</near><far>300</far></clip><noise><type>gaussian</type><mean>0.0</mean><stddev>0.007</stddev></noise></camera><plugin name="camera_controller" filename="libgazebo_ros_camera.so"><alwaysOn>true</alwaysOn><updateRate>0.0</updateRate><cameraName>rrbot/camera1</cameraName><imageTopicName>image_raw</imageTopicName><cameraInfoTopicName>camera_info</cameraInfoTopicName><frameName>camera</frameName><hackBaseline>0.07</hackBaseline><distortionK1>0.0</distortionK1><distortionK2>0.0</distortionK2><distortionK3>0.0</distortionK3><distortionT1>0.0</distortionT1><distortionT2>0.0</distortionT2></plugin></sensor>
</gazebo>

配置imu插件：

<gazebo reference="imu"><material>Gazebo/Orange</material><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</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>

5►配置gazebo插件在gazebo中显示并演示传感器效果

创建racebot_gazebo功能包，并创建如下文件夹：

在launch文件夹中创建racebot.launch文件：

<?xml version="1.0"?>
<launch><!-- 设置launch文件的参数 --><arg name="paused" default="false"/><arg name="use_sim_time" default="true"/><arg name="gui" default="true"/><arg name="headless" default="false"/><arg name="debug" default="false"/><!--模型车的起点放置位置--><arg name="x_pos" default="0"/><arg name="y_pos" default="0"/><arg name="z_pos" default="0"/><arg name="R_pos" default="0"/><arg name="P_pos" default="0"/><arg name="Y_pos" default="0"/><!--运行gazebo仿真环境--><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="debug" value="$(arg debug)" /><arg name="gui" value="$(arg gui)" /><arg name="paused" value="$(arg paused)"/><arg name="use_sim_time" value="$(arg use_sim_time)"/><arg name="headless" value="$(arg headless)"/><arg name="world_name" value="$(find racebot_gazebo)/worlds/room_mini.world"/><!-- .world文件的地址--></include><!-- 加载机器人模型描述参数 --><param name="robot_description" command="$(find xacro)/xacro --inorder '$(find racebot_description)/urdf/ackermann/racecar.xacro'"/><!-- 在gazebo中加载机器人模型--><node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"args="-urdf -model racebot -param robot_description -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -R $(arg R_pos) -P $(arg P_pos) -Y $(arg Y_pos)"/> <!-- <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"args="-urdf -model tianracer -param robot_description -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos)"/>  --><!-- ros_control racecar launch file --><!--Launch the simulation joystick control-->
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /real_sense /real_sense_depth" /> -->
</launch>

在worlds文件夹中，将之前用于mini小车仿真的room_mini.world文件放入，使得racebot载入有room_mini地图的gazebo仿真环境中。

roslaunch racebot_gazebo racebot.launch

可以看到小车已经加载到gazebo环境中，然而此时无法控制小车运动，因为还未配置controllers，但是我们可以读取到传感器参数。

读取imu参数：

rostopic echo /imu

读取雷达数据：

打开rviz

rviz

将fixed_frame改为base_footprint, 点击add-->by topic-->laserScan,此时在rviz中即可显示雷达点云

单目摄像头：

在rviz中add-->by topic-->/rrbot/camera1/image_raw/camera即可添加相机插件，并显示图像：

深度摄像头：

深度摄像头查看深度图像与单目摄像头同理add-->by topic-->realsense/depth/image_raw/camera

只是在添加pointcloud2时会出现问题，add-->by topic-->realsense/depth/points/pointcloud2,会发现彩色点云出现在小车正上方,原因是在kinect中图像数据与点云数据使用了两套坐标系统，且两套坐标系统位姿并不一致。

解决方法有两种：

1.可以在launch文件中加入坐标变换，即可解决这一现象。

<node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /support /support_depth" />

详细参考奥特学园《kinect信息仿真以及显示》https://sourl.cn/ziwDEU

2.第二种方法是在urdf中加入新的link，即使用点云数据的link，并通过joint做坐标变换来解决。

<link name="realsense_depth"/><joint name="realsense_depth_joint" type="fixed"><origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/><parent link="real_sense"/><child link="realsense_depth"/></joint>

修改插件

<gazebo reference="real_sense"><sensor type="depth" name="real_sense"><always_on>true</always_on><update_rate>20.0</update_rate><camera><horizontal_fov>${60.0*3.14/180.0}</horizontal_fov><image><format>R8G8B8</format><width>640</width><height>480</height></image><clip><near>0.05</near><far>8.0</far></clip></camera><plugin name="kinect_real_sense_controller" filename="libgazebo_ros_openni_kinect.so"><cameraName>real_sense</cameraName><alwaysOn>true</alwaysOn><updateRate>10</updateRate><imageTopicName>rgb/image_raw</imageTopicName><depthImageTopicName>depth/image_raw</depthImageTopicName><pointCloudTopicName>depth/points</pointCloudTopicName><cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName><depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName><frameName>realsense_depth</frameName><baseline>0.1</baseline><distortion_k1>0.0</distortion_k1><distortion_k2>0.0</distortion_k2><distortion_k3>0.0</distortion_k3><distortion_t1>0.0</distortion_t1><distortion_t2>0.0</distortion_t2><pointCloudCutoff>0.4</pointCloudCutoff></plugin></sensor></gazebo>