[icp_odometry]: Did not receive data since 5 seconds! & initial pose cannot be set

[danit-niwattananan]

Hi everyone, I am running my robot on simulation using ROS2 Humble within Gazebo and testing SLAM and localization of different sensors.

Setup:
I am using camera, imu, and lidar (with only 90 deg horizontal FOV like our hardware and not 360). I am running rgbd_odometry node to extract the camera odometry and also icp_odometry node to extract lidar odometry from 3D lidar point cloud. All of these odometry and imu data are then fed into ekf_node from robot_localization package to extract filtered odometry /odometry/filtered, which are then fed into rtab_slam node to generate maps. All of these are running inside a modified version of ROS2 Docker container with some extra packages.

After I built my packages with colcon build, source it, and launch the simulation, I then drive my rover around using teleop_twist_keyboard and record all of the odometry values for evaluation using evo package. You can see the plot below.

Problem:

1. Did not receive data since 5 seconds! - As I launched the simulation, there was no any warning or error messages. However, as soon as I stop the ros2 bag record with KeyboardInterrupt, this warning message appears.

[icp_odometry-5] [WARN] [1720610340.705824127] [rtab_icp_odometry]: rtab_icp_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
[icp_odometry-5] rtab_icp_odometry subscribed to /unused_scan and /bf/points_raw (make sure only one of this topic is published, otherwise remap one to a dummy topic name).
[icp_odometry-5] [WARN] [1720610349.774851320] [rtab_icp_odometry]: rtab_icp_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
[icp_odometry-5] rtab_icp_odometry subscribed to /unused_scan and /bf/points_raw (make sure only one of this topic is published, otherwise remap one to a dummy topic name). 

And the output frequency of the icp_odometry jumps from around 0.5 Hz to 4.4 Hz, which is still lower than lidar points cloud publishing at around 5.5 Hz when checked with ros2 topic hz /bf/points_raw after that. That means the input topic is NOT empty like the message suggested. I am also sure that I mapped the /scan topic to a ghost topic /unused_scan where no message is published in the launch file. So, I really don't know why this happens. Here is my launch file for reference:

from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from nav2_common.launch import RewrittenYaml
import os
from launch.actions import GroupAction
from launch_ros.actions import SetRemap


def generate_launch_description():
    """Launches an Extended Kalman Filter node to fuse visual odometry, wheel odometry and IMU data. The Extended Kalman Filter publishes
    the odom->base_link TF."""
    conf_use_sim_time = LaunchConfiguration("use_sim_time", default="True")
    lidar_topic_name = LaunchConfiguration("topic", default="bf/points_raw")

    pkg_crater_localization = get_package_share_directory("crater_localization")

    # rgbd odometry
    rgbd_odometry_params = [pkg_crater_localization, "/config/", "rtab_rgbd_odometry.yaml"]
    rewritten_rgbd_odometry_params = RewrittenYaml(
        source_file=rgbd_odometry_params,
        param_rewrites={
            'publish_tf': "False",
        },
    )
    remappings = [
        ("rgb/image", "camera/image_raw"),
        ("rgb/camera_info", "camera/camera_info"),
        ("depth/image", "camera/depth/image_raw"),
        ("imu", "imu"),
        ("odom", "odometry/camera")
    ]
    rgbd_odometry = Node(
        package="rtabmap_odom",
        executable="rgbd_odometry",
        name="rtab_rgbd_odometry",
        output="log",
        parameters=[rewritten_rgbd_odometry_params, {"use_sim_time": conf_use_sim_time}],
        remappings=remappings,
        arguments=["--ros-args", "--log-level", "Warn"])
    
    # lidar odometry, use kiss_icp instead
    icp_odometry_params = [pkg_crater_localization, "/config/", "rtab_icp_odometry.yaml"]
    rewritten_icp_odometry_params = RewrittenYaml( # let the ekf publish the odom frame, hence set to false for this node
        source_file=icp_odometry_params,
        param_rewrites={
            'publish_tf': "False",
        },
    )
    remappings = [
        ("scan", "unused_scan"),
        ("scan_cloud", "bf/points_raw"),
        ("imu", "imu"),
        ("odom", "odometry/lidar")
    ]
    icp_odometry = Node(
        package="rtabmap_odom",
        executable="icp_odometry",
        name="rtab_icp_odometry",
        output="log",
        parameters=[rewritten_icp_odometry_params, {"use_sim_time": conf_use_sim_time}],
        remappings=remappings,
        arguments=["--ros-args", "--log-level", "Warn"])

    # extended kalman filter, use lidar odometry, rgbd odometry and imu data
    ekf_params_file = os.path.join(pkg_crater_localization, "config", "ekf_lid_rgbd.yaml")
    ekf = Node(
        package="robot_localization",
        executable="ekf_node",
        name="ekf_filter_node",
        output="log",
        parameters=[ekf_params_file, {"use_sim_time": conf_use_sim_time}],
        remappings=[
            # ("odometry/filtered", "/odom"),
            # ("accel/filtered", "/accel")
        ]
    )

    rtabmap = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_crater_localization,
                         "launch", "rtabmap.launch.py")
        ),
        launch_arguments=[
            ("use_sim_time", conf_use_sim_time),
            ("odometry_topic", "odometry/filtered")
        ]
    )

    kiss_icp_odometry = GroupAction(
        actions=[
            SetRemap(src="/kiss/odometry", dst="/odometry/lidar"),
            
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource([
                    os.path.join(
                        get_package_share_directory('kiss_icp'),
                        'launch',
                        'odometry.launch.py'
                    )
                ]),
                launch_arguments={
                    'topic': lidar_topic_name,
                    'use_sim_time': conf_use_sim_time,
                    'base_frame': 'base_link',
                    'visualize': 'true', # set to false to not open the kiss_icp rviz window 
                }.items(),
            )
        ]
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument("use_sim_time", default_value=conf_use_sim_time),
            DeclareLaunchArgument("lidar_topic_name", default_value=lidar_topic_name),
            ekf,
            rgbd_odometry,
            # kiss_icp_odometry, # Just try out kiss_icp but its output is also always almost 0, not sure why.
            icp_odometry, 
            rtabmap
        ]
    )

