<?xml version="1.0"?>
<robot name="gazebo_macro" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="gazebo_macro" params="color laser_visual camera_visual imu_visual">
<!--  <xacro:arg name="laser_visual"  default="false"/>-->
<!--  <xacro:arg name="camera_visual" default="false"/>-->
<!--  <xacro:arg name="imu_visual"    default="false"/>-->

  <gazebo reference="base_link">
<!--    <material>Gazebo/DarkGrey</material>-->
    <material>Gazebo/${color}</material>
  </gazebo>

  <gazebo reference="base_link">
    <sensor name="base_link" type="contact">
      <update_rate>1000.0</update_rate>
      <always_on>true</always_on>
      <contact>
        <collision>base_footprint_fixed_joint_lump__base_link_collision_collision</collision>
      </contact>
      <plugin name="base_link_contact_sensor_plugin" filename="libgazebo_ros_bumper.so">
        <bumperTopicName>contact</bumperTopicName>
        <frameName>world</frameName>
      </plugin>
    </sensor>
  </gazebo>

  <gazebo reference="wheel_left_link">
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <kp>500000.0</kp>
    <kd>10.0</kd>
    <minDepth>0.001</minDepth>
    <maxVel>0.1</maxVel>
    <fdir1>1 0 0</fdir1>
    <!--    Gazebo properties are global: https://answers.ros.org/question/216975/loading-variables-with-xacroinclude/ -->
    <material>Gazebo/${color}</material>
  </gazebo>

  <gazebo reference="wheel_right_link">
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <kp>500000.0</kp>
    <kd>10.0</kd>
    <minDepth>0.001</minDepth>
    <maxVel>0.01</maxVel>
    <fdir1>1 0 0</fdir1>
    <!--    Gazebo properties are global: https://answers.ros.org/question/216975/loading-variables-with-xacroinclude/ -->
    <material>Gazebo/${color}</material>
  </gazebo>

  <gazebo reference="caster_back_right_link">
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <minDepth>0.001</minDepth>
    <maxVel>1.0</maxVel>
    <material>Gazebo/FlatBlack</material>
  </gazebo>

  <gazebo reference="caster_back_left_link">
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <minDepth>0.001</minDepth>
    <maxVel>1.0</maxVel>
    <material>Gazebo/FlatBlack</material>
  </gazebo>

  <gazebo reference="imu_link">
    <sensor type="imu" name="imu">
      <always_on>true</always_on>
      <visualize>${imu_visual}</visualize>
    </sensor>
    <material>Gazebo/Grey</material>
  </gazebo>

  <!--Using gazebo or ROS native controller for differential drives.-->
  <xacro:include filename="$(find softbot_description)/urdf/ros_diff_drive.gazebo.xacro"/>

  <gazebo>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <alwaysOn>true</alwaysOn>
      <bodyName>imu_link</bodyName>
      <frameName>imu_link</frameName>
      <topicName>imu</topicName>
      <serviceName>imu_service</serviceName>
      <gaussianNoise>0.0</gaussianNoise>
      <updateRate>200</updateRate>
      <imu>
        <noise>
          <type>gaussian</type>
          <rate>
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </plugin>
  </gazebo>

  <gazebo reference="base_scan">
    <material>Gazebo/FlatBlack</material>
    <sensor type="ray" name="lds_lfcd_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>${laser_visual}</visualize>
<!--      <visualize>true</visualize>-->
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>0.0</min_angle>
            <max_angle>6.28319</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.120</min>
          <max>3.5</max>
          <resolution>0.015</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName>
        <frameName>base_scan</frameName>
      </plugin>
    </sensor>
  </gazebo>



</xacro:macro>
</robot>
