123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113 |
- <?xml version="1.0"?>
- <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:include filename="$(find hector_xacro_tools)/urdf/inertia_tensors.urdf.xacro"/>
- <xacro:property name="M_PI" value="3.1415926535897931"/>
- <xacro:macro name="vlp16_model" params="name parent *origin">
- <joint name="${name}_joint" type="fixed">
- <xacro:insert_block name="origin"/>
- <parent link="${parent}"/>
- <child link="${name}_frame"/>
- </joint>
- <link name="${name}_frame">
- <xacro:inertial_sphere mass="0.83" diameter="0.1033"/>
- <visual>
- <origin xyz="0 0 -0.0378" rpy="${M_PI/2} 0 ${M_PI}"/>
- <geometry>
- <mesh filename="package://hector_sensors_description/meshes/vlp16/VLP16_base_1.dae" scale="0.001 0.001 0.001"/>
- </geometry>
- </visual>
- <visual>
- <origin xyz="0 0 -0.0378" rpy="${M_PI/2} 0 ${M_PI}"/>
- <geometry>
- <mesh filename="package://hector_sensors_description/meshes/vlp16/VLP16_base_2.dae" scale="0.001 0.001 0.001"/>
- </geometry>
- </visual>
- <visual>
- <origin xyz="0 0 -0.0378" rpy="${M_PI/2} 0 ${M_PI}"/>
- <geometry>
- <mesh filename="package://hector_sensors_description/meshes/vlp16/VLP16_scan.dae" scale="0.001 0.001 0.001"/>
- </geometry>
- </visual>
- <collision>
- <origin rpy="0 0 0" xyz="0 0 -0.00225"/>
- <geometry>
- <cylinder radius="0.0516" length="0.0717"/>
- </geometry>
- </collision>
- </link>
- </xacro:macro>
- <xacro:macro name="vlp16" params="name parent *origin ros_topic">
- <xacro:vlp16_advanced_parameters name="${name}" parent="${parent}" ros_topic="${ros_topic}" update_rate="10" horizontal_samples="512" vertical_samples="8" min_range="0.45" max_range="100">
- <xacro:insert_block name="origin"/>
- </xacro:vlp16_advanced_parameters>
- </xacro:macro>
- <xacro:macro name="vlp16_advanced_parameters" params="name parent *origin ros_topic update_rate horizontal_samples vertical_samples min_range max_range use_gpu:='false'">
- <xacro:vlp16_model name="${name}" parent="${parent}">
- <xacro:insert_block name="origin"/>
- </xacro:vlp16_model>
- <gazebo reference="${name}_frame">
- <xacro:if value="${use_gpu}">
- <xacro:property name="sensor_type" value="gpu_ray"/>
- <xacro:property name="gazebo_plugin_name" value="libgazebo_ros_velodyne_gpu_laser.so"/>
- </xacro:if >
- <xacro:unless value="${use_gpu}">
- <xacro:property name="sensor_type" value="ray"/>
- <xacro:property name="gazebo_plugin_name" value="libgazebo_ros_velodyne_laser.so"/>
- </xacro:unless>
- <sensor type="${sensor_type}" name="${name}-VLP16">
- <pose>0 0 0 0 0 0</pose>
- <visualize>false</visualize>
- <update_rate>${update_rate}</update_rate>
- <ray>
- <scan>
- <horizontal>
- <samples>${horizontal_samples}</samples>
- <resolution>1</resolution>
- <min_angle>-${M_PI}</min_angle>
- <max_angle>${M_PI}</max_angle>
- </horizontal>
- <vertical>
- <samples>${vertical_samples}</samples>
- <resolution>1</resolution>
- <min_angle>-${15.0*M_PI/180.0}</min_angle>
- <max_angle>${15.0*M_PI/180.0}</max_angle>
- </vertical>
- </scan>
- <range>
- <min>${min_range}</min>
- <max>${max_range}</max>
- <resolution>0.001</resolution>
- </range>
- <noise>
- <type>gaussian</type>
- <mean>0.0</mean>
- <stddev>0.0</stddev>
- </noise>
- </ray>
- <plugin name="gazebo_ros_laser_controller" filename="${gazebo_plugin_name}">
- <topicName>${ros_topic}</topicName>
- <frameName>${name}_frame</frameName>
- <min_range>${min_range}</min_range>
- <max_range>${max_range}</max_range>
- <gaussianNoise>0.008</gaussianNoise>
- </plugin>
- </sensor>
- </gazebo>
- </xacro:macro>
- </robot>
|