<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
     xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
     xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
     xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" name="myrobot">

    <!-- Included URDF Files -->
    <include filename="$(find myrobot_description)/urdf/myrobot_materials.xacro" />
    <include filename="$(find myrobot_description)/urdf/myrobot_constants.xacro" />
    <include filename="$(find myrobot_description)/urdf/myrobot_wheel.xacro" />



<xacro:macro name="myrobot">
        <!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin,
             navigation stack depends on this frame -->
        <link name="base_footprint">
            <inertial>
                <mass value="0.0001" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
                         iyy="0.0001" iyz="0.0"
                         izz="0.0001" />
            </inertial>

            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
                <material name="Green" />
            </visual>

            <collision>
                <origin xyz="0 0 ${base_size_z/2 + 0.051}" rpy="0 0 0" />
                <geometry>
                  <box size="0.001 0.001 0.001" />
                </geometry>
            </collision>
        </link>

        <joint name="base_footprint_joint" type="fixed">
            <!-- NB: While you would think this would make it go up, it is oddly reversed.
                 This moves the joint to 0,0,0 where we want it for rviz, etc. -->
            <origin xyz="0 0 ${base_size_z/2 + 0.051}" rpy="0 0 0" />
            <parent link="base_footprint"/>
            <child link="base_link" />
        </joint>

    <link name="base_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="${base_size_x} ${base_size_y} ${base_size_z}"/>
            </geometry>
            <material name="Grey2" />
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="${base_size_x} ${base_size_y} ${base_size_z}"/>
            </geometry>
        </collision>

            <inertial>
                <mass value="${body_mass}" />
                <origin xyz="0 0 0" />
                <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                         iyy="1.0" iyz="0.0"
                         izz="1.0" />
            </inertial>

        <gazebo reference="base_link">
            <material>Gazebo/Gray</material>
            <turnGravityOff>false</turnGravityOff>
        </gazebo>
    </link>

    <!-- MACRO INSTANTIATION -->
    <xacro:wheel prefix="front_right" front="1" right="1" />
    <xacro:wheel prefix="front_left" front="1" right="-1" />
    <xacro:wheel prefix="back_right" front="-1" right="1" />
    <xacro:wheel prefix="back_left" front="-1" right="-1" />

  <gazebo>
    <controller:myrobot_diffdrive_plugin name="differential_drive_plugin" plugin="libmyrobot_diffdrive_plugin.so">
      <alwaysOn>true</alwaysOn>
      <node_namespace>myrobot_node</node_namespace>
      <front_left_wheel_joint>front_left_wheel_joint</front_left_wheel_joint>
      <front_right_wheel_joint>front_right_wheel_joint</front_right_wheel_joint>
      <back_left_wheel_joint>back_left_wheel_joint</back_left_wheel_joint>
      <back_right_wheel_joint>back_right_wheel_joint</back_right_wheel_joint>
      <wheel_separation>${body_width}</wheel_separation>
      <wheel_diameter>${wheel_diameter}</wheel_diameter>
      <base_geom>base_link_geom_base_link</base_geom>
    <interface:position name="position_iface_0"/>
      <updateRate>100</updateRate>
      <torque>1.0</torque>
    </controller:myrobot_diffdrive_plugin>



            <controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>100.0</updateRate>
                <bodyName>base_link</bodyName>
                <topicName>base_pose_ground_truth</topicName>
                <gaussianNoise>0.01</gaussianNoise>
                <frameName>map</frameName>
                <xyzOffsets>0 0 0</xyzOffsets>
                <rpyOffsets>0 0 0</rpyOffsets>
                <interface:position name="p3d_base_position"/>
            </controller:gazebo_ros_p3d>

            <canonicalBody>base_footprint</canonicalBody>

            <!-- this publishes empty joint_states due to no transmission, but
             triggering robot_state_publisher to publish tf between fixed joints in erratic,
             (e.g. base_laser_link for the base_scan frame) -->
            <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>100.0</updateRate>
                <interface:audio name="gazebo_ros_controller_manager_dummy_iface" />
            </controller:gazebo_ros_controller_manager>

  </gazebo>

</xacro:macro>

    <xacro:myrobot />
</robot>