<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro name="d435i_urdf" params="prefix:=''">
    <xacro:property name="M_PI" value="3.1415926535897931" />
    <link name="${prefix}link_eef">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="file:///$(find xarm_description)/meshes/camera/realsense/visual/d435_with_cam_stand.STL"/>
        </geometry>
        <material name="${prefix}Silver" />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="file:///$(find xarm_description)/meshes/camera/realsense/collision/d435_with_cam_stand.STL"/>
        </geometry>
      </collision>
    </link>

    <link name="${prefix}camera_link"></link>

    <link name="${prefix}camera_depth_frame"></link>

    <link name="${prefix}camera_depth_optical_frame"></link>

    <link name="${prefix}camera_color_frame"></link>

    <link name="${prefix}camera_color_optical_frame"></link>

    <link name="${prefix}camera_left_ir_frame"></link>

    <link name="${prefix}camera_left_ir_optical_frame"></link>

    <link name="${prefix}camera_right_ir_frame"></link>

    <link name="${prefix}camera_right_ir_optical_frame"></link>

    <joint name="${prefix}camera_link_joint" type="fixed">
      <parent link="${prefix}link_eef" />
      <child link="${prefix}camera_link" />
      <!-- <origin xyz="0.067985 0 0.02725" rpy="0 ${-M_PI/2} 0" /> -->
      <origin xyz="0.06746 0 0.0205" rpy="0 ${-M_PI/2} 0" />
    </joint>

    <joint name="${prefix}camera_depth_joint" type="fixed">
      <parent link="${prefix}camera_link" />
      <child link="${prefix}camera_depth_frame" />
      <origin xyz="0 0 0" rpy="0 0 0" />
    </joint>

    <joint name="${prefix}camera_depth_optical_joint" type="fixed">
      <parent link="${prefix}camera_depth_frame" />
      <child link="${prefix}camera_depth_optical_frame" />
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    </joint>

    <joint name="${prefix}camera_color_joint" type="fixed">
      <parent link="${prefix}camera_link" />
      <child link="${prefix}camera_color_frame" />
      <origin xyz="0 0.015 0" rpy="0 0 0" />
    </joint>

    <joint name="${prefix}camera_color_optical_joint" type="fixed">
      <parent link="${prefix}camera_color_frame" />
      <child link="${prefix}camera_color_optical_frame" />
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    </joint>

    <joint name="${prefix}camera_left_ir_joint" type="fixed">
      <parent link="${prefix}camera_link" />
      <child link="${prefix}camera_left_ir_frame" />
      <origin xyz="0 0 0" rpy="0 0 0" />
    </joint>

    <joint name="${prefix}camera_left_ir_optical_joint" type="fixed">
      <parent link="${prefix}camera_left_ir_frame" />
      <child link="${prefix}camera_left_ir_optical_frame" />
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    </joint>

    <joint name="${prefix}camera_right_ir_joint" type="fixed">
      <parent link="${prefix}camera_link" />
      <child link="${prefix}camera_right_ir_frame" />
      <origin xyz="0 -0.050 0" rpy="0 0 0" />
    </joint>

    <joint name="${prefix}camera_right_ir_optical_joint" type="fixed">
      <parent link="${prefix}camera_right_ir_frame" />
      <child link="${prefix}camera_right_ir_optical_frame" />
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    </joint>
  
  </xacro:macro>
</robot>