cps_rmp220_support/description/include/depthai_macro.urdf.xacro

<?xml version="1.0"?>
<robot name="depthai_camera"
    xmlns:xacro="http://ros.org/wiki/xacro">

    <xacro:macro name="depthai_camera" params="camera_name camera_model parent base_frame 
                                           cam_pos_x cam_pos_y cam_pos_z 
                                           cam_roll cam_pitch cam_yaw r:=0.8 g:=0.8 b:=0.8 a:=0.8 ">

        <xacro:include filename="$(find cps_rmp220_support)/description/include/base_macro.urdf.xacro"/>
        <xacro:property name="M_PI" value="3.1415926535897931" />
        <xacro:property name="model" value="${camera_model}" />
        <xacro:property name="has_imu" value="false" />
        <xacro:property name="baseline" value="0.075" />

        <xacro:if value="${model == 'OAK-D'}">
            <xacro:property name="has_imu" value="true" />
        </xacro:if>

        <xacro:base camera_name = "$(arg camera_name)" parent = "$(arg parent_frame)" camera_model = "$(arg camera_model)" base_frame = "$(arg base_frame)" cam_pos_x = "$(arg cam_pos_x)" cam_pos_y = "$(arg cam_pos_y)" cam_pos_z = "$(arg cam_pos_z)" cam_roll = "$(arg cam_roll)" cam_pitch = "$(arg cam_pitch)" cam_yaw = "$(arg cam_yaw)" has_imu="${has_imu}"/>

        <!-- RGB Camera -->
        <link name="${camera_name}_rgb_camera_frame" />

        <joint name="${camera_name}_rgb_camera_joint" type="fixed">
            <parent link="${base_frame}"/>
            <child link="${camera_name}_rgb_camera_frame"/>
            <origin xyz="0 0 0" rpy="0 0 0" />
        </joint>

        <link name="${camera_name}_rgb_camera_optical_frame"/>

        <joint name="${camera_name}_rgb_camera_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
            <parent link="${camera_name}_rgb_camera_frame"/>
            <child link="${camera_name}_rgb_camera_optical_frame"/>
        </joint>

        <!-- Left Camera -->
        <link name="${camera_name}_left_camera_frame" />

        <joint name="${camera_name}_left_camera_joint" type="fixed">
            <parent link="${base_frame}"/>
            <child link="${camera_name}_left_camera_frame"/>
            <origin xyz="0 ${baseline/2} 0" rpy="0 0 0" />
        </joint>

        <link name="${camera_name}_left_camera_optical_frame"/>

        <joint name="${camera_name}_left_camera_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
            <parent link="${camera_name}_left_camera_frame"/>
            <child link="${camera_name}_left_camera_optical_frame"/>
        </joint>


        <!-- right Camera -->
        <link name="${camera_name}_right_camera_frame" />

        <joint name="${camera_name}_right_camera_joint" type="fixed">
            <parent link="${base_frame}"/>
            <child link="${camera_name}_right_camera_frame"/>
            <origin xyz="0 -${baseline/2} 0" rpy="0 0 0" />
        </joint>

        <link name="${camera_name}_right_camera_optical_frame"/>

        <joint name="${camera_name}_right_camera_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
            <parent link="${camera_name}_right_camera_frame"/>
            <child link="${camera_name}_right_camera_optical_frame"/>
        </joint>


    </xacro:macro>

</robot>