<?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>
