0
$\begingroup$

I"m trying to setup a robot to do stereo mapping with rtabmap, and depthai's Oak-D-Pro cameras. Mainly testing with gazebo at the moment. When I rotate the robot in gazebo the TF tree rotates around the head of the robot, when moving forward/backward the robot moves up/down.

The TF tree looks correct and the transforms all seem correct.

Video example ROS bag

I did have to modify the depthai urdf to get the simulated cameras pointing in the correct direction (the relevant section is bellow).

Launch file for rtabmap:

def generate_launch_description():
    rviz_arg = DeclareLaunchArgument("rtabmap_rviz", default_value="false")

    remap = [
        # ("/imu", "/imu"),
        # ("/left/image_rect", "/left"),
        # ("/right/image_rect", "/right"),
        # ("/odom", "/odom/visual"),
    ]

    left_rectify = Node(
        package="image_proc",
        executable="image_proc",
        name="left_rectify",
        remappings=[
            ("/image", "/left/image_raw"),
            ("/camera_info", "/left/camera_info"),
            ("/image_rect", "/left/image_rect"),
        ],
    )

    right_rectify = Node(
        package="image_proc",
        executable="image_proc",
        name="right_rectify",
        remappings=[
            ("/image", "/right/image_raw"),
            ("/camera_info", "/right/camera_info"),
            ("/image_rect", "/right/image_rect"),
        ],
    )

    rtab_stereo_odometry = Node(
        package="rtabmap_odom",
        executable="stereo_odometry",
        output="screen",
        parameters=[
            {
                "frame_id": "base_footprint",
                "wait_for_transform": 1.0,
                "publish_tf": True,
            },
        ],
    )

    rtabmap = Node(
        package="rtabmap_slam",
        executable="rtabmap",
        output="screen",
        parameters=[
            {
                "frame_id": "base_footprint",
                "subscribe_depth": False,
                "subscribe_stereo": True,
                "subscribe_rgbd": False,
                "subscribe_scan_cloud": False,
                "approx_sync": False,
                "wait_imu_to_init": False,
                "subscribe_rgb": False,
                "publish_tf": True,
                "database_path": "/data/rtabmap_sim.db",
                "use_sim_time": True,
                "Grid/FootprintLength": "0.52",
                "Grid/FootprintWidth": "0.52",
                "Grid/FootprintHeight": "1.8",
                "Grid/MaxObstacleHeight": "1.8",
            }
        ],
        remappings=remap,
        arguments=["-d"],
    )

    rtab_rviz = Node(
        package="rtabmap_viz",
        executable="rtabmap_viz",
        output={"both": "log"},
        parameters=[
            {
                "frame_id": "base_footprint",
                "approx_sync": True,
                "approx_sync_max_interval": 0.1,
                "wait_imu_to_init": False,
                "publish_tf": False,
                "subscribe_stereo": True,
                "subscribe_scan_cloud": False,
                "use_sim_time": True,
                "subscribe_rgb": False,
            }
        ],
        remappings=remap,
        condition=IfCondition(LaunchConfiguration("rtabmap_rviz")),
    )

    return LaunchDescription(
        [
            # ==== Launch Arguments
            rviz_arg,
            # ==== Nodes
            rtab_rviz,
            left_rectify,
            right_rectify,
            rtab_stereo_odometry,
            rtabmap,
        ]
    )

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

    <xacro:property name="camera_name" value="oak" />
    <xacro:property name="camera_model" value="OAK-D-PRO" />
    <xacro:property name="base_frame" value="oak-d_frame" />
    <xacro:property name="parent_frame" value="oak-d_frame" />
    <xacro:property name="cam_pos_x" value="0.03" />
    <xacro:property name="cam_pos_y" value="0.0" />
    <xacro:property name="cam_pos_z" value="-0.003" />
    <xacro:property name="cam_roll" value="0.0" />
    <xacro:property name="cam_pitch" value="0.2" />
    <xacro:property name="cam_yaw" value="0.0" />
    <xacro:property name="r" value="0.8" />
    <xacro:property name="g" value="0.8" />
    <xacro:property name="b" value="0.8" />
    <xacro:property name="a" value="0.8" />

    <xacro:property name="M_PI" value="3.1415926535897931" />
    <xacro:property name="model" value="${camera_model}" />
    <xacro:property name="has_imu" value="true" />

    <xacro:property name="baseline" value="0.075" />

    <xacro:property name="has_imu" value="true" />
    <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" />
    <xacro:property name="imu_p" value="1.5708" />
    <xacro:property name="imu_y" value="0" />

    <!-- base_link of the sensor-->
    <link name="${base_frame}" />

    <joint name="${camera_name}_center_joint" type="fixed">
    <parent link="cam_mount_center_link_1" />
    <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="file://$(find avatar)/meshes/${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>

    <!-- 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="0.0 0.0 0.0" />
        <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="0.0 0.0 0.0" />
        <parent link="${camera_name}_right_camera_frame" />
    <child link="${camera_name}_right_camera_optical_frame" />
    </joint>

    <gazebo reference="${camera_name}_left_camera_optical_frame">
    <sensor name="${camera_name}_left_camera_optical_frame" type="camera">
            <camera>
                <horizontal_fov>1.047</horizontal_fov>
                <image>
                    <width>800</width>
                    <height>480</height>
                </image>
                <clip>
                    <near>0.1</near>
                    <far>100</far>
                </clip>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>true</visualize>
            <topic>/left/image_raw</topic>
            <cameraInfoTopicName>/left/camera_info</cameraInfoTopicName>
            <ignition_frame_id>${camera_name}_left_camera_optical_frame</ignition_frame_id>
        </sensor>
    </gazebo>

    <gazebo reference="${camera_name}_right_camera_optical_frame">
    <sensor name="${camera_name}_right_camera_optical_frame" type="camera">
            <camera>
                <horizontal_fov>1.047</horizontal_fov>
                <image>
                    <width>800</width>
                    <height>480</height>
                </image>
                <clip>
                    <near>0.1</near>
                    <far>100</far>
                </clip>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>true</visualize>
            <topic>/right/image_raw</topic>
            <cameraInfoTopicName>/right/camera_info</cameraInfoTopicName>
            <ignition_frame_id>${camera_name}_right_camera_optical_frame</ignition_frame_id>
        </sensor>
    </gazebo>
