<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:macro name="steer_controller_plugin_gazebo" params="prefix steer_joint steerdrive_wheel_joint wheel_radius">
    <gazebo>
      <plugin name="steer_drive_controller" filename="libgazebo_ros_steer_drive.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>100</updateRate>
        <steerJoint>${prefix}base2steer_joint</steerJoint>
        <driveJoint>${prefix}steer2sd_wheel_joint</driveJoint>
        <fixedWheelLeftJoint>${prefix}fixed_left_wheel_joint</fixedWheelLeftJoint>
        <fixedWheelRightJoint>${prefix}fixed_right_wheel_joint</fixedWheelRightJoint>
  
        <wheelDiameter>1.05</wheelDiameter>
        <steeringFixWheelDistanceX>1.1</steeringFixWheelDistanceX>
        <steeringFixWheelDistanceY>0.0</steeringFixWheelDistanceY>
        
        <steerTorque>1000</steerTorque>
        <driveTorque>1000</driveTorque>
  
        <commandTopic>mobile_base_controller/cmd_vel</commandTopic>
        <odometryTopic>mobile_base_controller/odom</odometryTopic>
        <odometryFrame>world</odometryFrame>
        <robotBaseFrame>base_footprint</robotBaseFrame>
  
        <odomEncSteeringAngleOffset>0.01</odomEncSteeringAngleOffset>
   
        <!--odometrySource>encoder</odometrySource-->
        <publishWheelTF>false</publishWheelTF>
        <publishWheelJointState>true</publishWheelJointState>

      </plugin>
    </gazebo>
  </xacro:macro>

  <xacro:macro name="set_wheel_friction" params="link friction">
    <gazebo reference="${link}">
      <mu1 value="${friction}"/>
      <mu2 value="${friction}"/>
      <kp value="10000000.0"/>
      <kd value="1.0"/>
      <minDepth>0.01</minDepth>
    </gazebo>
  </xacro:macro>

  <xacro:macro name="set_all_wheel_frictions" params="prefix">
    <xacro:set_wheel_friction link="${prefix}steer_link" friction="200"/>
    <xacro:set_wheel_friction link="${prefix}drive_wheel_link" friction="200"/>
    <xacro:set_wheel_friction link="${prefix}fixed_wheel_left_link" friction="1"/>
    <xacro:set_wheel_friction link="${prefix}fixed_wheel_right_link" friction="1"/>
  </xacro:macro>

  <xacro:macro name="p3d_base_controller" params="prefix">
    <gazebo>
      <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>50.0</updateRate>
        <bodyName>${prefix}base_footprint</bodyName>
        <topicName>base_pose_ground_truth</topicName>
        <gaussianNoise>0.01</gaussianNoise>
        <frameName>map</frameName>
        <xyzOffsets>0 0 0</xyzOffsets>
        <rpyOffsets>0 0 0</rpyOffsets>
      </plugin>
    </gazebo>
  </xacro:macro>
</robot>
