<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:arg name="robot_type" default="demo" />     
  <xacro:property name="robot_type" value="$(arg robot_type)" />  <!-- necessary because args cannot be accessed inside ${} expressions -->

  <xacro:include filename="$(find cititruck_description)/urdf/include/common_properties.urdf.xacro" />
  <xacro:include filename="$(find cititruck_description)/urdf/include/cititruck.gazebo.xacro" />
  <xacro:include filename="$(find cititruck_description)/urdf/include/cititruck.transmission.xacro" />
  <xacro:include filename="$(find cititruck_description)/urdf/include/imu.gazebo.urdf.xacro" />
  <xacro:include filename="$(find cititruck_description)/urdf/include/sick_s300.urdf.xacro" />
  
  <xacro:property name="deg_to_rad" value="0.017453293" />

  <!-- The inertia for the MiR platform is intentionally chosen to be smaller than
  the bounding box and also shifted a bit to the back, because most of the mass
  is in the lower center back (because of the batteries). -->

  <xacro:macro name="actuated_wheel" params="prefix locationprefix locationright">
    <joint name="fixed_${locationprefix}_wheel_joint" type="continuous">
      <parent link="${prefix}base_link"/>
      <child link="${prefix}fixed_wheel_${locationprefix}_link"/>
      <origin xyz="${locationright}" rpy="0 0 0" />
      <axis xyz="0 1 0" />
    </joint>

    <link name="${prefix}fixed_wheel_${locationprefix}_link">
      <inertial>
        <mass value="1"/>
        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
        <origin/>
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <geometry>
          <cylinder radius="0.05" length="0.08" />
        </geometry>
        <xacro:insert_block name="material_black" />
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
        <geometry>
          <cylinder radius="0.04" length="0.08" />
        </geometry>
      </collision>
    </link>

    <gazebo reference="${prefix}fixed_wheel_${locationprefix}_link">
      <material>Gazebo/FlatBlack</material>
    </gazebo>
  </xacro:macro>
  
  <xacro:macro name="fork" params="locationprefix locationright"> 
      <xacro:box_inertial mass="5" x="1" y="0" z="0">
        <origin xyz="0 0 0" rpy="0 0 0" />
      </xacro:box_inertial>
      <visual>
        <origin xyz="${locationright}" rpy="0 0 0" /> 
        <geometry>
          <box size="1.35 0.15 0.04" />
        </geometry>
          <xacro:insert_block name="material_grey" />
      </visual>
  </xacro:macro>

