<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="rbs_arm" params="namespace parent *origin tf_prefix hardware gripper_name controllers">
    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_base_link.xacro"></xacro:include>
    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_ee_link.xacro"></xacro:include>
    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_fork_link.xacro"></xacro:include>
    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_main_link.xacro"></xacro:include>
    <xacro:include filename="$(find rbs_arm)/urdf/sensors.xacro"></xacro:include>
    <!-- BEGIN robot description  -->
    <!-- link 0-->
    <xacro:base_link hardware="${hardware}" parent="${parent}" tf_prefix="${tf_prefix}">
      <xacro:insert_block name="origin"></xacro:insert_block>
    </xacro:base_link>

    <!-- link 1-->
    <xacro:fork_link d="0.2" hardware="${hardware}" name="fork0_link" p="10000" parent="base_link" tf_prefix="${tf_prefix}" initial_joint_position="1.57"></xacro:fork_link>
    <!-- link 2-->
    <xacro:main_link d="0.2" hardware="${hardware}" name="main0_link" p="10000" parent="fork0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.5"></xacro:main_link>
    <!-- link 3-->
    <xacro:fork_link d="0.2" hardware="${hardware}" name="fork1_link" p="10000" parent="main0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.0"></xacro:fork_link>
    <!-- link 4-->
    <xacro:main_link d="0.2" hardware="${hardware}" name="main1_link" p="1000" parent="fork1_link" tf_prefix="${tf_prefix}" initial_joint_position="1.5"></xacro:main_link>
    <!-- ee link also contain tool0-->
    <!-- link 5-->
    <xacro:ee_link d="0.2" hardware="${hardware}" p="20" parent="main1_link" tf_prefix="${tf_prefix}"></xacro:ee_link>
    <!-- END robot description -->
    <xacro:rgbd parent="${tf_prefix}ee_link" tf_prefix="${tf_prefix}">
      <origin rpy="0.0 -1.57 0.0" xyz="0.0 0.0 0.0754"></origin>
    </xacro:rgbd>

    <!-- Insert gripper in robot description if exist -->
    <!-- <xacro:if value="${gripper_name=='rbs_gripper'}"> -->
    <!--   <xacro:include filename="$(find rbs_gripper)/urdf/rbs_gripper_macro.xacro"></xacro:include> -->
    <!--   <xacro:rbs_gripper gripper_name="rbs_gripper" hardware="${hardware}"
    parent="${tf_prefix}tool0" tf_prefix="${tf_prefix}"> -->
    <!--     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"></origin> -->
    <!--   </xacro:rbs_gripper> -->
    <!-- </xacro:if> -->

    <!-- If hardware == gazebo so insert additional parameters for it -->
    <xacro:if value="${hardware=='gazebo'}">
      <!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
      <gazebo>
        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
          <parameters>$(arg simulation_controllers)</parameters>
          <ros>
            <namespace>${namespace}</namespace>
          </ros>
        </plugin>
      </gazebo>
    </xacro:if>
  </xacro:macro>
</robot>
