From 56ea9e1738dc0274af3a332e027cc4ad5e337785 Mon Sep 17 00:00:00 2001
From: Rui Figueiredo <ruihortafigueiredo@gmail.com>
Date: Wed, 23 Jan 2019 15:47:45 +0000
Subject: [PATCH] replaced Asus xtion with Intel realsense D435

---
 vizzy_description/robots/vizzy.urdf.xacro     |  11 +-
 .../urdf/sensors/_d435.urdf.xacro             | 139 ++++++++++++++++++
 .../urdf/sensors/asus_camera.urdf.xacro       |  85 -----------
 vizzy_robot/launch/real_robot.launch          |   5 +
 4 files changed, 149 insertions(+), 91 deletions(-)
 create mode 100644 vizzy_description/urdf/sensors/_d435.urdf.xacro
 delete mode 100644 vizzy_description/urdf/sensors/asus_camera.urdf.xacro

diff --git a/vizzy_description/robots/vizzy.urdf.xacro b/vizzy_description/robots/vizzy.urdf.xacro
index 5c14607f..5cccf69c 100644
--- a/vizzy_description/robots/vizzy.urdf.xacro
+++ b/vizzy_description/robots/vizzy.urdf.xacro
@@ -22,8 +22,8 @@
 
   <!-- Hokuyo URG-04LX laser scanner -->
   <xacro:include filename="$(find vizzy_description)/urdf/sensors/hokuyo_04lx_laser.urdf.xacro"/>
-  <!-- TODO: Update Asus Xiton Live Pro -->
-  <xacro:include filename="$(find vizzy_description)/urdf/sensors/asus_camera.urdf.xacro"/>
+  <!-- RealSense D435 -->
+  <xacro:include filename="$(find vizzy_description)/urdf/sensors/_d435.urdf.xacro"/>
 
   <!-- Robot body -->
 
@@ -96,10 +96,9 @@
 
   <!-- Instantiate depth camera -->
   <xacro:unless value="$(arg disable_3d_sensor)">
-    <xacro:sensor_asus_xtion_pro name="asus_camera" parent="waist_link"
-      ros_topic="asus_camera">
-      <origin xyz="-0.08 0.16 0.02" rpy="${-M_PI/2} ${M_PI} ${-7*(M_PI/180)}"/>
-    </xacro:sensor_asus_xtion_pro>
+    <xacro:sensor_d435 name="realsense" parent="waist_link">
+      <origin xyz="-0.09516 0.143 0.0" rpy="${-M_PI/2} ${M_PI} 0"/> <!-- In the drawing z=0.158!!!-->
+    </xacro:sensor_d435>
   </xacro:unless>
 
   <!-- Instantiate plugins common to ROS and Yarp -->
diff --git a/vizzy_description/urdf/sensors/_d435.urdf.xacro b/vizzy_description/urdf/sensors/_d435.urdf.xacro
new file mode 100644
index 00000000..953341f0
--- /dev/null
+++ b/vizzy_description/urdf/sensors/_d435.urdf.xacro
@@ -0,0 +1,139 @@
+<?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">
+  <xacro:macro name="sensor_d435" params="name parent *origin">
+    <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="${d435_cam_height/2}"/>
+
+    <material 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://realsense2_camera/meshes/d435.dae" />
+          <!--<mesh filename="package://realsense2_camera/meshes/d435/d435.dae" />-->
+
+        </geometry>
+        <material 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"/>
+  </xacro:macro>
+</robot>
diff --git a/vizzy_description/urdf/sensors/asus_camera.urdf.xacro b/vizzy_description/urdf/sensors/asus_camera.urdf.xacro
deleted file mode 100644
index dc62a0a8..00000000
--- a/vizzy_description/urdf/sensors/asus_camera.urdf.xacro
+++ /dev/null
@@ -1,85 +0,0 @@
-<?xml version="1.0"?>
-<robot name="sensor_asus_xtion_pro" xmlns:xacro="http://ros.org/wiki/xacro">
-  
-  <xacro:include filename="$(find vizzy_description)/urdf/sensors/asus_camera.gazebo.xacro"/>
-
-  <!-- Xacro properties -->
-  <xacro:property name="M_SCALE" value="0.001"/>
-
-  <xacro:macro name="sensor_asus_xtion_pro"
-    params="name parent *origin ros_topic">
-
-    <joint name="${name}_rgb_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="${name}_rgb_frame"/>
-    </joint>
-    <link name="${name}_rgb_frame"/>
-
-    <joint name="${name}_rgb_optical_joint" type="fixed">
-      <origin
-        xyz="0 0 0"
-        rpy="${-M_PI/2} 0 ${-M_PI/2}"/>
-      <parent link="${name}_rgb_frame"/>
-      <child link="${name}_rgb_optical_frame"/>
-    </joint>
-    <link name="${name}_rgb_optical_frame"/>
-
-    <joint name="${name}_joint" type="fixed">
-      <origin
-        xyz="0 0 0" 
-        rpy="0 0 0"/>
-      <parent link="${name}_rgb_frame"/>
-      <child link="${name}_link"/>
-    </joint>
-    <link name="${name}_link">
-      <visual>
-        <origin
-          xyz="-0.015 0.0035 0.004"
-          rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://vizzy_description/meshes/asus_xtion_pro/xtion_pro_camera.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin
-          xyz="0 0 0"
-          rpy="0 0 0"/>
-        <geometry>
-        <box size="0.0730 0.2760 0.0720"/>
-      </geometry>
-      </collision>
-      <inertial>
-        <mass value="0.170"/>
-        <origin xyz="0 0 0"/>
-        <inertia
-          ixx="0.001152600" ixy="0.0" ixz="0.0"
-          iyy="0.000148934" iyz="0.0"
-          izz="0.001154654"/>
-      </inertial>
-    </link>
-
-    <joint name="${name}_depth_joint" type="fixed">
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0"/>
-      <parent link="${name}_rgb_frame"/>
-      <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"/>
-
-    <!-- Gazebo extension -->
-    <xacro:vizzy_sim_3dsensor name="${name}" ros_topic="${ros_topic}"/>
-
-  </xacro:macro>
-
-</robot>
\ No newline at end of file
diff --git a/vizzy_robot/launch/real_robot.launch b/vizzy_robot/launch/real_robot.launch
index c38a47e6..1adfcf5a 100644
--- a/vizzy_robot/launch/real_robot.launch
+++ b/vizzy_robot/launch/real_robot.launch
@@ -76,5 +76,10 @@
         <arg name="frame_rate" value="10"/>
         <arg name="camera_intrinsics_file" value="$(find vizzy_robot)/config/right_camera_params.yaml" />
       </include> 
+
+      <!-- Realsense -->
+      <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
+        <arg name="camera" value="realsense" />
+      </include> 
   </group>
 </launch>