<!-- robot -->
  <xacro:macro name="cititruck" params="prefix">

  <link name="${prefix}base_footprint" />

  <joint name="${prefix}base_joint" type="fixed">
    <parent link="${prefix}base_footprint" />
    <child link="${prefix}base_link" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <!-- Base Link -->
  <link name="${prefix}base_link">
    <xacro:box_inertial mass="30" x="1" y="0" z="0">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </xacro:box_inertial>
    <visual>
      <origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.8/2 + 0.3337 }" rpy="0 0 0" />
      <geometry>
        <box size="0.40 0.65 0.80" />
      </geometry>
        <xacro:insert_block name="material_white" />
    </visual>

    <visual>
      <origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.3 - 0.01}" rpy="0 0 0" />
      <geometry>
        <box size="0.40 0.65 0.02" />
      </geometry>
        <xacro:insert_block name="material_grey" />
    </visual>

    <visual>
      <origin xyz="${1.3268 - 0.21 + 0.02/2} 0 ${0.3 - (0.3 - 0.01)/2}" rpy="0 0 0" />
      <geometry>
        <box size="0.02 0.65 ${0.3 - 0.01}" />
      </geometry>
        <xacro:insert_block name="material_grey" />
    </visual>

    <collision>
      <origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.8/2 + 0.3337 }" rpy="0 0 0" />
      <geometry>
        <box size="0.40 0.70 0.80" />
      </geometry>
    </collision>

    <xacro:fork locationprefix="left" locationright="${1.12 - 1.35/2} 0.205 ${0.05/2 +  0.3 - 0.29}"/>
    <xacro:fork locationprefix="right" locationright="${1.12 - 1.35/2} -0.205 ${0.05/2 +  0.3 - 0.29}"/>

  </link>

  <gazebo reference="${prefix}base_link">
    <material>Gazebo/White</material>
  </gazebo>

  <!-- <xacro:fork prefix="${prefix}" locationprefix="left" parent="base_link" locationright="${0.9 - 1.15/2} 0.205 ${0.05/2 +  0.3 - 0.29}"/> -->
  <!-- <xacro:fork prefix="${prefix}" locationprefix="right" parent="base_link" locationright="${0.9 - 1.15/2} -0.205 ${0.05/2 +  0.3 - 0.29}"/> -->

  <joint name="${prefix}base2steer_joint" type="revolute">
    <parent link="${prefix}base_link"/>
    <child link="${prefix}steer_link"/>
    <limit effort="10000.0" lower="-1.8" upper="1.8" velocity="2500"/>
    <origin xyz="1.3268 0 0.125" rpy="0 0 0" />
    <axis xyz="0 0 1" />
  </joint>
  <xacro:cititruck_steer_transmission prefix="${prefix}" locationprefix="base2steer"/>

  <link name="${prefix}steer_link">
    <inertial>
      <mass value="45"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
      <origin/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder radius="0.09" length="0.1" />
      </geometry>
      <xacro:insert_block name="material_dark_grey" />
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder radius="0.09" length="0.1" />
      </geometry>
    </collision>
  </link>

  <gazebo reference="${prefix}steer_link" >
    <material>Gazebo/FlatBlack</material>
  </gazebo> 

  <link name="${prefix}steer_link_2" />

  <joint name="${prefix}steer2steer_2_joint" type="fixed">
    <parent link="${prefix}steer_link" />
    <child link="${prefix}steer_link_2" />
    <origin xyz="0 0 1.02" rpy="0 0 0" />
  </joint>

  <joint name="${prefix}steer2sd_wheel_joint" type="continuous">
    <parent link="${prefix}steer_link"/>
    <child link="${prefix}drive_wheel_link"/>
    <limit effort="10000.0" velocity="100"/>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <axis xyz="0 1 0" />
  </joint>
  <xacro:cititruck_wheel_transmission prefix="${prefix}" locationprefix="steer2sd"/>

  <link name="${prefix}drive_wheel_link">
    <inertial>
      <mass value="10"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
      <origin/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder radius="0.125" length="0.08" />
      </geometry>
      <xacro:insert_block name="material_yellow" />
    </visual>
    
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder radius="0.125" length="0.08" />
      </geometry>
    </collision>
    
  </link>

  <gazebo reference="${prefix}drive_wheel_link" >
    <material>Gazebo/FlatBlack</material>
  </gazebo>

  <xacro:actuated_wheel prefix="${prefix}" locationprefix="left" locationright="0.0 0.205 ${0.05}"/>
  <xacro:actuated_wheel prefix="${prefix}" locationprefix="right" locationright="0.0 -0.205 ${0.05}" />

  <!-- IMU -->
  <joint name="${prefix}base_link_to_imu_joint" type="fixed">
    <parent link="${prefix}base_link" />
    <child link="${prefix}imu_link" />
    <origin xyz="${1.3268 -0.15} 0.0 0.3" rpy="0 0 0" />  <!-- same as real MiR -->
  </joint>

  <link name="${prefix}imu_link" />

  <xacro:imu_gazebo link="${prefix}imu_link" imu_topic="imu_data" update_rate="50.0" />

  <!-- Create an alias for imu_link. This is necessary because the real MiR's
        TF has imu_link, but the imu_data topic is published in the imu_frame
        frame. -->
  <joint name="${prefix}imu_link_to_imu_frame_joint" type="fixed">
    <parent link="${prefix}imu_link" />
    <child link="${prefix}imu_frame" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <link name="${prefix}imu_frame" />


  <!-- Laser scanners -->
  <joint name="${prefix}base_link_to_left_laser_joint" type="fixed">
    <parent link="${prefix}base_link" />
    <child link="${prefix}left_laser_link" />
    <origin xyz="1.5447 0.2953 0.1743" rpy="0.0 0.0 ${0.25 * pi}" />
  </joint>
  <xacro:sick_s300 prefix="${prefix}" link="left_laser_link" topic="l_scan" />

  <joint name="${prefix}base_link_to_right_laser_joint" type="fixed">
    <parent link="${prefix}base_link" />
    <child link="${prefix}right_laser_link" />
    <origin xyz="1.5447 -0.2953 0.1743" rpy="0.0 0.0 ${-0.25 * pi}" />
  </joint>

  <xacro:sick_s300 prefix="${prefix}" link="right_laser_link" topic="r_scan" />

  <!-- set the gazebo friction parameters for the wheels -->
  <xacro:set_all_wheel_frictions prefix="${prefix}"/>

  <xacro:p3d_base_controller prefix="${prefix}" />
</xacro:macro>

</robot>