</robot>

$\endgroup$

1 Answer 1

1
$\begingroup$

So I found the answer: in the modified camera.xacro file here

<link name="${camera_name}_right_camera_optical_frame" />
<joint name="${camera_name}_right_camera_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
    <parent link="${camera_name}_right_camera_frame" />
<child link="${camera_name}_right_camera_optical_frame"
</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="0.0 0.0 0.0" />
        <parent link="${camera_name}_left_camera_frame" />
<child link="${camera_name}_left_camera_optical_frame"
</joint>

I changed the rotation values in rpy, because when testing in the simulation the images coming from the sensors were rotated, so I unrotated them in the xacro file. However that is incorrect as the optical_frame should be rotated (as per depthai's original urdf).

The answer was to set gazebo's reference and sensor name to remove the optical portion, but keep the optical frame for ignition's frame_id.

This is the updated gazebo section of the xacro:

<gazebo reference="${camera_name}_left_camera_frame">
<sensor name="${camera_name}_left_camera_frame" type="camera">
            <camera>
                <horizontal_fov>1.047</horizontal_fov>
                <image>
                    <width>800</width>
                    <height>480</height>
                </image>
                <clip>
                    <near>0.1</near>
                    <far>100</far>
                </clip>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>true</visualize>
            <topic>/left/image_raw</topic>
            <cameraInfoTopicName>/left/camera_info</cameraInfoTopicName>
            <ignition_frame_id>${camera_name}_left_camera_optical_frame</ignition_frame_id>
        </sensor>
</gazebo>

<gazebo reference="${camera_name}_right_camera_frame">
<sensor name="${camera_name}_right_camera_frame" type="camera">
            <camera>
                <horizontal_fov>1.047</horizontal_fov>
                <image>
                    <width>800</width>
                    <height>480</height>
                </image>
                <clip>
                    <near>0.1</near>
                    <far>100</far>
                </clip>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>true</visualize>
            <topic>/right/image_raw</topic>
            <cameraInfoTopicName>/right/camera_info</cameraInfoTopicName>
            <ignition_frame_id>${camera_name}_right_camera_optical_frame</ignition_frame_id>
    </sensor>
</gazebo>
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.