<?xml version="1.0"?>
<robot name="sensor_velodyne" xmlns:xacro="http://ros.org/wiki/xacro"
                              xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
                              xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">

  <xacro:property name="velodyne_link" value="0.1" /> <!-- Size of square 'hokuyo' box -->
  <xacro:macro name="sensor_velodyne" params="name parent *origin min_angle max_angle samples">
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin"/>
      <axis xyz="0 1 0"/>
      <parent link="${parent}"/>
      <child link="${name}_link"/>
    </joint>

    <link name="${name}_link">
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <box size="0.1 0.1 0.1"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://tsukuba2022/meshes/velodyne.dae"/>
        </geometry>
      </visual>

      <inertial>
        <mass value="0.0001"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial>
    </link>
    
    <gazebo reference="velodyne_link">
      <sensor type="ray" name="head_velodyne_sensor">
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <update_rate>10</update_rate>
        <ray>
          <scan>
            <horizontal>
              <resolution>1</resolution>
              <samples>1875</samples>
              <min_angle>${-PI}</min_angle>
              <max_angle>${PI}</max_angle>
            </horizontal>
            <vertical>
              <resolution>1</resolution>
              <samples>16</samples>
              <min_angle>${-PI/12}</min_angle>
              <max_angle>${PI/12}</max_angle>
            </vertical>
          </scan>
          <range>
            <min>0.9</min>
            <max>100</max>
            <resolution>0.001</resolution>
          </range>
        </ray>
        
        <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
          <topicName>velodyne_points</topicName>
          <frameName>velodyne_link</frameName>
          <min_range>0.9</min_range>
          <max_range>100</max_range>
          <gaussianNoise>0.01</gaussianNoise>
        </plugin>
      </sensor>
    </gazebo>
    
    <!--
    <gazebo reference="velodyne_link">
    <sensor type="ray" name="head_velodyne_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <resolution>1</resolution>
            <samples>${samples}</samples>
            <min_angle>${min_angle}</min_angle>
            <max_angle>${max_angle}</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>100.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>velodyne_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  -->

  </xacro:macro>

</robot>
