<?xml version="1.0"?>
<robot name="atlascar2" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!--
    Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
    -->
    <xacro:include filename="$(find atlascar2_description)/urdf/atlascar2_wheel.urdf.xacro"/>
    <xacro:include filename="$(find atlascar2_description)/urdf/atlascar2_ackermann.macro.urdf.xacro"/>
    <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
    <!-- <xacro:include filename="$(find sick_ldmrs_description)/urdf/sick_ldmrs_macros.urdf.xacro"/> -->
    <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
    <!-- Constants for robot dimensions -->
    <xacro:property name="length" value="3.475" />
    <xacro:property name="width" value="1.475" />
    <xacro:property name="height" value="1.61"/>
    <xacro:property name="length_offset" value="0.2"/>
    <xacro:property name="height_offset" value="0.1"/>
    <xacro:property name="wheel_radius" value="0.285" /> <!-- Link 1 -->
    <xacro:property name="thickness" value="0.145" /> <!-- Link 2 -->
    <!--  we need two because the car isn't a square-->
    <xacro:property name="axel_offset_x" value="0.5" /> <!-- Space btw top of beam and the each joint -->
    <xacro:property name="axel_offset_y" value="0" /> <!-- Space btw top of beam and the each joint -->
    <xacro:property name="steer_offset" value="0.02" /> <!-- Link 1 -->

    <!-- Add car macro -->
    <xacro:atlascar2 length="${length}" width="${width}" height="${height}" wheel_radius="${wheel_radius}"/>

    <!--    Add sensors-->

    <!-- top-right-camera -->
    <xacro:pointgrey_flea3_joints name="top_right_camera" frame="top_right_camera" parent="base_link" 
                                  x="${length_offset}" y="-0.22" z="${1.35+height_offset}"
                                  rol="0" pit="0.4" yaw="0"/>

    <!-- top-left-camera -->
    <xacro:pointgrey_flea3_joints name="top_left_camera" frame="top_left_camera" parent="base_link" 
                                  x="${length_offset}" y="0.22" z="${1.35+height_offset}"
                                  rol="0" pit="0.4" yaw="0"/>

    <!-- top velodyne lidar -->
    <xacro:VLP-16 name="top_laser" parent="base_link" topic="top_laser/cloud">
      <origin xyz="0 0 ${1.35+height_offset}" rpy="0 0 0"/>
    </xacro:VLP-16>

    <!-- joints for ackermann_steering_controller -->
    <xacro:front_steer name="front" parent="base" radius="${wheel_radius}" thickness="${thickness}"
                       length="${length}" width="${width}" axel_offset="${axel_offset_x}" steer_height="${wheel_radius+steer_offset}">
        <origin xyz="${length/2-axel_offset_x} 0 0" rpy="${-90 * deg_to_rad} 0 ${90 * deg_to_rad}"/>
    </xacro:front_steer>
    <xacro:rear_wheel name="rear" parent="base" radius="${wheel_radius}" thickness="${thickness}">
        <origin xyz="${-length/2+axel_offset_x} 0 0" rpy="${-90 * deg_to_rad} 0 ${-90 * deg_to_rad}"/>
    </xacro:rear_wheel>

    <!-- laser sensor lms_ldmrs400001 -->
    <!-- <xacro:sick_ldmrs name="frontal_laser"/> -->
    <!-- <joint name="base_link_to_frontal_laser_mount_link" type="fixed"> -->
      <!-- <origin xyz="${length/2 + length_offset} 0 ${height_offset}" rpy="0 0 0"/> -->
      <!-- <parent link="base_link"/> -->
      <!-- <child link="frontal_laser_mount_link"/> -->
    <!-- </joint> -->

    <!-- left lms151 laser -->
    <xacro:sick_lms1xx_joints frame="left_laser" topic="/left_laser/scan" parent="base_link" 
                                  x="${length/2 + length_offset - 0.08}" y="0.76" z="${0.27+height_offset}"
                                  rol="0" pit="0" yaw="0.7854"/>

    <!-- right lms151 laser -->
    <xacro:sick_lms1xx_joints frame="right_laser" topic="/right_laser/scan" parent="base_link" 
                                  x="${length/2 + length_offset - 0.08}" y="-0.76" z="${0.27+height_offset}"
                                  rol="0" pit="0" yaw="-0.7854"/>

    <!-- Wheels -->
    <xacro:front_wheel_lr name="front_right" parent="base" radius="${wheel_radius}" thickness="${thickness}"
                          length="${length}" width="${width}" axel_offset="${axel_offset_x}" right_left="1" steer_height="${wheel_radius+steer_offset}">
        <origin xyz="${length/2-axel_offset_x} ${width/2+axel_offset_y} 0" rpy="${-90 * deg_to_rad} 0 0"/>
    </xacro:front_wheel_lr>
    <xacro:front_wheel_lr name="front_left" parent="base" radius="${wheel_radius}" thickness="${thickness}"
                          length="${length}" width="${width}" axel_offset="${axel_offset_x}" right_left="-1" steer_height="${wheel_radius+steer_offset}">
        <origin xyz="${length/2-axel_offset_x} ${-width/2-axel_offset_y} 0" rpy="${-90 * deg_to_rad} 0 0"/>
    </xacro:front_wheel_lr>
    <xacro:rear_wheel_lr name="rear_right" parent="base" radius="${wheel_radius}" right_left="1" thickness="${thickness}">
        <origin xyz="${-length/2+axel_offset_x - 0.05} ${width/2+axel_offset_y} 0" rpy="${-90 * deg_to_rad} 0 ${-90 * deg_to_rad}"/>
    </xacro:rear_wheel_lr>
    <xacro:rear_wheel_lr name="rear_left" parent="base" radius="${wheel_radius}" right_left="-1" thickness="${thickness}">
        <origin xyz="${-length/2+axel_offset_x - 0.05} ${-width/2-axel_offset_y} 0" rpy="${-90 * deg_to_rad} 0 ${90 * deg_to_rad}"/>
    </xacro:rear_wheel_lr>

    <!-- Gazebo plugin for ROS Control -->
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotSimType>steer_bot_hardware_gazebo/SteerBotHardwareGazebo</robotSimType>
        </plugin>
    </gazebo>

    <!-- Colour -->
    <gazebo reference="base_footprint">
        <material>Gazebo/Purple</material>
    </gazebo>

</robot>