<?xml version="1.0"?>

<!--
License: Apache 2.0. See LICENSE file in root directory.
Copyright(c) 2017 Intel Corporation. All Rights Reserved

This is the URDF model for the Intel RealSense 430 camera, in it's
aluminum peripherial evaluation case.
-->

<robot name="sensor_d435" xmlns:xacro="http://ros.org/wiki/xacro">
  <!--File includes-->
  <xacro:include filename="$(find mors_sim)/urdf/d435_gazebo.xacro"/>

  <xacro:macro name="sensor_d435" params="name:=camera topics_ns:=camera parent *origin publish_pointcloud:=true">
    <xacro:property name="M_PI" value="3.1415926535897931" />
  
    <!-- The following values are approximate, and the camera node
     publishing TF values with actual calibrated camera extrinsic values -->
    <xacro:property name="d435_cam_depth_to_left_ir_offset" value="0.0"/>
    <xacro:property name="d435_cam_depth_to_right_ir_offset" value="-0.050"/>
    <xacro:property name="d435_cam_depth_to_color_offset" value="0.015"/>
  
    <!-- The following values model the aluminum peripherial case for the
    D435 camera, with the camera joint represented by the actual 
    peripherial camera tripod mount -->
    <xacro:property name="d435_cam_width" value="0.090"/>
    <xacro:property name="d435_cam_height" value="0.025"/>
    <xacro:property name="d435_cam_depth" value="0.02505"/>
    <xacro:property name="d435_cam_mount_from_center_offset" value="0.0149"/>
  
    <!-- The following offset is relative the the physical D435 camera peripherial
    camera tripod mount -->
    <xacro:property name="d435_cam_depth_px" value="${d435_cam_mount_from_center_offset}"/>
    <xacro:property name="d435_cam_depth_py" value="0.0175"/>
    <xacro:property name="d435_cam_depth_pz" value="0"/>

    <material name="${name}_aluminum">
      <color rgba="0.5 0.5 0.5 1"/>
    </material>


    <!-- camera body, with origin at bottom screw mount -->
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_bottom_screw_frame" />
    </joint>
    <link name="${name}_bottom_screw_frame"/>

    <joint name="${name}_link_joint" type="fixed">
      <origin xyz="0 ${d435_cam_depth_py} ${d435_cam_depth_pz}" rpy="0 0 0"/>
      <parent link="${name}_bottom_screw_frame"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <visual>
      <origin xyz="${d435_cam_mount_from_center_offset} ${-d435_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
        <geometry>
          <!-- <box size="${d435_cam_width} ${d435_cam_height} ${d435_cam_depth}"/> -->
          <mesh filename="package://walkerbro_sim/urdf/meshes/d435.dae" scale="1 1 1"/>
          <!--<mesh filename="package://realsense2_description/meshes/d435/d435.dae" />-->
        </geometry>
        <material name="${name}_aluminum"/>
      </visual>
      <collision>
        <origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
        <geometry>
        <box size="${d435_cam_depth} ${d435_cam_width} ${d435_cam_height}"/>
        </geometry>
      </collision>
      <inertial>
        <!-- The following are not reliable values, and should not be used for modeling -->
        <mass value="0.564" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257" />
      </inertial>
    </link>
   
    <!-- camera depth joints and links -->
    <joint name="${name}_depth_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <parent link="${name}_link"/>
      <child link="${name}_depth_frame" />
    </joint>
    <link name="${name}_depth_frame"/>

    <joint name="${name}_depth_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="${name}_depth_frame" />
      <child link="${name}_depth_optical_frame" />
    </joint>
    <link name="${name}_depth_optical_frame"/>
      
    <!-- camera left IR joints and links -->
    <joint name="${name}_left_ir_joint" type="fixed">
      <origin xyz="0 ${d435_cam_depth_to_left_ir_offset} 0" rpy="0 0 0" />
      <parent link="${name}_depth_frame" />
      <child link="${name}_left_ir_frame" />
    </joint>
    <link name="${name}_left_ir_frame"/>

    <joint name="${name}_left_ir_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="${name}_left_ir_frame" />
      <child link="${name}_left_ir_optical_frame" />
    </joint>
    <link name="${name}_left_ir_optical_frame"/>

    <!-- camera right IR joints and links -->
    <joint name="${name}_right_ir_joint" type="fixed">
      <origin xyz="0 ${d435_cam_depth_to_right_ir_offset} 0" rpy="0 0 0" />
      <parent link="${name}_depth_frame" />
      <child link="${name}_right_ir_frame" />
    </joint>
    <link name="${name}_right_ir_frame"/>

    <joint name="${name}_right_ir_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="${name}_right_ir_frame" />
      <child link="${name}_right_ir_optical_frame" />
    </joint>
    <link name="${name}_right_ir_optical_frame"/>

    <!-- camera color joints and links -->
    <joint name="${name}_color_joint" type="fixed">
      <origin xyz="0 ${d435_cam_depth_to_color_offset} 0" rpy="0 0 0" />
      <parent link="${name}_depth_frame" />
      <child link="${name}_color_frame" />
    </joint>
    <link name="${name}_color_frame"/>

    <joint name="${name}_color_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="${name}_color_frame" />
      <child link="${name}_color_optical_frame" />
    </joint>
    <link name="${name}_color_optical_frame"/>

    <!-- Realsense Gazebo Plugin -->
    <xacro:gazebo_d435 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared1_optical_frame="${name}_left_ir_optical_frame" infrared2_optical_frame="${name}_right_ir_optical_frame" publish_pointcloud="${publish_pointcloud}"/>

  </xacro:macro>
</robot>