replaced Asus xtion with Intel realsense D435

vislab-tecnico-lisboa · Jan 23, 2019 · 56ea9e1 · 56ea9e1
1 parent 8614be9
commit 56ea9e1
Show file tree

Hide file tree

Showing 4 changed files with 149 additions and 91 deletions.
diff --git 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
@@ -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
diff --git 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>