Skip to content

Commit

Permalink
add urdf and mesh files for d585
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun committed Jun 7, 2023
1 parent fb25b7e commit be9bb20
Show file tree
Hide file tree
Showing 3 changed files with 232 additions and 0 deletions.
Binary file added realsense2_description/meshes/d585.stl
View file
Binary file not shown.
221 changes: 221 additions & 0 deletions realsense2_description/urdf/_d585.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,221 @@
<?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 D585 camera, in its
aluminum peripherial evaluation case.
-->

<robot name="sensor_d585" 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_d585" 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="d585_cam_depth_to_infra1_offset" value="0.0"/>
<xacro:property name="d585_cam_depth_to_labeled_point_cloud_offset" value="0.0"/>
<xacro:property name="d585_cam_depth_to_infra2_offset" value="-0.095"/>
<xacro:property name="d585_cam_depth_to_color_offset" value="-0.0475"/>


<!-- The following values model the aluminum peripherial case for the
d585 camera, with the camera joint represented by the actual
peripherial camera tripod mount -->
<xacro:property name="d585_cam_width" value="0.182"/>
<xacro:property name="d585_cam_height" value="0.043"/>
<xacro:property name="d585_cam_depth" value="0.0754"/>
<xacro:property name="d585_cam_mount_from_center_offset" value="0.0377"/> <!-- TODO Samer: check -->
<!-- glass cover is 0.1 mm inwards from front aluminium plate -->
<xacro:property name="d585_glass_to_front" value="0.1e-3"/> <!-- TODO Samer: check -->
<!-- see datasheet Revision 009, Fig. 4-4 page 68 -->
<xacro:property name="d585_zero_depth_to_glass" value="4.55e-3"/> <!-- TODO Samer: check -->
<!-- convenience precomputation to avoid clutter-->
<xacro:property name="d585_mesh_x_offset" value="${d585_cam_mount_from_center_offset-d585_glass_to_front-d585_zero_depth_to_glass}"/>

<!-- The following offset is relative to the physical d585 camera peripherial
camera tripod mount -->
<xacro:property name="d585_cam_depth_px" value="${d585_cam_mount_from_center_offset}"/>
<xacro:property name="d585_cam_depth_py" value="0.0475"/>
<xacro:property name="d585_cam_depth_pz" value="${d585_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="${d585_mesh_x_offset} ${d585_cam_depth_py} ${d585_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="${d585_zero_depth_to_glass-d585_cam_depth/2} ${-d585_cam_depth_py} 0" rpy="0 0 0"/> -->
<origin xyz="${d585_zero_depth_to_glass + d585_glass_to_front} ${-d585_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
<geometry>
<!-- <box size="${d585_cam_depth} ${d585_cam_width} ${d585_cam_height}"/> -->
<mesh filename="package://realsense2_description/meshes/d585.stl" scale="0.001 0.001 0.001" />
</geometry>
<material name="aluminum"/>
</visual>
<collision>
<origin xyz="${d585_zero_depth_to_glass-d585_cam_depth/2} ${-d585_cam_depth_py} 0" rpy="0 0 0"/>
<geometry>
<box size="${d585_cam_depth} ${d585_cam_width} ${d585_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 ${d585_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 ${d585_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 ${d585_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"/>

<!-- TODO Samer: check -->
<joint name="${name}_labeled_point_cloud_joint" type="fixed">
<origin xyz="0 ${d585_cam_depth_to_labeled_point_cloud_offset} 0" rpy="0 0 0" />
<parent link="${name}_link" />
<child link="${name}_labeled_point_cloud_frame" />
</joint>
<link name="${name}_labeled_point_cloud_frame"/>


<!-- IMU -->
<!-- see datasheet Revision 009, page 114 -->
<xacro:property name="d585_imu_px" value="0.0195"/> <!-- TODO Samer: check -->
<xacro:property name="d585_imu_py" value="0.0"/> <!-- TODO Samer: check -->
<xacro:property name="d585_imu_pz" value="0.0114"/> <!-- TODO Samer: check -->

<link name="${name}_accel_frame" />
<link name="${name}_accel_optical_frame" />
<link name="${name}_gyro_frame" />
<link name="${name}_gyro_optical_frame" />
<link name="${name}_imu_optical_frame" />

<joint name="${name}_accel_joint" type="fixed">
<origin xyz = "${d585_imu_px} ${d585_imu_py} ${d585_imu_pz}" rpy = "0 0 0" />
<parent link="${name}_link" />
<child link="${name}_accel_frame" />
</joint>

<joint name="${name}_accel_optical_joint" type="fixed">
<origin xyz = "0 0 0" rpy = "${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="${name}_accel_frame" />
<child link="${name}_accel_optical_frame" />
</joint>

<joint name="${name}_gyro_joint" type="fixed">
<origin xyz = "${d585_imu_px} ${d585_imu_py} ${d585_imu_pz}" rpy = "0 0 0" />
<parent link="${name}_link" />
<child link="${name}_gyro_frame" />
</joint>

<joint name="${name}_gyro_optical_joint" type="fixed">
<origin xyz = "0 0 0" rpy = "${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="${name}_gyro_frame" />
<child link="${name}_gyro_optical_frame" />
</joint>

<joint name="${name}_imu_optical_joint" type="fixed">
<origin xyz = "0 0 0" rpy = "0 0 0" />
<parent link="${name}_gyro_optical_frame" />
<child link="${name}_imu_optical_frame" />
</joint>
</xacro:if>

<xacro:if value="$(arg add_plug)">
<xacro:usb_plug parent="${name}_link" name="${name}_usb_plug">
<origin xyz="${-0.00406-d585_cam_depth_px} ${-0.03701-d585_cam_depth_py} ${-d585_cam_depth_pz}" rpy="${M_PI/2} 0 0"/>
</xacro:usb_plug>
</xacro:if>
</xacro:macro>
</robot>

11 changes: 11 additions & 0 deletions realsense2_description/urdf/test_d585_camera.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?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/_d585.urdf.xacro" />

<link name="base_link" />
<xacro:sensor_d585 parent="base_link" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:sensor_d585>
</robot>

0 comments on commit be9bb20

Please sign in to comment.