Skip to content

Commit

Permalink
Added urdf & mesh files for D405 model
Browse files Browse the repository at this point in the history
  • Loading branch information
@Arun-Prasad-V
Arun-Prasad-V committed Dec 22, 2023
1 parent 9d0a77c commit 0ab7e4f
Show file tree
Hide file tree
Showing 4 changed files with 267 additions and 0 deletions.
92 changes: 92 additions & 0 deletions realsense2_camera/examples/pointcloud/rs_d405_pointcloud_launch.py
View file
Original file line number Diff line number Diff line change
@@ -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]
)
])
Binary file added realsense2_description/meshes/d405.stl
View file
Binary file not shown.
165 changes: 165 additions & 0 deletions realsense2_description/urdf/_d405.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -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>
10 changes: 10 additions & 0 deletions realsense2_description/urdf/test_d405_camera.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -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>

0 comments on commit 0ab7e4f

Please sign in to comment.