<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm$(arg dof)">
    <!-- <robot xmlns:xacro="http://ros.org/wiki/xacro" name="lite6"> -->
  <xacro:arg name="prefix" default=""/>
  <xacro:arg name="hw_ns" default="xarm"/>
  <xacro:arg name="limited" default="false"/>
  <xacro:arg name="effort_control" default="false"/>
  <xacro:arg name="velocity_control" default="false"/>
  <xacro:arg name="add_gripper" default="false"/>
  <xacro:arg name="add_vacuum_gripper" default="false"/>
  <xacro:arg name="dof" default="6"/>
  <xacro:arg name="robot_type" default="lite"/>
  <xacro:arg name="ros2_control_plugin" default="uf_robot_hardware/UFRobotSystemHardware"/>
  <xacro:arg name="ros2_control_params" default="$(find xarm_controller)/config/xarm$(arg dof)_controllers.yaml"/>

  <xacro:arg name="add_other_geometry" default="false"/>
  <xacro:arg name="geometry_type" default="box"/>
  <xacro:arg name="geometry_mass" default="0.1"/>
  <xacro:arg name="geometry_height" default="0.1"/>
  <xacro:arg name="geometry_radius" default="0.1"/>
  <xacro:arg name="geometry_length" default="0.1"/>
  <xacro:arg name="geometry_width" default="0.1"/>
  <xacro:arg name="geometry_mesh_filename" default=""/>
  <xacro:arg name="geometry_mesh_origin_xyz" default="0 0 0"/>
  <xacro:arg name="geometry_mesh_origin_rpy" default="0 0 0"/>
  <xacro:arg name="geometry_mesh_tcp_xyz" default="0 0 0"/>
  <xacro:arg name="geometry_mesh_tcp_rpy" default="0 0 0"/>

  <xacro:arg name="robot_ip" default=""/>
  <xacro:arg name="report_type" default="normal"/>
  <xacro:arg name="baud_checkset" default="true"/>
  <xacro:arg name="default_gripper_baud" default="2000000"/>

  <!-- gazebo_plugin -->
  <xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
  <xacro:gazebo_ros_control_plugin ros2_control_params="$(arg ros2_control_params)"/>

  <!-- load xarm device -->
  <!-- Load Lite6 Robot Model URDF -->
  <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6_robot_macro.xacro" />
  <xacro:lite6_robot prefix="$(arg prefix)" namespace="xarm" limited="$(arg limited)"
                     effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
                     ros2_control_plugin="$(arg ros2_control_plugin)"
                     attach_to="world" xyz="0 0 0" rpy="0 0 0"
                     ros2_control_params="$(arg ros2_control_params)"
                     load_gazebo_plugin="false"
                     add_gripper="$(arg add_gripper)"
                     robot_ip="$(arg robot_ip)"
                     report_type="$(arg report_type)"
                     baud_checkset="$(arg baud_checkset)"
                     default_gripper_baud="$(arg default_gripper_baud)" />

  <joint name="eef_joint" type="fixed">
      <parent link="link_eef"/>
      <child link="pen_link"/>
  </joint>
  <link name="pen_link">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <cylinder radius="0.005" length="0.2"/>
        </geometry>
        <material name="Cyan">
            <color rgba="0.0 1.0 1.0 1.0"/>
        </material>
    </visual>

    <collision>
        <geometry>
            <box size="0 0 0"/>
        </geometry>
    </collision>
  </link>

  <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
  <gazebo>
    <plugin name="pen_position" filename="libgazebo_ros_p3d.so">
        <!-- <alwaysOn>true</alwaysOn> -->
      <ros>
        <remapping>odom:=pen_position</remapping>
      </ros>
      <frame_name>world</frame_name>
      <!-- <body_name>pen_link</body_name> -->
      <body_name>link6</body_name>
      <!--       topic name is always /odom -->
      <!-- <topic_name>pen_position</topic_name> -->
      <!--       Update rate in Hz -->
      <update_rate>10</update_rate>
      <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
      <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
    </plugin>
  </gazebo>

</robot>
<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
     <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
     <xacro:lite6_urdf prefix=''/> -->

<!--
     </robot> -->