I looked into similar issues in the past and I found these issues #189 and #1054. I don't think that the synchronizer is the cause of this problem like suggested here #1054 (comment) since I set approx_sync to false. And I am sure that my tf is working and the base_link frame is there. Here is the transform of my robot and simulation:

2. Initial pose cannot be changed - Since I need to spawn my robot not at origin (due to problem with path planner cannot be spawn in area with high cost, else it will crash), I want to change the initial pose that the icp_odometry node will estimate. I did that by declaring the parameter initial_pose in yaml file in config folder under my package directory. All of the parameters come from the official wiki http://wiki.ros.org/rtabmap_odom#icp_odometry. However, when I plot the odometry out with evo_traj from evo package, the odometry from lidar estimated by that node always start at somewhere close to the origin and not at the coordinate I gave it! Here is the picture of the xyz of odometry. (If initial pose declaration is working, it should start very close to /odometry/camera and /odometry/lidar)
   

Here is my yaml file in which I declare initial pose. This yaml file is then used by the launch file above.

rtab_rgbd_odometry:
  ros__parameters:
    frame_id: base_link
    wait_imu_to_init: true
    approx_sync: false # before it was true
    subscribe_scan_cloud: true
    publish_tf: true
    qos_imu: 2
    queue_size: 100
    # roll and pitch cannot be improved by changing number as it seems to start randomly 
    initial_pose: -1.17 -9.0 1.715 0 0 0.04712388 # Before was z=1 and zeros elsewhere,
    # changed so that the /odometry/camera starts at the same position as /odometry/ideal. rpy in radian not degree.
    # better way might be just to declare ground_truth_frame_id with ground_truth frame if exists

I have also tried declaring ground_truth_frame_id and ground_truth_base_frame_id using my base_link (I have a node declared in my robot's URDF from libgazebo_ros_p3d.so to publish ground truth using base_link) but that didn't work. Do you guys have any ideas on how to solve it?

This problem is really annoying since that also cause a "teleportation" problem as those lidar input close to 0 made the filtered odometry jump instantly like in the plot below.

3. Estimated odometry always almost 0 - As you can see from the graph above, the xyz value of the lidar odometry outputted by icp_odometry node is always almost 0 and much worse than the camera odometry, which shouldn't be the case. Do you guys also know what could have gone wrong here? I tried using kiss_icp package https://github.com/PRBonn/kiss-icp?tab=readme-ov-file but the odometry from that is also almost zero.

I have been trying to solve this problem for 2-3 weeks already and I am really out of ideas. Any help would be greatly appreciated!

[matlabbe]

For problem 1, I created #1186, which may be the cause. I think you could just ignore the warning if it was one of the causes cited there.

For problem 2, I tried with this demo (adding 'initial_pose': '10 10 0 0 0 0' to icp_odometry parameters) and it seems to work as expected:

For problem 3, could you record a rosbag of the scans you are feeding to icp_odometry or kiss-icp?

[danit-niwattananan]

Hi @matlabbe , thank you for your response. Here is my rosbag record of my 3d lidar point cloud that I feed into icp_odometry and kiss-icp node. https://drive.google.com/file/d/1UNnMuyYQ84lfDErb_yFD-Y8YijEjnnbH/view?usp=sharing

I have recorded it as soon as I started my simulation. I also drove my rover around a little bit during the recording.

[matlabbe]

The point cloud topic doesn't look right. Instead of setting 0 or NaN for invalid readings, it sets a maximum of some distance:

When doing ICP on this, it will always converge on the points on the arc, so it may thinks the robot is not moving.

Try with Icp/RangeMax: '50'. Here an example:

ros2 run rtabmap_odom icp_odometry --ros-args \
   -r scan_cloud:=/bf/points_raw \
   -p Icp/VoxelSize:="'0.3'" \
   -p Icp/CorrespondenceRatio:="'0.01'" \
   -p Icp/MaxCorrespondenceDistance:="'1'" \
   -p Icp/MaxTranslation:="'1'" \
   -p Icp/RangeMax:="'50'" \
   -p Icp/OutlierRatio:="'0.7'" \
   -p Odom/ScanKeyFrameThr:="'0.4'" \
   -p OdomF2M/ScanSubtractRadius:="'0.3'" \
   -p OdomF2M/ScanMaxSize:="'15000'"

ros2 bag play  rosbag2_2024_08_04-16_38_20_0.db3

[danit-niwattananan]

Thank you so much for your help. I will try this out.