<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="VLP-16">
  <xacro:property name="M_PI" value="3.1415926535897931" />
  <xacro:macro name="vlp16" params="*origin parent:=base_link name:=velodyne topic:=/velodyne_points organize_cloud:=false hz:=10 lasers:=16 samples:=1875 collision_range:=0.3 min_range:=0.9 max_range:=130.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI} gpu:=false **color_rviz color_gazebo">

    <joint name="${name}_base_mount_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_base_link"/>
    </joint>

    <link name="${name}_base_link">
      <inertial>
        <mass value="0.83"/>
        <origin xyz="0 0 0.03585"/>
        <inertia ixx="${(0.83 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" ixy="0" ixz="0"
          iyy="${(0.83 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" iyz="0"
          izz="${0.5 * 0.83 * (0.0516*0.0516)}"/>
      </inertial>
      <visual>
        <geometry>
          <mesh filename="package://rlbot_description/meshes/vlp16/vlp16_base_1.stl" />
        </geometry>
				<xacro:insert_block name="color_rviz" />
      </visual>
      <visual>
        <geometry>
          <mesh filename="package://rlbot_description/meshes/vlp16/vlp16_base_2.stl" />
        </geometry>
				<xacro:insert_block name="color_rviz" />
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0.03585"/>
        <geometry>
          <cylinder radius="0.0516" length="0.0717"/>
        </geometry>
      </collision>
    </link>

		<gazebo reference="${name}_base_link">
			<material>Gazebo/${color_gazebo}</material>
		</gazebo>


    <joint name="${name}_base_scan_joint" type="fixed" >
      <origin xyz="0 0 0.0377" rpy="0 0 0" />
      <parent link="${name}_base_link" />
      <child link="${name}"/>
    </joint>

    <link name="${name}">
      <inertial>
        <mass value="0.01"/>
        <origin xyz="0 0 0"/>
        <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
      </inertial>
      <visual>
        <origin xyz="0 0 -0.0377" />
        <geometry>
          <mesh filename="package://rlbot_description/meshes/vlp16/vlp16_scan.dae" />
        </geometry>
				<xacro:insert_block name="color_rviz" />

      </visual>
      <collision>
        <geometry>
          <sphere radius="0"/>
        </geometry>
      </collision>
    </link>



    <!-- Gazebo requires the velodyne_gazebo_plugins package -->
    <gazebo reference="${name}">

      <xacro:if value="${gpu}">
        <sensor type="gpu_ray" name="${name}-vlp16">
          <pose>0 0 0 0 0 0</pose>
          <visualize>false</visualize>
          <update_rate>${hz}</update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>${samples}</samples>
                <resolution>1</resolution>
                <min_angle>${min_angle}</min_angle>
                <max_angle>${max_angle}</max_angle>
              </horizontal>
              <vertical>
                <samples>${lasers}</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>${collision_range}</min>
              <max>${max_range+1}</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="libgazebo_ros_velodyne_gpu_laser.so">
            <topicName>${topic}</topicName>
            <frameName>${name}</frameName>
            <organize_cloud>${organize_cloud}</organize_cloud>
            <min_range>${min_range}</min_range>
            <max_range>${max_range}</max_range>
            <gaussianNoise>${noise}</gaussianNoise>
          </plugin>
        </sensor>
      </xacro:if>
      <xacro:unless value="${gpu}">
        <sensor type="ray" name="${name}-vlp16">
          <pose>0 0 0 0 0 0</pose>
          <visualize>false</visualize>
          <update_rate>${hz}</update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>${samples}</samples>
                <resolution>1</resolution>
                <min_angle>${min_angle}</min_angle>
                <max_angle>${max_angle}</max_angle>
              </horizontal>
              <vertical>
                <samples>${lasers}</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>${collision_range}</min>
              <max>${max_range+1}</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="libgazebo_ros_velodyne_laser.so">
            <topicName>${topic}</topicName>
            <frameName>${name}</frameName>
            <organize_cloud>${organize_cloud}</organize_cloud>
            <min_range>${min_range}</min_range>
            <max_range>${max_range}</max_range>
            <gaussianNoise>${noise}</gaussianNoise>
          </plugin>
        </sensor>
      </xacro:unless>
    </gazebo>

  </xacro:macro>
</robot>
