[custom] Problem trying to run with rtabmap_odom

[khuechuong]

I'm using ROS noetic and was trying to get odometry for oak-d pro poe. First, I tried the stereo_inertial_node.launch in depthai_example and wrote another launch odom.launch.

odom.launch

<node type="rgbd_odometry" name="rgbd_odometry" pkg="rtabmap_odom">
        <remap from="/rgb/image"        to="/stereo_inertial_publisher/color/image"/>
        <remap from="/rgb/camera_info"  to="/stereo_inertial_publisher/color/camera_info"/>
        <remap from="/depth/image"      to="/stereo_inertial_publisher/stereo/depth"/>
        <param name="frame_id"      type="string"   value="oak-d_frame"/>
        <param name="initial_pose"  type="string"   value="0 0 0.636 0 0 0"/>
        <param name="publish_tf"    type="bool"     value="true"/>

        <rosparam param="wait_imu_to_init">False</rosparam>
        <rosparam param="approx_sync">True</rosparam>
        <rosparam param="approx_sync_max_interval">0.1</rosparam>        
    </node>

it worked as it suppose to; it gives odometry and the tf tree:

However, when I tried it with the ros driver, it didn't give any error message, when I echo /odom, nothing appears. The tree also didn't change. basically I didn't see the odom frame in tf tree.

# YAML
/oak_d_pro_poe_front:
    camera_i_mx_id: 1844301041E8E9F400
    camera_i_nn_type: none   
    camera_i_ip: '169.254.1.1'
    camera_i_pipeline_type: rgbd
    stereo_i_subpixel: true
    camera_i_enable_imu: true
    camera_i_enable_ir: true
    stereo_i_enable_threshold_filter: true
    stereo_i_threshold_filter_min_range: 700
    stereo_i_threshold_filter_max_range: 4000
    camera_i_restart_on_diagnostics_error: true

