一、添加超声波仿真模型
1.在文件夹下创建 ultrasonic.xacro 文件,在文件内添加以下代码:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ultrasonic">
<xacro:macro name="ultrasonic" params="prefix:=ultrasonic">
<!-- Create ultrasonic reference frame -->
<link name="ultrasonic_sensor">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<collision >
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<!--the senser laser in gazebo,be careful that the names of reference and link should be same-->
<gazebo reference="ultrasonic_sensor">
<sensor type="ray" name="ultrasonic_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<ray>
<!--define the line parameter of laser senser -->
<scan>
<horizontal>
<samples>8</samples>
<resolution>1</resolution>
<min_angle>-0.14</min_angle>
<max_angle>0.14</max_angle>
</horizontal>
<vertical>
<samples>8</samples>
<resolution>1</resolution>
<min_angle>-0.14</min_angle>
<max_angle>0.14</max_angle>
</vertical>
</scan>
<range>
<min>0.05</min>
<max>3</max>
<resolution>0.1</resolution>
</range>
</ray>
<!--define the topic of the senser laser between the gazebo and ros in simulation information exchange.-->
<plugin name="ultrasonic_sensor_controller" filename="libgazebo_ros_range.so">
<gaussianNoise>0.01</gaussianNoise>
<alwaysOn>true</alwaysOn>
<fov>0.1</fov>
<topicName>ultrasonic_sensor</topicName>
<frameName>ultrasonic_sensor</frameName>
<radiation>ultrasound</radiation>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
2、在机器人配置文件(urdf)中添加以下代码:
<!--ultrasonic_sensor-->
<joint name="ultrasonic_sensor_joint" type="fixed">
<origin xyz="0.4 0 0.25" rpy=&