<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rack">
  <xacro:macro name="rgbd" params="*origin parent tf_prefix">
    <link name="${tf_prefix}rgbd_camera">
      <inertial>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        <mass value="0.1" />
        <inertia ixx="0.000166667" ixy="0.0" ixz="0.0" iyy="0.000166667" iyz="0.0" izz="0.000166667" />
      </inertial>
      <visual name="">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>
        <material name="">
          <color rgba="0.0 0.0 0.0 1.0" />
          <texture filename="" />
        </material>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>
      </collision>
    </link>

    <gazebo reference="${tf_prefix}rgbd_camera">
      <sensor name="${tf_prefix}rgbd_camera" type="rgbd_camera">
        <camera>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>1920</width>
            <height>1080</height>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
        </camera>
        <always_on>1</always_on>
        <update_rate>30</update_rate>
        <visualize>true</visualize>
        <topic>${tf_prefix}rgbd_camera</topic>
        <enable_metrics>true</enable_metrics>
      </sensor>
    </gazebo>

    <joint name="${tf_prefix}rgbd_camera_to_parent" type="fixed">
      <parent link="${parent}" />
      <child link="${tf_prefix}rgbd_camera" />
      <xacro:insert_block name="origin" />
    </joint>

    <gazebo reference="${tf_prefix}rgbd_camera_to_parent">
      <preserveFixedJoint>1</preserveFixedJoint>
    </gazebo>
  </xacro:macro>

  <xacro:macro name="fts" params="name tf_prefix link">

    <ros2_control name="${tf_prefix}${name}" type="sensor">
      <hardware>
        <plugin>gz_ros2_control/GzFts</plugin>
      </hardware>

      <sensor name="${tf_prefix}${name}">
        <state_interface name="force.x" />
        <state_interface name="force.y" />
        <state_interface name="force.z" />
        <state_interface name="torque.x" />
        <state_interface name="torque.y" />
        <state_interface name="torque.z" />
      </sensor>
    </ros2_control>

    <gazebo reference="${tf_prefix}${link}_joint">
      <preserveFixedJoint>true</preserveFixedJoint>
      <sensor name="${tf_prefix}${name}" type="force_torque">
        <always_on>true</always_on>
        <update_rate>50</update_rate>
        <visualize>true</visualize>
        <topic>${tf_prefix}ft_data</topic>
        <force_torque>
          <frame>sensor</frame>
          <measure_direction>child_to_parent</measure_direction>
        </force_torque>
      </sensor>
    </gazebo>
  </xacro:macro>

</robot>
