<?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 pointgrey_camera_description)/urdf/pointgrey_flea3.urdf.xacro"/>
    <xacro:include filename="$(find sick_ldmrs_description)/urdf/sick_ldmrs_macros.urdf.xacro"/>
    <xacro:include filename="$(find lms1xx)/urdf/sick_lms1xx.urdf.xacro"/>
    <xacro:include filename="$(find atlascar2_description)/urdf/asus_xtion.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="weight" value="1080"/>
    <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 -->

    <!-- Links: inertial,visual,collision -->
    <link name="base_link">
        <inertial>
            <!-- origin is relative -->
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="${weight}"/>
            <inertia ixx="${weight*(width*width + height*height)/12}" ixy="0" ixz="0"
                     iyy="${weight*(length*length + height*height)/12}" iyz="0"
                     izz="${weight*(length*length + height*height)/12}"/>
        </inertial>
        <visual>
            <origin xyz=" ${length/2} ${-width/2 - 0.15} ${-wheel_radius + 0.05}" rpy="0 0 ${90*deg_to_rad}"/>
            <geometry>
                <mesh filename="file:///$(find atlascar2_description)/meshes/atlascar2_nowheels.dae"/>
            </geometry>
        </visual>
        <collision>
            <!-- origin is relative -->
            <origin xyz=" 0 0 ${-wheel_radius + 0.05 + height/2}"/>
            <geometry>
                <box size="${length} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>


    <link name="base_footprint">
        <collision>
            <origin xyz="0 0 0"/>
            <geometry>
                <sphere radius="0.00000001"/>
            </geometry>
        </collision>
    </link>
    <joint name="base_footprint_joint" type="fixed">
        <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
        <child link="base_link"/>
        <parent link="base_footprint"/>
    </joint>

<!--    Add sensors-->
<!-- top-right-camera -->
    <xacro:pointgrey_flea3 name="top_right_camera" frame="top_right_camera"/>
    <joint name="base_link_to_top_right_camera" type="fixed">
      <origin xyz="${length_offset} -0.22 ${1.35+height_offset}" rpy="0 0.4 0"/>
      <parent link="base_link"/>
      <child link="top_right_camera"/>
    </joint>
    <!-- top-left-camera -->
    <xacro:pointgrey_flea3 name="top_left_camera" frame="top_left_camera"/>
    <joint name="base_link_to_top_left_camera" type="fixed">
      <origin xyz="${length_offset} 0.22 ${1.35+height_offset}" rpy="0 0.4 0"/>
      <parent link="base_link"/>
      <child link="top_left_camera"/>
    </joint>
    <!-- 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 frame="right_laser"/>
    <joint name="base_link_to_right_laser" type="fixed">
      <origin xyz="${length/2 + length_offset - 0.08} -0.76 ${height_offset+0.27}" rpy="0 0 ${-0.7854}"/>
      <parent link="base_link"/>
      <child link="right_laser"/>
    </joint>
    <!-- right lms151 laser -->
    <xacro:sick_lms1xx frame="left_laser"/>
    <joint name="base_link_to_left_laser" type="fixed">
      <origin xyz="${length/2 + length_offset - 0.08} 0.76 ${height_offset+0.27}" rpy="0 0 ${0.7854}"/>
      <parent link="base_link"/>
      <child link="left_laser"/>
    </joint>
    <!-- RGB-D camera -->
    <xacro:asus_camera_model name="top_length_rgbd_camera"/>
    <joint name="base_link_to_top_length_rgbd_camera" type="fixed">
      <!--            <origin xyz="-1.510 -0.010 1.397" rpy="-0.000 0.223 -0.000" />-->
      <origin xyz="${length_offset} -0.010 ${height_offset+1.338}" rpy="-0.000 0.207 -0.000"/>
      <!--            <origin xyz="-1.51 -0.01 1.27" rpy="0 0.4 0" />-->
      <parent link="base_link"/>
      <child link="top_length_rgbd_camera_link"/>
    </joint>

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

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