<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find gz_quadruped_hardware)/xacro/foot_force_sensor.xacro"/>

  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
    </hardware>

    <joint name="FR_HipX">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FR_HipY">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FR_Knee">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FL_HipX">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FL_HipY">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FL_Knee">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="HR_HipX">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="HR_HipY">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="HR_Knee">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="HL_HipX">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="HL_HipY">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="HL_Knee">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <command_interface name="kp"/>
      <command_interface name="kd"/>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <sensor name="imu_sensor">
      <state_interface name="orientation.x"/>
      <state_interface name="orientation.y"/>
      <state_interface name="orientation.z"/>
      <state_interface name="orientation.w"/>
      <state_interface name="angular_velocity.x"/>
      <state_interface name="angular_velocity.y"/>
      <state_interface name="angular_velocity.z"/>
      <state_interface name="linear_acceleration.x"/>
      <state_interface name="linear_acceleration.y"/>
      <state_interface name="linear_acceleration.z"/>
    </sensor>

    <sensor name="foot_force">
      <state_interface name="FR_foot_force"/>
      <state_interface name="FL_foot_force"/>
      <state_interface name="HR_foot_force"/>
      <state_interface name="HL_foot_force"/>
    </sensor>

  </ros2_control>

  <gazebo>
    <plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
      <parameters>$(find lite3_description)/config/gazebo.yaml</parameters>
    </plugin>
    <plugin filename="gz-sim-imu-system"
            name="gz::sim::systems::Imu">
    </plugin>
    <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
    <!--    <plugin filename="gz-sim-odometry-publisher-system"-->
    <!--            name="gz::sim::systems::OdometryPublisher">-->
    <!--      <odom_frame>odom</odom_frame>-->
    <!--      <robot_base_frame>base</robot_base_frame>-->
    <!--      <odom_publish_frequency>1000</odom_publish_frequency>-->
    <!--      <odom_topic>odom</odom_topic>-->
    <!--      <dimensions>3</dimensions>-->
    <!--      <odom_covariance_topic>odom_with_covariance</odom_covariance_topic>-->
    <!--      <tf_topic>tf</tf_topic>-->
    <!--    </plugin>-->
  </gazebo>

  <gazebo reference="trunk">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
  </gazebo>

  <gazebo reference="imu_link">
    <sensor name="imu_sensor" type="imu">
      <always_on>1</always_on>
      <update_rate>500</update_rate>
      <visualize>true</visualize>
      <topic>imu</topic>
      <imu>
        <angular_velocity>
          <x>
            <noise type="gaussian">
              <mean>0.0</mean>
              <stddev>2e-4</stddev>
              <bias_mean>0.0000075</bias_mean>
              <bias_stddev>0.0000008</bias_stddev>
            </noise>
          </x>
          <y>
            <noise type="gaussian">
              <mean>0.0</mean>
              <stddev>2e-4</stddev>
              <bias_mean>0.0000075</bias_mean>
              <bias_stddev>0.0000008</bias_stddev>
            </noise>
          </y>
          <z>
            <noise type="gaussian">
              <mean>0.0</mean>
              <stddev>2e-4</stddev>
              <bias_mean>0.0000075</bias_mean>
              <bias_stddev>0.0000008</bias_stddev>
            </noise>
          </z>
        </angular_velocity>
        <linear_acceleration>
          <x>
            <noise type="gaussian">
              <mean>0.0</mean>
              <stddev>1.7e-2</stddev>
              <bias_mean>0.1</bias_mean>
              <bias_stddev>0.001</bias_stddev>
            </noise>
          </x>
          <y>
            <noise type="gaussian">
              <mean>0.0</mean>
              <stddev>1.7e-2</stddev>
              <bias_mean>0.1</bias_mean>
              <bias_stddev>0.001</bias_stddev>
            </noise>
          </y>
          <z>
            <noise type="gaussian">
              <mean>0.0</mean>
              <stddev>1.7e-2</stddev>
              <bias_mean>0.1</bias_mean>
              <bias_stddev>0.001</bias_stddev>
            </noise>
          </z>
        </linear_acceleration>
      </imu>
    </sensor>
  </gazebo>

  <gazebo reference="imu_joint">
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>

  <xacro:foot_force_sensor name="FL" suffix="Ankle"/>
  <xacro:foot_force_sensor name="HL" suffix="Ankle"/>
  <xacro:foot_force_sensor name="FR" suffix="Ankle"/>
  <xacro:foot_force_sensor name="HR" suffix="Ankle"/>

  <xacro:arg name="EXTERNAL_SENSORS" default="false"/>
  <xacro:if value="$(arg EXTERNAL_SENSORS)">
    <xacro:include filename="$(find gz_quadruped_playground)/models/D435/model.xacro"/>
    <xacro:include filename="$(find gz_quadruped_playground)/models/Lidar3DV1/model.xacro"/>

    <xacro:d435 camID="1" name="d435">
      <origin rpy="0 0.1 0" xyz="0.23 0 0.06"/>
    </xacro:d435>

    <xacro:Lidar3D vertical="16" name="lslidar">
      <origin rpy="0 0 0" xyz="0.163 0 0.058"/>
    </xacro:Lidar3D>

  </xacro:if>

</robot>
