1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768 |
- <?xml version="1.0"?>
- <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:macro name="sonar_sensor" params="name parent *origin ros_topic update_rate min_range max_range field_of_view ray_count">
- <joint name="${name}_joint" type="fixed">
- <xacro:insert_block name="origin" />
- <parent link="${parent}"/>
- <child link="${name}_link"/>
- </joint>
- <link name="${name}_link">
- <inertial>
- <mass value="0.001" />
- <origin xyz="0 0 0" rpy="0 0 0" />
- <inertia ixx="0.000000017" ixy="0" ixz="0" iyy="0.000000017" iyz="0" izz="0.000000017" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <!--<box size="0.01 0.01 0.01" /> -->
- <mesh filename="package://hector_sensors_description/meshes/sonar_sensor/max_sonar_ez4.dae"/>
- </geometry>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <box size="0.01 0.01 0.01" />
- </geometry>
- </collision>
- </link>
- <gazebo reference="${name}_link">
- <sensor type="ray" name="${name}">
- <always_on>true</always_on>
- <update_rate>${update_rate}</update_rate>
- <pose>0 0 0 0 0 0</pose>
- <visualize>false</visualize>
- <ray>
- <scan>
- <horizontal>
- <samples>${ray_count}</samples>
- <resolution>1</resolution>
- <min_angle>-${field_of_view/2}</min_angle>
- <max_angle> ${field_of_view/2}</max_angle>
- </horizontal>
- <vertical>
- <samples>${ray_count}</samples>
- <resolution>1</resolution>
- <min_angle>-${field_of_view/2}</min_angle>
- <max_angle> ${field_of_view/2}</max_angle>
- </vertical>
- </scan>
- <range>
- <min>${min_range}</min>
- <max>${max_range}</max>
- <resolution>0.01</resolution>
- </range>
- </ray>
- <plugin name="gazebo_ros_${name}_controller" filename="libhector_gazebo_ros_sonar.so">
- <gaussianNoise>0.005</gaussianNoise>
- <topicName>${ros_topic}</topicName>
- <frameId>${name}_link</frameId>
- </plugin>
- </sensor>
- </gazebo>
- </xacro:macro>
- </robot>
|