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