170 lines
7.7 KiB
XML
170 lines
7.7 KiB
XML
<?xml version="1.0"?>
|
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
|
|
<xacro:macro name="ouster_lidar" params="name parent xyz rpy model frequency topic">
|
|
|
|
<!-- different lidar models -->
|
|
<xacro:if value="${model == 'vlp16'}">
|
|
<xacro:property name="min_range" value="0.01"/>
|
|
<xacro:property name="max_range" value="100.0"/>
|
|
<xacro:property name="precision" value="0.03"/>
|
|
<xacro:property name="max_frequency" value="20.0"/>
|
|
<xacro:property name="vertical_fov" value="30.0"/>
|
|
<xacro:property name="horizontal_res" value="900"/>
|
|
<xacro:property name="nr_of_channels" value="16"/>
|
|
</xacro:if>
|
|
<xacro:if value="${model == 'vlp16hi'}">
|
|
<xacro:property name="min_range" value="0.5"/>
|
|
<xacro:property name="max_range" value="100.0"/>
|
|
<xacro:property name="precision" value="0.03"/>
|
|
<xacro:property name="max_frequency" value="20.0"/>
|
|
<xacro:property name="vertical_fov" value="40.0"/>
|
|
<xacro:property name="horizontal_res" value="900"/>
|
|
<xacro:property name="nr_of_channels" value="16"/>
|
|
</xacro:if>
|
|
<xacro:if value="${model == 'vlp32'}">
|
|
<xacro:property name="min_range" value="0.5"/>
|
|
<xacro:property name="max_range" value="200.0"/>
|
|
<xacro:property name="precision" value="0.03"/>
|
|
<xacro:property name="max_frequency" value="20.0"/>
|
|
<xacro:property name="vertical_fov" value="40.0"/>
|
|
<xacro:property name="horizontal_res" value="900"/>
|
|
<xacro:property name="nr_of_channels" value="32"/>
|
|
</xacro:if>
|
|
<xacro:if value="${model == 'vls128'}">
|
|
<xacro:property name="min_range" value="0.5"/>
|
|
<xacro:property name="max_range" value="300.0"/>
|
|
<xacro:property name="precision" value="0.03"/>
|
|
<xacro:property name="max_frequency" value="20.0"/>
|
|
<xacro:property name="vertical_fov" value="40.0"/>
|
|
<xacro:property name="horizontal_res" value="900"/>
|
|
<xacro:property name="nr_of_channels" value="128"/>
|
|
</xacro:if>
|
|
<xacro:if value="${model == 'OS0'}">
|
|
<xacro:property name="min_range" value="0.3"/>
|
|
<xacro:property name="max_range" value="100.0"/>
|
|
<xacro:property name="precision" value="0.03"/>
|
|
<xacro:property name="max_frequency" value="20.0"/>
|
|
<xacro:property name="vertical_fov" value="90.0"/>
|
|
<xacro:property name="horizontal_res" value="1024"/>
|
|
<xacro:property name="nr_of_channels" value="128"/>
|
|
</xacro:if>
|
|
<xacro:if value="${model == 'OS1'}">
|
|
<xacro:property name="min_range" value="0.3"/>
|
|
<xacro:property name="max_range" value="200.0"/>
|
|
<xacro:property name="precision" value="0.03"/>
|
|
<xacro:property name="max_frequency" value="20.0"/>
|
|
<xacro:property name="vertical_fov" value="45.0"/>
|
|
<xacro:property name="horizontal_res" value="1024"/>
|
|
<xacro:property name="nr_of_channels" value="128"/>
|
|
</xacro:if>
|
|
<xacro:if value="${model == 'OS2'}">
|
|
<xacro:property name="min_range" value="0.3"/>
|
|
<xacro:property name="max_range" value="400.0"/>
|
|
<xacro:property name="precision" value="0.02"/>
|
|
<xacro:property name="max_frequency" value="20.0"/>
|
|
<xacro:property name="vertical_fov" value="22.5"/>
|
|
<xacro:property name="horizontal_res" value="1024"/>
|
|
<xacro:property name="nr_of_channels" value="128"/>
|
|
</xacro:if>
|
|
|
|
|
|
<link name="${name}_link">
|
|
<visual name="base">
|
|
<pose>0 0 -0.035 0 0 0</pose>
|
|
<geometry>
|
|
<cylinder radius="0.06" length="0.0015"/>
|
|
</geometry>
|
|
</visual>
|
|
<collision name="base_collision">
|
|
<pose>0 0 -0.035 0 0 0</pose>
|
|
<geometry>
|
|
<cylinder radius="0.06" length="0.01"/>
|
|
</geometry>
|
|
</collision>
|
|
<visual name="suport_1">
|
|
<pose>-0.035 0.035 -0.08 0 0 0</pose>
|
|
<geometry>
|
|
<cylinder radius="0.005" length="0.09"/>
|
|
</geometry>
|
|
</visual>
|
|
<visual name="suport_2">
|
|
<pose>-0.035 -0.035 -0.08 0 0 0</pose>
|
|
<geometry>
|
|
<cylinder radius="0.005" length="0.09"/>
|
|
</geometry>
|
|
</visual>
|
|
<visual name="suport_3">
|
|
<pose>0.0495 0 -0.08 0 0 0</pose>
|
|
<geometry>
|
|
<cylinder radius="0.005" length="0.09"/>
|
|
</geometry>
|
|
</visual>
|
|
<!-- Bottom cylinder of Ouster/Velodyne 3D Lidar -->
|
|
<visual name="bot_cylinder">
|
|
<pose>0 0 -0.035 0 0 0</pose>
|
|
<geometry>
|
|
<mesh filename="package://sherpa/urdf/meshes/VLP16_base_1.dae" scale="1 1 1"/>
|
|
</geometry>
|
|
</visual>
|
|
<!-- Top cylinder of Ouster/Velodyne 3D Lidar -->
|
|
<visual name="top_cylinder">
|
|
<pose>0 0 -0.035 0 0 0</pose>
|
|
<geometry>
|
|
<mesh filename="package://sherpa/urdf/meshes/VLP16_base_2.dae" scale="1 1 1"/>
|
|
</geometry>
|
|
</visual>
|
|
<!-- Main cylinder of Ouster/Velodyne 3D Lidar -->
|
|
<visual name="main_cylinder">
|
|
<pose>0 0 -0.035 0 0 0</pose>
|
|
<geometry>
|
|
<mesh filename="package://sherpa/urdf/meshes/VLP16_scan.dae" scale="1 1 1"/>
|
|
</geometry>
|
|
</visual>
|
|
<inertial>
|
|
<mass value="0.83"/>
|
|
<inertia
|
|
ixx="0.001828"
|
|
ixy="0.0"
|
|
ixz="0.0"
|
|
iyy="0.001828"
|
|
iyz="0.0"
|
|
izz="0.003528"
|
|
/>
|
|
</inertial>
|
|
</link>
|
|
<joint name="${name}_joint" type="fixed">
|
|
<parent link="${parent}_link"/>
|
|
<child link="${name}_link"/>
|
|
<origin xyz="${xyz}" rpy="${rpy}"/>
|
|
</joint>
|
|
<gazebo reference="${name}_link">
|
|
<sensor name="${name}" type="gpu_lidar">
|
|
<update_rate>${frequency}</update_rate>
|
|
<topic>/velodyne_points</topic>
|
|
<gz_frame_id>${name}_link</gz_frame_id>
|
|
<lidar>
|
|
<scan>
|
|
<horizontal>
|
|
<samples>${horizontal_res * (max_frequency / frequency)}</samples>
|
|
<resolution>1</resolution>
|
|
<min_angle>-3.141592654</min_angle>
|
|
<max_angle>3.141592654</max_angle>
|
|
</horizontal>
|
|
<vertical>
|
|
<samples>${nr_of_channels}</samples>
|
|
<resolution>1</resolution>
|
|
<min_angle>${vertical_fov * -0.0087266463}</min_angle>
|
|
<max_angle>${vertical_fov * 0.0087266463}</max_angle>
|
|
</vertical>
|
|
</scan>
|
|
<range>
|
|
<min>${min_range}</min>
|
|
<max>${max_range}</max>
|
|
<resolution>${precision}</resolution>
|
|
</range>
|
|
</lidar>
|
|
</sensor>
|
|
</gazebo>
|
|
</xacro:macro>
|
|
</robot> |