<?xml version="1.0"?>

<robot name="scout_v2" 
    xmlns:xacro="http://ros.org/wiki/xacro">

    <xacro:arg name="urdf_extras" default="empty.urdf" />

    <!-- Included URDF/XACRO Files -->
    <xacro:include filename="$(find scout_description)/urdf/scout_wheel.xacro" />

    <xacro:property name="M_PI" value="3.14159"/>

    <!-- Base Size -->
    <xacro:property name="base_x_size" value="0.9250000" />
    <xacro:property name="base_y_size" value="0.380000" />
    <xacro:property name="base_z_size" value="0.210000" />

    <!-- Wheel Mounting Positions -->
    <xacro:property name="wheelbase" value="0.498" />
    <xacro:property name="track" value="0.58306" />
    <xacro:property name="wheel_vertical_offset" value="-0.0702" />

    <!-- Wheel Properties -->
    <xacro:property name="wheel_length" value="1.1653e-01" />
    <xacro:property name="wheel_radius" value="1.6459e-01" />

    <!-- Base link is the center of the robot's bottom plate -->
    <link name="base_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://scout_description/meshes/body_assembly.dae" />
            </geometry>
        </visual>
        <collision>
            <origin xyz="0 0 0.008" rpy="0 0 0" />
            <geometry>
                <box size="${base_x_size} ${base_y_size} ${base_z_size}"/>
            </geometry>
        </collision>
        <collision>
            <origin xyz="0 0 ${base_z_size/6}" rpy="0 0 0" />
            <geometry>
                <box size="${base_x_size/6} ${base_y_size*1.65} ${base_z_size/3}"/>
            </geometry>
        </collision>
    </link>

    <!-- Base footprint is on the ground under the robot -->
    <link name="base_footprint"/>

    <joint name="base_footprint_joint" type="fixed">
        <origin xyz="0 0 ${wheel_vertical_offset - wheel_radius}" rpy="0 0 0" />
        <parent link="base_link" />
        <child link="base_footprint" />
    </joint>

    <!-- Interial link stores the robot's inertial information -->
    <link name="inertial_link">
        <inertial>
            <mass value="46.034" />
            <origin xyz="-0.00065 -0.085 0.062" />
            <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" />
        </inertial>
    </link>

    <joint name="inertial_joint" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0" />
        <parent link="base_link" />
        <child link="inertial_link" />
    </joint>

    <!-- IMU Link is the standard mounting position for the UM6 IMU.-->
    <!-- Can be modified with environment variables in /etc/ros/setup.bash -->
    <!-- <link name="imu_link"/>
  <joint name="imu_joint" type="fixed">
    <origin xyz="$(optenv scout_IMU_XYZ 0.19 0 0.149)" rpy="$(optenv scout_IMU_RPY 0 -1.5708 3.1416)" />
    <parent link="base_link" />
    <child link="imu_link" />
  </joint>
  <gazebo reference="imu_link">
  </gazebo> -->

    <!-- scout wheel macros -->
    <xacro:scout_wheel wheel_prefix="front_left">
        <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
    </xacro:scout_wheel>
    <xacro:scout_wheel wheel_prefix="front_right">
        <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
    </xacro:scout_wheel>
    <xacro:scout_wheel wheel_prefix="rear_left">
        <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
    </xacro:scout_wheel>
    <xacro:scout_wheel wheel_prefix="rear_right">
        <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
    </xacro:scout_wheel>

    <!-- <xacro:scout_decorate /> -->

    <!-- <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
      <updateRate>50.0</updateRate>
      <bodyName>base_link</bodyName>
      <topicName>imu/data</topicName>
      <accelDrift>0.005 0.005 0.005</accelDrift>
      <accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
      <rateDrift>0.005 0.005 0.005 </rateDrift>
      <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>
      <headingDrift>0.005</headingDrift>
      <headingGaussianNoise>0.005</headingGaussianNoise>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
      <updateRate>40</updateRate>
      <bodyName>base_link</bodyName>
      <frameId>base_link</frameId>
      <topicName>navsat/fix</topicName>
      <velocityTopicName>navsat/vel</velocityTopicName>
      <referenceLatitude>49.9</referenceLatitude>
      <referenceLongitude>8.9</referenceLongitude>
      <referenceHeading>0</referenceHeading>
      <referenceAltitude>0</referenceAltitude>
      <drift>0.0001 0.0001 0.0001</drift>
    </plugin>
  </gazebo> -->

    <!-- Optional custom includes. -->
    <!-- <xacro:include filename="$(arg urdf_extras)" /> -->

</robot>