#camera_.launch
?xml version="1.0"?>
<launch>

    <arg name="publish_tf_from_calibration" default="false" />
    <arg name="imu_from_descr" default="false" />
    <arg name="override_cam_model" default="false" />
    <arg name="rectify_rgb" default="false"/>

    <arg name="name" default="oak" />
    <arg name="params_file" default="$(find depthai_ros_driver)/config/camera.yaml"/>
    <arg name="camera_model" default="OAK-D" />

    <arg name="base_frame" default="oak-d_frame" />
    <arg name="parent_frame" default="oak-d-base-frame" />

    <arg name="cam_pos_x" default="0.0" />
    <!-- Position respect to base frame (i.e. "base_link) -->
    <arg name="cam_pos_y" default="0.0" />
    <!-- Position respect to base frame (i.e. "base_link) -->
    <arg name="cam_pos_z" default="0.0" />
    <!-- Position respect to base frame (i.e. "base_link) -->
    <arg name="cam_roll" default="0.0" />
    <!-- Orientation respect to base frame (i.e. "base_link) -->
    <arg name="cam_pitch" default="0.0" />
    <!-- Orientation respect to base frame (i.e. "base_link) -->
    <arg name="cam_yaw" default="0.0" />
    <!-- Orientation respect to base frame (i.e. "base_link) -->

    <param name="$(arg name)/camera_i_camera_model" value="$(arg camera_model)" if="$(arg override_cam_model)"/> 
    <param name="$(arg name)/camera_i_base_frame" value="$(arg base_frame)"/>
    <param name="$(arg name)/camera_i_parent_frame" value="$(arg parent_frame)"/>
    <param name="$(arg name)/camera_i_cam_pos_x" value="$(arg cam_pos_x)"/>
    <param name="$(arg name)/camera_i_cam_pos_y" value="$(arg cam_pos_y)"/>
    <param name="$(arg name)/camera_i_cam_pos_z" value="$(arg cam_pos_z)"/>
    <param name="$(arg name)/camera_i_cam_roll" value="$(arg cam_roll)"/>
    <param name="$(arg name)/camera_i_cam_pitch" value="$(arg cam_pitch)"/>
    <param name="$(arg name)/camera_i_cam_yaw" value="$(arg cam_yaw)"/>
    <param name="$(arg name)/camera_i_imu_from_descr" value="$(arg imu_from_descr)"/>
    <param name="$(arg name)/camera_i_publish_tf_from_calibration" value="$(arg publish_tf_from_calibration)"/>

    <arg name="launch_prefix" default=""/>

    <rosparam file="$(arg params_file)" />
    <node pkg="rosservice" if="$(optenv DEPTHAI_DEBUG 0)" type="rosservice" name="set_log_level" args="call --wait /oak_nodelet_manager/set_logger_level 'ros.depthai_ros_driver' 'debug'" />

    <include unless="$(arg publish_tf_from_calibration)" file="$(find depthai_descriptions)/launch/urdf.launch">
        <arg name="base_frame" value="$(arg  name)" />
        <arg name="parent_frame" value="$(arg  parent_frame)"/>
        <arg name="camera_model" value="$(arg  camera_model)"/>
        <arg name="tf_prefix" value="$(arg  name)" />
        <arg name="cam_pos_x" value="$(arg  cam_pos_x)" />
        <arg name="cam_pos_y" value="$(arg  cam_pos_y)" />
        <arg name="cam_pos_z" value="$(arg  cam_pos_z)" />
        <arg name="cam_roll" value="$(arg  cam_roll)" />
        <arg name="cam_pitch" value="$(arg  cam_pitch)" />
        <arg name="cam_yaw" value="$(arg  cam_yaw)" />
    </include>

    <node pkg="nodelet" type="nodelet" name="$(arg  name)_nodelet_manager"  launch-prefix="$(arg launch_prefix)" args="manager" output="screen">
        <remap from="/nodelet_manager/load_nodelet" to="$(arg name)/nodelet_manager/load_nodelet"/>
        <remap from="/nodelet_manager/unload_nodelet" to="$(arg name)/nodelet_manager/unload_nodelet"/>
        <remap from="/nodelet_manager/list" to="$(arg name)/nodelet_manager/list"/>
    </node>

    <node name="$(arg  name)" pkg="nodelet" type="nodelet" output="screen" required="true" args="load depthai_ros_driver/Camera $(arg  name)_nodelet_manager">
    </node>

    <node pkg="nodelet" type="nodelet" name="rectify_color"
            args="load image_proc/rectify $(arg  name)_nodelet_manager" if="$(arg rectify_rgb)">
            <remap from="image_mono" to="$(arg name)/rgb/image_raw"/>
        <remap from="image_rect" to="$(arg name)/rgb/image_rect"/>
    </node>  

    <node pkg="nodelet" type="nodelet" name="$(arg  name)_depth_image_to_rgb_pointcloud"
        args="load depth_image_proc/point_cloud_xyzrgb $(arg  name)_nodelet_manager">
        <param name="queue_size"          value="10"/>

        <remap from="rgb/camera_info" to="$(arg name)/rgb/camera_info"/>
        <remap from="rgb/image_rect_color" to="$(arg name)/rgb/image_rect" if="$(arg rectify_rgb)"/>
        <remap from="rgb/image_rect_color" to="$(arg name)/rgb/image_raw" unless="$(arg rectify_rgb)"/>
        <remap from="depth_registered/image_rect" to="$(arg name)/stereo/image_raw"/>    
        <remap from="depth_registered/points" to="$(arg name)/points"/>
    </node>

</launch>

# main launch file

    <arg name="plate_frame" default="sensor_plate_link" />
    <include file="$(find depthai_ros_driver)/launch/camera_.launch">
        <arg name="name" value="oak_d_pro_poe_front"/>
        <arg name="parent_frame" value="$(arg plate_frame)"/>
        <arg name="cam_pos_x" default="0.15" />
        <arg name="cam_pos_y" default="0.01" />
        <arg name="cam_pos_z" default="0.0225" />
        <arg name="cam_roll" default="0.0" />
        <arg name="cam_pitch" default="0.015" />
        <arg name="cam_yaw" default="-0.005" />
    </include>

        <node type="rgbd_odometry" name="rgbd_odometry" pkg="rtabmap_odom">
        <remap from="/rgb/image"        to="oak_d_pro_poe_front/rgb/image_raw"/>
        <remap from="/rgb/camera_info"  to="oak_d_pro_poe_front/rgb/camera_info"/>
        <remap from="/depth/image"      to="oak_d_pro_poe_front/stereo/image_raw"/>
        <!-- <remap from="/imu"              to="/oak_d_pro_poe_front/imu/data"/> -->

        <param name="frame_id"      type="string"   value="$(arg plate_frame)"/>
        <param name="initial_pose"  type="string"   value="0 0 0.636 0 0 0"/>
        <param name="publish_tf"    type="bool"     value="true"/>

        <rosparam param="wait_imu_to_init">False</rosparam>
        <rosparam param="approx_sync">True</rosparam>
        <rosparam param="approx_sync_max_interval">0.1</rosparam>

I'm not sure what's going on. Any thoughts? Thanks!

[Serafadam]

Hi, is every topic connected properly when you run rqt_graph? Also, is this file working for you?