PR #2953 from Arun-Prasad-V: Added urdf & mesh files for D405 model

realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py

@@ -0,0 +1,92 @@
+# Copyright 2023 Intel Corporation. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# DESCRIPTION #
+# ----------- #
+# Use this launch file to launch a device and visualize pointcloud along with the camera model D405.
+# The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters
+# command line example:
+# ros2 launch realsense2_camera rs_d405_pointcloud_launch.py
+
+"""Launch realsense2_camera node."""
+from launch import LaunchDescription
+import launch_ros.actions
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+import os
+import sys
+import pathlib
+import xacro
+import tempfile
+from ament_index_python.packages import get_package_share_directory
+sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
+sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch'))
+import rs_launch
+
+local_parameters = [{'name': 'camera_name',                  'default': 'camera', 'description': 'camera unique name'},
+                    {'name': 'camera_namespace',             'default': 'camera', 'description': 'camera namespace'},
+                    {'name': 'device_type',                  'default': "d405", 'description': 'choose device by type'},
+                    {'name': 'enable_color',                 'default': 'true', 'description': 'enable color stream'},
+                    {'name': 'enable_depth',                 'default': 'true', 'description': 'enable depth stream'},
+                    {'name': 'pointcloud.enable',            'default': 'true', 'description': 'enable pointcloud'},
+                   ]
+
+def to_urdf(xacro_path, parameters=None):
+    """Convert the given xacro file to URDF file.
+    * xacro_path -- the path to the xacro file
+    * parameters -- to be used when xacro file is parsed.
+    """
+    urdf_path = tempfile.mktemp(prefix="%s_" % os.path.basename(xacro_path))
+
+    # open and process file
+    doc = xacro.process_file(xacro_path, mappings=parameters)
+    # open the output file
+    out = xacro.open_output(urdf_path)
+    out.write(doc.toprettyxml(indent='  '))
+
+    return urdf_path
+
+def set_configurable_parameters(local_params):
+    return dict([(param['name'], LaunchConfiguration(param['name'])) for param in local_params])
+
+
+def generate_launch_description():
+    params = rs_launch.configurable_parameters
+    xacro_path = os.path.join(get_package_share_directory('realsense2_description'), 'urdf', 'test_d405_camera.urdf.xacro')
+    urdf = to_urdf(xacro_path, {'use_nominal_extrinsics': 'true', 'add_plug': 'true'})
+    return LaunchDescription(
+        rs_launch.declare_configurable_parameters(local_parameters) +
+        rs_launch.declare_configurable_parameters(params) + 
+        [
+        OpaqueFunction(function=rs_launch.launch_setup,
+                kwargs = {'params' : set_configurable_parameters(params)}
+        ),
+        launch_ros.actions.Node(
+            package='rviz2',
+            namespace='',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', [ThisLaunchFileDir(), '/rviz/urdf_pointcloud.rviz']],
+            output='screen',
+            parameters=[{'use_sim_time': False}]
+        ),
+        launch_ros.actions.Node(
+            name='model_node',
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            namespace='',
+            output='screen',
+            arguments=[urdf]
+        )
+    ])

realsense2_description/urdf/_d405.urdf.xacro

