<?xml version="1.0"?>
<!--

  Copyright (c) 2021 PAL Robotics S.L. All rights reserved.
  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at
      http://www.apache.org/licenses/LICENSE-2.0
  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

<robot name="tiago" xmlns:xacro="http://ros.org/wiki/xacro">
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <ns></ns>
      <robotSimType>pal_hardware_gazebo/PalHardwareGazebo</robotSimType>
      <xacro:if value="${is_multiple}">
          <robotNamespace>/${nsp}</robotNamespace>
      </xacro:if>
      <xacro:unless value="${is_multiple}">
          <robotNamespace></robotNamespace>
      </xacro:unless>
      <controlPeriod>0.001</controlPeriod>
    </plugin>

    <xacro:if value="${base_type == 'omni_base'}">
      <plugin filename="libgazebo_ros_force_based_move.so" name="gazebo_force_based_move" >
        <commandTopic>mobile_base_controller/cmd_vel</commandTopic>
        <odometryTopic>mobile_base_controller/odom</odometryTopic>
        <odometryFrame>odom</odometryFrame>
        <odometryRate>50.0</odometryRate>
        <robotBaseFrame>base_footprint</robotBaseFrame>
        <publishOdometryTf>true</publishOdometryTf>
      </plugin>
    </xacro:if>

    <plugin filename="libgazebo_ros_p3d.so" name="gazebo_ros_p3d">
      <bodyName>base_footprint</bodyName>
      <topicName>ground_truth_odom</topicName>
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <xacro:if value="${is_multiple}">
          <robotNamespace>/${nsp}</robotNamespace>
      </xacro:if>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_odometry">
      <frameName>base_footprint</frameName>
      <topicName>floating_base_pose_simulated</topicName>
      <xacro:if value="${is_multiple}">
          <robotNamespace>/${nsp}</robotNamespace>
      </xacro:if>
    </plugin>
  </gazebo>


  <gazebo reference="base_imu_link">
    <!-- this is expected to be reparented to pelvis with appropriate offset
         when imu_link is reduced by fixed joint reduction -->
    <!-- todo: this is working with gazebo 1.4, need to write a unit test -->
    <sensor name="imu_sensor" type="imu">
      <always_on>1</always_on>
      <update_rate>1000.0</update_rate>
      <imu>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters from Boston Dynamics
               (http://gazebosim.org/wiki/Sensor_noise):
                 rates (rad/s): mean=0, stddev=2e-4
                 accels (m/s/s): mean=0, stddev=1.7e-2
                 rate bias (rad/s): 5e-6 - 1e-5
                 accel bias (m/s/s): 1e-1
               Experimentally, simulation provide rates with noise of
               about 1e-3 rad/s and accels with noise of about 1e-1 m/s/s.
               So we don't expect to see the noise unless number of inner iterations
               are increased.

               We will add bias.  In this model, bias is sampled once for rates
               and once for accels at startup; the sign (negative or positive)
               of each bias is then switched with equal probability.  Thereafter,
               the biases are fixed additive offsets.  We choose
               bias means and stddevs to produce biases close to the provided
               data. -->
          <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>
    </sensor>
  </gazebo>

  <!-- define global properties -->
  <xacro:property name="M_PI" value="3.1415926535897931" />

</robot>
