mirror of
https://github.com/bjoernellens1/cps_rmp220_support.git
synced 2024-11-27 17:35:46 +00:00
78 lines
3.3 KiB
XML
78 lines
3.3 KiB
XML
<?xml version="1.0"?>
|
|
<robot name="base"
|
|
xmlns:xacro="http://ros.org/wiki/xacro">
|
|
|
|
<xacro:macro name="base" params="camera_name camera_model parent base_frame
|
|
cam_pos_x cam_pos_y cam_pos_z
|
|
cam_roll cam_pitch cam_yaw has_imu r:=0.8 g:=0.8 b:=0.8 a:=0.8 ">
|
|
<!-- base_link of the sensor-->
|
|
<link name="${base_frame}"/>
|
|
<xacro:property name="M_PI" value="3.1415926535897931" />
|
|
<xacro:property name="model" value="${camera_model}" />
|
|
|
|
<joint name="${camera_name}_center_joint" type="fixed">
|
|
<parent link="${parent}"/>
|
|
<child link="${base_frame}"/>
|
|
<origin xyz="${cam_pos_x} ${cam_pos_y} ${cam_pos_z}" rpy="${cam_roll} ${cam_pitch} ${cam_yaw}" />
|
|
</joint>
|
|
|
|
<!-- device Center -->
|
|
<link name="${camera_name}_model_origin">
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="package://depthai_descriptions/urdf/models/${model}.stl" />
|
|
</geometry>
|
|
<material name="mat">
|
|
<color rgba="${r} ${g} ${b} ${a}"/>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
|
|
<joint name="${camera_name}_model_origin_joint" type="fixed">
|
|
<parent link="${base_frame}"/>
|
|
<child link="${camera_name}_model_origin"/>
|
|
<origin xyz="0 0 0" rpy="1.5708 0 1.5708" />
|
|
</joint>
|
|
|
|
|
|
<!-- IMU -->
|
|
|
|
<xacro:if value="${model == 'OAK-D'}">
|
|
<xacro:property name="imu_offset_x" value="0.0" />
|
|
<xacro:property name="imu_offset_y" value="-0.015" />
|
|
<xacro:property name="imu_offset_z" value="-0.013662" />
|
|
<xacro:property name="imu_r" value="0.0" />
|
|
<xacro:property name="imu_p" value="${M_PI/2.0}" />
|
|
<xacro:property name="imu_y" value="0.0" />
|
|
</xacro:if>
|
|
|
|
<xacro:if value="${model == 'OAK-D-PRO'}">
|
|
<xacro:property name="imu_offset_x" value="-0.008" />
|
|
<xacro:property name="imu_offset_y" value="-0.037945" />
|
|
<xacro:property name="imu_offset_z" value="-0.00079" />
|
|
<xacro:property name="imu_r" value="${M_PI}" />
|
|
<xacro:property name="imu_p" value="${M_PI/2.0}" />
|
|
<xacro:property name="imu_y" value="0.0" />
|
|
</xacro:if>
|
|
|
|
<xacro:if value="${model == 'OAK-D-POE'}">
|
|
<xacro:property name="imu_offset_x" value="-0.008" />
|
|
<xacro:property name="imu_offset_y" value="-0.04" />
|
|
<xacro:property name="imu_offset_z" value="-0.020265" />
|
|
<xacro:property name="imu_r" value="${M_PI}" />
|
|
<xacro:property name="imu_p" value="${M_PI/2.0}" />
|
|
<xacro:property name="imu_y" value="0.0" />
|
|
</xacro:if>
|
|
|
|
<xacro:if value="${has_imu}">
|
|
<link name="${camera_name}_imu_frame" />
|
|
<joint name="${camera_name}_imu_joint" type="fixed">
|
|
<parent link="${base_frame}"/>
|
|
<child link="${camera_name}_imu_frame"/>
|
|
<origin xyz="${imu_offset_x} ${imu_offset_y} ${imu_offset_z}" rpy="${imu_r} ${imu_p} ${imu_y}" />
|
|
</joint>
|
|
</xacro:if>
|
|
|
|
</xacro:macro>
|
|
</robot> |