@@ -0,0 +1,165 @@
+<?xml version="1.0"?>
+
+<!--
+# Copyright 2023 Intel Corporation. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+-->
+
+<!--
+This is the URDF model for the Intel RealSense 405 camera, in its
+aluminum peripherial evaluation case.
+-->
+
+<robot name="sensor_d405" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- Includes -->
+  <xacro:include filename="$(find realsense2_description)/urdf/_materials.urdf.xacro" />
+  <xacro:include filename="$(find realsense2_description)/urdf/_usb_plug.urdf.xacro" />
+
+  <xacro:macro name="sensor_d405" params="parent *origin name:=camera use_nominal_extrinsics:=false">
+    <xacro:arg name="add_plug" default="false" />
+    <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="d405_cam_depth_to_infra1_offset" value="0.0"/>
+    <xacro:property name="d405_cam_depth_to_infra2_offset" value="-0.018"/>
+    <xacro:property name="d405_cam_depth_to_color_offset" value="0.0"/>
+
+    <!-- The following values model the aluminum peripherial case for the
+  	d405 camera, with the camera joint represented by the actual
+  	peripherial camera tripod mount -->
+    <xacro:property name="d405_cam_width" value="0.042"/>
+    <xacro:property name="d405_cam_height" value="0.042"/>
+    <xacro:property name="d405_cam_depth" value="0.023"/>
+    <xacro:property name="d405_cam_mount_from_center_offset" value="0.01465"/>
+    <!-- glass cover is 0.1 mm inwards from front aluminium plate -->
+    <xacro:property name="d405_glass_to_front" value="0.1e-3"/>
+    <!-- see datasheet Revision 017, Table. 4-16 page 97 -->
+    <xacro:property name="d405_zero_depth_to_glass" value="3.7e-3"/>
+    <!-- convenience precomputation to avoid clutter-->
+    <xacro:property name="d405_mesh_x_offset" value="${d405_cam_mount_from_center_offset-d405_glass_to_front-d405_zero_depth_to_glass}"/>
+
+    <!-- The following offset is relative to the physical d405 camera peripherial
+  	camera tripod mount -->
+    <xacro:property name="d405_cam_depth_px" value="${d405_cam_mount_from_center_offset}"/>
+    <xacro:property name="d405_cam_depth_py" value="0.009"/>
+    <xacro:property name="d405_cam_depth_pz" value="${d405_cam_height/2}"/>
+
+    <!-- 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="${d405_mesh_x_offset} ${d405_cam_depth_py} ${d405_cam_depth_pz}" rpy="0 0 0"/>
+      <parent link="${name}_bottom_screw_frame"/>
+      <child link="${name}_link" />
+    </joint>
+
+    <link name="${name}_link">
+      <visual>
+        <!-- the mesh origin is at front plate in between the two infrared camera axes -->
+        <origin xyz="${d405_zero_depth_to_glass + d405_glass_to_front} ${-d405_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
+        <geometry>
+	        <mesh filename="file://$(find realsense2_description)/meshes/d405.stl" scale="0.001 0.001 0.001" />
+        </geometry>
+        <material name="aluminum"/>
+      </visual>
+      <collision>
+        <origin xyz="${d405_zero_depth_to_glass-d405_cam_depth/2} ${-d405_cam_depth_py} 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="${d405_cam_depth} ${d405_cam_width} ${d405_cam_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <!-- The following are not reliable values, and should not be used for modeling -->
+        <mass value="0.072" />
+        <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>
+
+    <!-- Use the nominal extrinsics between camera frames if the calibrated extrinsics aren't being published. e.g. running the device in simulation  -->
+    <xacro:if value="${use_nominal_extrinsics}">
+      <!-- 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}_infra1_joint" type="fixed">
+        <origin xyz="0 ${d405_cam_depth_to_infra1_offset} 0" rpy="0 0 0" />
+        <parent link="${name}_link" />
+        <child link="${name}_infra1_frame" />
+      </joint>
+      <link name="${name}_infra1_frame"/>
+
+      <joint name="${name}_infra1_optical_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+        <parent link="${name}_infra1_frame" />
+        <child link="${name}_infra1_optical_frame" />
+      </joint>
+      <link name="${name}_infra1_optical_frame"/>
+
+      <!-- camera right IR joints and links -->
+      <joint name="${name}_infra2_joint" type="fixed">
+        <origin xyz="0 ${d405_cam_depth_to_infra2_offset} 0" rpy="0 0 0" />
+        <parent link="${name}_link" />
+        <child link="${name}_infra2_frame" />
+      </joint>
+      <link name="${name}_infra2_frame"/>
+
+      <joint name="${name}_infra2_optical_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+        <parent link="${name}_infra2_frame" />
+        <child link="${name}_infra2_optical_frame" />
+      </joint>
+      <link name="${name}_infra2_optical_frame"/>
+
+      <!-- camera color joints and links -->
+      <joint name="${name}_color_joint" type="fixed">
+        <origin xyz="0 ${d405_cam_depth_to_color_offset} 0" rpy="0 0 0" />
+        <parent link="${name}_link" />
+        <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:if>
+
+    <xacro:if value="$(arg add_plug)">
+      <xacro:usb_plug parent="${name}_link" name="${name}_usb_plug">
+        <origin xyz="${-0.00153-d405_mesh_x_offset} ${d405_cam_width/2-d405_cam_depth_py} 0" rpy="${M_PI} 0 0"/>
+      </xacro:usb_plug>
+    </xacro:if>
+  </xacro:macro>
+</robot>

realsense2_description/urdf/test_d405_camera.urdf.xacro

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<robot name="realsense2_camera" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:arg name="use_nominal_extrinsics" default="false"/>
+  <xacro:include filename="$(find realsense2_description)/urdf/_d405.urdf.xacro" />
+
+  <link name="base_link" />
+  <xacro:sensor_d405 parent="base_link" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </xacro:sensor_d405>
+</robot>