<?xml version="1.0" ?>
<robot name="softbot" xmlns:xacro="http://ros.org/wiki/xacro">
  <!--  Xacro description of turtlebot robot waffle pi used to play team hunt.-->
  <!--  Miguel Riem Oliveira-->
  <!--  PARI, November 2020-->

  <!--Args are local, properties are global, so we copy the values of the arguments to the properties-->
  <xacro:arg name="name" default="softbot" />
  <xacro:arg name="color" default="Orange" /> <!-- e.g. Red, Orange. Must exist in Gazebo/Colors and be defined in properties.xacro.-->

  <!--Properties are global so they can be viewed from inside the included xacros-->
  <!--  args are used with $(arg arg_name), properties are used as ${property_name}-->
  <xacro:property name="name" value="$(arg name)"/>
  <xacro:property name="color" value="$(arg color)"/>

  <!-- Include other files-->
  <xacro:include filename="$(find softbot_description)/urdf/properties.xacro"/>
  <xacro:include filename="$(find softbot_description)/urdf/VLP-16.urdf.xacro"/>
  <xacro:include filename="$(find softbot_description)/urdf/VLP-16_blue.urdf.xacro"/>
  <xacro:include filename="$(find softbot_description)/urdf/astra.urdf.xacro" />
  <xacro:include filename="$(find softbot_description)/urdf/astra_red.urdf.xacro" />
  <xacro:include filename="$(find softbot_description)/urdf/astra_green.urdf.xacro" />


  <xacro:include filename="$(find softbot_description)/urdf/softbot.gazebo.macro.xacro"/>
  <xacro:gazebo_macro color="$(arg color)" laser_visual="false" camera_visual="false" imu_visual="true"/>

  <!-- Links and joints-->
  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link" />
    <origin xyz="0 0 0.010" rpy="0 0 0"/>
  </joint>

  <link name="base_link">
    <visual>
      <origin xyz="-0.064 0 0.0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/bases/waffle_pi_base.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="light_black"/>
    </visual>

    <collision name="base_link_collision">
      <origin xyz="-0.064 0 0.047" rpy="0 0 0"/>
      <geometry>
        <!--        <box size="0.266 0.266 0.094"/>-->
        <box size="0.3 0.3 0.094"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="1.3729096e+00"/>
      <inertia ixx="8.7002718e-03" ixy="-4.7576583e-05" ixz="1.1160499e-04"
               iyy="8.6195418e-03" iyz="-3.5422299e-06"
               izz="1.4612727e-02" />
    </inertial>
  </link>

  <joint name="wheel_left_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_left_link"/>
    <origin xyz="0.0 0.144 0.023" rpy="-1.57 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="wheel_left_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.018" radius="0.033"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="2.8498940e-02" />
      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
               iyy="1.1192413e-05" iyz="-1.4400107e-11"
               izz="2.0712558e-05" />
    </inertial>
  </link>

  <joint name="wheel_right_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_right_link"/>
    <origin xyz="0.0 -0.144 0.023" rpy="-1.57 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="wheel_right_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.018" radius="0.033"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="2.8498940e-02" />
      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
               iyy="1.1192413e-05" iyz="-1.4400107e-11"
               izz="2.0712558e-05" />
    </inertial>
  </link>

  <joint name="caster_back_right_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_back_right_link"/>
    <origin xyz="-0.177 -0.064 -0.004" rpy="-1.57 0 0"/>
  </joint>

  <link name="caster_back_right_link">
    <collision>
      <origin xyz="0 0.001 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.030 0.009 0.020"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="0.005" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

  <joint name="caster_back_left_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_back_left_link"/>
    <origin xyz="-0.177 0.064 -0.004" rpy="-1.57 0 0"/>
  </joint>

  <link name="caster_back_left_link">
    <collision>
      <origin xyz="0 0.001 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.030 0.009 0.020"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="0.005" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0.0 0 0.068" rpy="0 0 0"/>
  </joint>

  <link name="imu_link"/>

  <link name="platform_support_leg1_link">
    <visual>
      <origin xyz="0 0 -0.1" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.004"/>
      </geometry>
      <material name="grey"/>
    </visual>
  </link>

  <joint name="base_link_to_platform_support_leg1_link" type="fixed">
    <parent link="base_link"/>
    <child link="platform_support_leg1_link"/>
    <origin xyz="-0.114 0.05 0.222" rpy="0 0 0"/>
  </joint>

  <link name="platform_support_leg2_link">
    <visual>
      <origin xyz="0 0 -0.1" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.004"/>
      </geometry>
      <material name="grey"/>
    </visual>
  </link>

  <joint name="base_link_to_platform_support_leg2_link" type="fixed">
    <parent link="base_link"/>
    <child link="platform_support_leg2_link"/>
    <origin xyz="-0.114 -0.05 0.222" rpy="0 0 0"/>
  </joint>

  <link name="platform_support_leg3_link">
    <visual>
      <origin xyz="0 0 -0.1" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.004"/>
      </geometry>
      <material name="grey"/>
    </visual>
  </link>

  <joint name="base_link_to_platform_support_leg3_link" type="fixed">
    <parent link="base_link"/>
    <child link="platform_support_leg3_link"/>
    <origin xyz="-0.014 0.05 0.222" rpy="0 0 0"/>
  </joint>

  <link name="platform_support_leg4_link">
    <visual>
      <origin xyz="0 0 -0.1" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.004"/>
      </geometry>
      <material name="grey"/>
    </visual>
  </link>

  <joint name="base_link_to_platform_support_leg4_link" type="fixed">
    <parent link="base_link"/>
    <child link="platform_support_leg4_link"/>
    <origin xyz="-0.014 -0.05 0.222" rpy="0 0 0"/>
  </joint>

  <link name="lidar3d_plate_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.14 0.14 0.005"/>
      </geometry>
      <material name="grey"/>
    </visual>
  </link>

  <joint name="platform_support_leg1_link_to_lidar3d_plate_link" type="fixed">
    <parent link="platform_support_leg1_link"/>
    <child link="lidar3d_plate_link"/>
    <origin xyz="0.05 -0.05 0" rpy="0 0 0"/>
  </joint>


  <xacro:VLP-16_blue name="lidar3d" parent="lidar3d_plate_link" topic="/lidar3d/points">
    <origin xyz="0 0 0" rpy="0 0 0" />
  </xacro:VLP-16_blue>

  <link name="front_left_camera_support_link">
  </link>

  <joint name="platform_support_leg3_link_to_front_left_camera_support_link" type="fixed">
    <origin xyz="0.02 0 -0.09" rpy="0.01 -0.35 0.2"/>
    <parent link="platform_support_leg3_link"/>
    <child link="front_left_camera_support_link"/>
  </joint>

  <xacro:sensor_astra_red prefix="front_left_camera" parent="front_left_camera_support_link">
    <origin xyz="0.02 0.07 0.015" rpy="0 0.0 0" />
  </xacro:sensor_astra_red>

  <link name="front_right_camera_support_link">
  </link>

  <joint name="platform_support_leg4_link_to_front_right_camera_support_link" type="fixed">
    <origin xyz="0.02 0 -0.05" rpy="0.01 -0.35 -0.22"/>
    <parent link="platform_support_leg4_link"/>
    <child link="front_right_camera_support_link"/>
  </joint>

  <xacro:sensor_astra_green prefix="front_right_camera" parent="front_right_camera_support_link">
    <origin xyz="0.02 0.07 0.015" rpy="0 0.0 0" />
  </xacro:sensor_astra_green>

<!--  <link name="back_camera_support_link">-->
<!--  </link>-->

<!--  <joint name="platform_support_leg1_link_to_back_camera_support_link" type="fixed">-->
<!--    <origin xyz="-0.02 -0.05 -0.10" rpy="0.01 -0.35 3.14"/>-->
<!--    <parent link="platform_support_leg1_link"/>-->
<!--    <child link="back_camera_support_link"/>-->
<!--  </joint>-->

<!--  <xacro:sensor_astra prefix="back_camera" parent="back_camera_support_link">-->
<!--    <origin xyz="0.02 0.07 0.015" rpy="0 0.0 0" />-->
<!--  </xacro:sensor_astra>-->
</robot>