<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="workshop_infrastructure">

  <gazebo> <static>true</static> </gazebo>
  
  <link name="workshop_anchor">
    <visual> <geometry><sphere radius="0.01"/></geometry> </visual>
    <inertial>
      <mass value="100"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
  </link>

  <material name="lidar_black"> <color rgba="0.1 0.1 0.1 1"/> </material>

  <xacro:macro name="solid_state_lidar" params="name x y z yaw">
    <joint name="${name}_joint" type="fixed">
      <parent link="workshop_anchor"/>
      <child link="${name}_link"/>
      <origin xyz="${x} ${y} ${z}" rpy="0 0.35 ${yaw}"/>
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.5"/> <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      </inertial>
      <visual>
        <geometry><box size="0.114 0.136 0.049"/></geometry> 
        <material name="lidar_black"/>
      </visual>
      <collision>
        <geometry><box size="0.114 0.136 0.049"/></geometry>
      </collision>
    </link>

    <gazebo reference="${name}_link">
      <material>Gazebo/FlatBlack</material>
      
      <sensor type="gpu_ray" name="${name}_sensor">
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize> 
        <update_rate>10</update_rate> <ray>
          <scan>
            <horizontal>
              <samples>600</samples>
              <resolution>1</resolution>
              <min_angle>-0.7854</min_angle>
              <max_angle>0.7854</max_angle>
            </horizontal>
            
            <vertical>
              <samples>128</samples>
              <resolution>1</resolution>
              <min_angle>-0.2181</min_angle>
              <max_angle>0.2251</max_angle>
            </vertical>
          </scan>
          
          <range>
            <min>0.5</min> <max>210.0</max>
          </range>
          
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.03</stddev>
          </noise>
        </ray>
        
        <plugin name="${name}_controller" filename="libgazebo_ros_ray_sensor.so">
          <ros>
            <namespace>/infrastructure</namespace>
            <remapping>~/out:=${name}/points</remapping>
          </ros>
          <output_type>sensor_msgs/PointCloud2</output_type>
          <frame_name>${name}_link</frame_name>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>

  <xacro:solid_state_lidar name="lidar_fl" x="4.8"  y="4.8"  z="2.4" yaw="-2.356"/>
  <xacro:solid_state_lidar name="lidar_fr" x="4.8"  y="-4.8" z="2.4" yaw="2.356"/>
  <xacro:solid_state_lidar name="lidar_bl" x="-4.8" y="4.8"  z="2.4" yaw="-0.785"/>
  <xacro:solid_state_lidar name="lidar_br" x="-4.8" y="-4.8" z="2.4" yaw="0.785"/>

</robot>
