You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
145 lines
6.0 KiB
145 lines
6.0 KiB
<?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">
|
|
<!--File includes-->
|
|
<xacro:include filename="$(find mors_sim)/urdf/d435_gazebo.xacro"/>
|
|
|
|
<xacro:macro name="sensor_d435" params="name:=camera topics_ns:=camera parent *origin publish_pointcloud:=true">
|
|
<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="0"/>
|
|
|
|
<material name="${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://walkerbro_sim/urdf/meshes/d435.dae" scale="1 1 1"/>
|
|
<!--<mesh filename="package://realsense2_description/meshes/d435/d435.dae" />-->
|
|
</geometry>
|
|
<material name="${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"/>
|
|
|
|
<!-- Realsense Gazebo Plugin -->
|
|
<xacro:gazebo_d435 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared1_optical_frame="${name}_left_ir_optical_frame" infrared2_optical_frame="${name}_right_ir_optical_frame" publish_pointcloud="${publish_pointcloud}"/>
|
|
|
|
</xacro:macro>
|
|
</robot> |