Does robot need GPS?

[Russ76]

Hello! I'm wanting to fuse wheel odometry, icp odometry from the lidar, and IMU. Does Fusioncore handle this? Or must I have GPS as an input? One could also fuse odometry from the stereo camera, if that would be helpful?

[manankharwar]

Hey Russ! GPS is totally optional... FusionCore runs fine on just IMU + wheel odometry, or whatever combination you've got.

For your setup specifically:

Wheel odometry: yep, works natively via nav_msgs/Odometry

LiDAR ICP odometry (e.g. KISS-ICP): supported as a second velocity source through the encoder2 parameter. Same outlier gate and adaptive noise as wheel odometry, so it gets treated properly.

IMU: fully supported, gyro and accel bias are estimated in the filter state so you don't have to tune that out manually.

GPS: not required at all. Just set reference.use_first_fix: false and reference.x/y/z: 0.0 and the filter starts at the origin and runs on whatever sensors you give it.

Stereo camera odometry: depends on what your stereo pipeline outputs. If it publishes nav_msgs/Odometry with velocity in the twist field, it can plug straight into encoder2. If it's pose-only, you'd need a small wrapper to derive twist from it. Worth checking what your specific pipeline outputs.

One thing to flag: encoder2 is currently a single slot, so you can't run LiDAR odometry and stereo camera odometry simultaneously without a small modification. If that's a hard requirement for you let me know and we can figure something out to add on this package.

If you run into any trouble getting it configured, just drop your sensor setup here.... what topics you're publishing, message types, frequencies... and I'll put together a config file/any required file for you.

[Russ76]

Thank you for your answer! I worked with Fusioncore today but didn't get it to work. I didn't see it publishing /fusion/odom, either.

Here are the launch file I used, and the config file.

Requirements:

A OAK-D camera

Install depthai-ros package (https://github.com/luxonis/depthai-ros)

Example:

$ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D

most of processing done on Raspi 5 with 8 Gig mem.

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import LifecycleNode

def generate_launch_description():
parameters=[{'frame_id':'oak-d-base-frame',
'subscribe_rgbd':True,
'subscribe_odom_info':True,
'subscribe_scan':True,
'approx_sync':True,
'wait_imu_to_init':True}]

remappings=[('imu', '/imu/data')]

return LaunchDescription([

    # Launch camera driver
    IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('andino_bringup'), 'launch'),
            '/stereo_inertial_node.launch.py']),
            launch_arguments={'depth_aligned': 'false',
                              'enableRviz': 'false',
                              # 'parent_frame': 'second_base_link',
                              'cam_pos_x': '0.1',
                              'stereo_fps': '30',
                              'cam_pitch': '0.08726',
                              'cam_pos_z': '-0.06',
                              'odom_frame_id':'diff_controller/odom',
                              'monoResolution': '400p'}.items(),
    ),

    Node(
        package='rtabmap_odom', executable='icp_odometry', output='screen',
        parameters=[{'odom_frame_id':'icp_odom',
                   # 'guess_frame_id':'odom',
                   'OdomF2M/ScanSubtractRadius': '0.3',
                   'OdomF2M/ScanMaxSize': '10000'}],
        remappings=[('scan_cloud','/velodyne_points'),
                    ('odom','icp_odom')],
    ),

    # Connect velodyne to oak-d_frame
    #Node(
    #    package='tf2_ros', executable='static_transform_publisher', arguments=["-0.09", "0", "0.05", "0", "0", "0", "oak-d_frame", "velodyne"]),

    # Sync right/depth/camera_info together
    Node(   
        package='rtabmap_sync', executable='rgbd_sync', output='screen',
        parameters=parameters,
            # {'approx_sync':False}],
        remappings=[('rgb/image', '/right/image_rect'),
                    ('rgb/camera_info', '/right/camera_info'),
                    ('depth/image', '/stereo/depth')]),

    # Compute quaternion of the IMU
    Node(
        package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
        parameters=[{'use_mag': False, 
                     'world_frame':'enu', 
                     'publish_tf':False}],
        remappings=[('imu/data_raw', '/imu')]),

    # VSLAM
    Node(
        package='rtabmap_slam', executable='rtabmap', output='screen',
        parameters=parameters,
        remappings=remappings,
        arguments=['-d']),

    # fusioncore_node = LifecycleNode(
    Node(
        package="fusioncore_ros",
        executable="fusioncore_node",
        name="fusioncore",
        namespace="",
        output="screen",
        parameters=["/home/russ/ros2_ws/src/fusioncore/fusioncore_ros/config/wt_robot.yaml"],
        remappings=[
            ("/odom/wheels", "diff_controller/odom"),
            #("/gnss/fix",    "/fix"),
        ]),
])

Config file:

fusioncore:
ros__parameters:
base_frame: base_link
odom_frame: odom
publish_rate: 100.0

# Force 2D output: zeroes the Z position in the published odometry and TF.
# For ground robots where altitude is irrelevant (lawn mowers, AGVs, vacuums).
# Prevents GPS altitude noise or IMU Z drift from shifting the costmap plane.
# Orientation (roll/pitch) is untouched: only position.z is forced to zero.
# Leave false for any robot that actually moves in 3D (drones, arms, boats).
publish.force_2d: false

imu.gyro_noise: 0.005
# Set true if IMU has magnetometer (9-axis). False for 6-axis gyro/accel only.
imu.has_magnetometer: false
imu.accel_noise: 0.1
# Does your IMU driver subtract gravity before publishing?
#
#   false (default): IMU publishes raw specific force (~9.81 m/s² upward at rest).
#            This is the standard ROS convention (sensor_msgs/Imu spec).
#            Most drivers (Gazebo NavSat, ZED, Realsense) do NOT remove gravity.
#            Leave false: the filter handles gravity internally.
#
#   true: Your driver already subtracted gravity and publishes true linear
#         acceleration (~0 m/s² at rest). Enable this so FusionCore adds
#         gravity back before fusing, keeping the measurement consistent
#         with what the filter expects.
#
# NOTE: this is the OPPOSITE of robot_localization's imu0_remove_gravitational_acceleration.
# robot_localization: set true to tell rl to remove gravity from the raw reading.
# fusioncore:         set true to tell fusioncore your driver already removed it.
# When in doubt: leave false and check linear_acceleration.z at rest.
#   ~9.8 → leave false.   ~0.0 → set true.
imu.remove_gravitational_acceleration: false
# Override the IMU TF frame. Leave empty to use msg->header.frame_id.
# Set to your IMU frame if the driver publishes with an empty frame_id.
imu.frame_id: "oak_imu_frame"

encoder.vel_noise: 0.05
encoder.yaw_noise: 0.02

# Optional second encoder-twist source (e.g. KISS-ICP LiDAR odometry).
# When non-empty, FusionCore subscribes to this topic as nav_msgs/Odometry
# and fuses twist.linear.x/y + twist.angular.z via the same update_encoder
# path as the primary wheel encoder. Per-axis covariance is taken from the
# message twist.covariance when positive; otherwise adaptive/config noise
# is used. Does not drive ZUPT or ground-constraint updates (those remain
# anchored to the primary wheel encoder). Leave empty to disable.
# encoder2.topic: "/kiss/odometry"
encoder2.topic: "icp_odom"

gnss.base_noise_xy: 1.0
gnss.base_noise_z: 2.0
gnss.heading_noise: 0.02
gnss.max_hdop: 4.0
gnss.min_satellites: 4
# Minimum fix type for GNSS fusion: 1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED
# Note: NavSatFix status=2 maps to RTK_FIXED only.
# RTK_FLOAT (3) is unreachable with this message type.
# Use 4 to require RTK_FIXED, 2 to require any augmented fix.
gnss.min_fix_type: 1

# Antenna lever arm: offset from base_link to primary GNSS antenna in body frame
# x=forward, y=left, z=up (meters)
# Leave at 0.0 if antenna is directly above base_link origin
gnss.lever_arm_x: 0.0
gnss.lever_arm_y: 0.0
gnss.lever_arm_z: 0.0

# Optional second GNSS receiver: leave empty to disable
gnss.fix2_topic: ""

# Lever arm for the secondary GNSS receiver (fix2_topic).
# Set if the second antenna is at a different position than the first.
# Leave at 0.0 if fix2_topic is unused or both antennas are co-located.
gnss.lever_arm2_x: 0.0
gnss.lever_arm2_y: 0.0
gnss.lever_arm2_z: 0.0

# Dual antenna GNSS heading topic (sensor_msgs/Imu message carrying heading).
# Used with receivers that output a second IMU-style message for heading
# (e.g. u-blox F9H, Septentrio dual-antenna). Leave empty to disable.
# When provided, validates heading immediately without needing 5m of travel.
gnss.heading_topic: ""

# compass_msgs/Azimuth heading topic (REP-145 standard).
# Preferred over gnss.heading_topic when your receiver publishes Azimuth.
# Leave empty to disable.
gnss.azimuth_topic: ""

# Mahalanobis outlier rejection
# Rejects GPS jumps, encoder spikes, heading outliers
outlier_rejection: true
outlier_threshold_gnss: 16.27   # chi2(3, 0.999)
outlier_threshold_imu: 15.09    # chi2(6, 0.999)
outlier_threshold_enc: 11.34    # chi2(3, 0.999)
outlier_threshold_hdg: 10.83    # chi2(1, 0.999)

# Static bias initialization window (seconds). Default 0.0 = disabled.
#
# All MEMS IMUs have a small accelerometer and gyro bias that varies by unit.
# By default the filter starts with zero bias and takes ~60 seconds to learn
# the true value, causing a small position drift (~5-10cm) at startup.
#
# When set > 0, the filter collects IMU data for this duration before starting,
# estimates the bias directly from the mean readings, and initializes with the
# correct values. Startup drift drops from ~10cm to under 1cm.
#
# Recommended: 2.0 seconds. Robot MUST be stationary during the window.
# If the robot moves during the window, it falls back to zero bias automatically.
# No benefit if you already have GPS (position corrections override bias drift).
init.stationary_window: 0.0

# Zero-velocity updates (ZUPT)
# When encoder speed and UKF angular rate are both below threshold,
# the robot is considered stationary and velocity is fused to zero.
# This suppresses IMU drift during standstill.
zupt.enabled: true
zupt.velocity_threshold: 0.05   # m/s: below this = stationary
zupt.angular_threshold: 0.05    # rad/s: below this = not rotating
zupt.noise_sigma: 0.01          # m/s: tighter = stronger correction

# Adaptive noise covariance
# Automatically estimates true sensor noise from innovation sequence
# Set to false to use fixed config params only
adaptive.imu: true
adaptive.encoder: true
adaptive.gnss: true
adaptive.window: 50       # number of updates to track (50 = ~0.5s at 100Hz)
adaptive.alpha: 0.01      # learning rate (0.01 = slow, stable)

ukf.q_position: 0.01
ukf.q_orientation: 1.0e-9  # quaternion regularization only: see ukf.hpp
ukf.q_velocity: 0.1
ukf.q_angular_vel: 0.1
ukf.q_acceleration: 1.0
ukf.q_gyro_bias: 1.0e-5
ukf.q_accel_bias: 1.0e-5

# PROJ coordinate reference system
# input.gnss_crs: CRS of incoming NavSatFix messages
#   EPSG:4326 = WGS84 lat/lon (standard GPS: use this for almost all receivers)
#   EPSG:32617 = UTM zone 17N (example for agricultural RTK receivers in Ontario)
# output.crs: internal computation CRS
#   EPSG:4978 = ECEF XYZ (default, globally valid)
# output.convert_to_enu_at_reference: convert output to local ENU frame
#   true = required when output.crs is ECEF (EPSG:4978)
#   false = when output.crs is already a local projected CRS (e.g. UTM)
# reference.use_first_fix: anchor local origin to the first GPS fix received
#   false = use fixed reference.x/y/z in output.crs coordinates
input.gnss_crs: "EPSG:4326"
output.crs: "EPSG:4978"
output.convert_to_enu_at_reference: true
reference.use_first_fix: false
reference.x: 0.0
reference.y: 0.0
reference.z: 0.0

[manankharwar]

Aha... Found the issue. FusionCore is a lifecycle node and needs to go through configure → activate before it starts publishing. In your launch file you're using Node(...) but it needs to be LifecycleNode(...) with lifecycle transitions wired up. Without those the node sits in unconfigured state forever and publishes nothing.

Easiest fix is to add a LifecycleManager to your launch file instead of wiring the transitions manually:

from launch_ros.actions import LifecycleNode
from nav2_common.launch import Node as Nav2Node
from launch_ros.actions import LifecycleNode
from ros2lifecycle.api import call_get_states

# replace your Node(...) block with this
fusioncore_node = LifecycleNode(
    package="fusioncore_ros",
    executable="fusioncore_node",
    name="fusioncore",
    namespace="",
    output="screen",
    parameters=["/home/russ/ros2_ws/src/fusioncore/fusioncore_ros/config/wt_robot.yaml"],
    remappings=[
        ("/odom/wheels", "diff_controller/odom"),
    ]
)

# add a lifecycle manager that handles configure + activate automatically
lifecycle_manager = Node(
    package="nav2_lifecycle_manager",
    executable="lifecycle_manager",
    name="lifecycle_manager_fusioncore",
    output="screen",
    parameters=[{
        "autostart": True,
        "node_names": ["fusioncore"],
    }]
)

Then add both fusioncore_node and lifecycle_manager to your LaunchDescription. The manager will handle configure and activate automatically on startup.

One other thing I noticed: encoder2.topic: "icp_odom" should probably be encoder2.topic: "/icp_odom" with the leading slash, otherwise it resolves relative to whatever namespace and may not find the topic.

Try those two changes and let me know if /fusion/odom shows up. If you can share the output of ros2 lifecycle get /fusioncore after launching I can tell you exactly what state it's stuck in.

[Russ76]

Wow, that was fast! Thanks, I'll try that.

[Russ76]

The launch file won't execute. It stops on the line that says:
fusioncore_node = LifecycleNode(
It suggests changes for the equals sign, or asks, is a colon missing, etc.

russ@raspberrypi:~/launch$ ros2 launch depthai6.launch.py
[INFO] [launch]: All log files can be found below /home/russ/.ros/log/2026-04-27-20-32-39-376935-raspberrypi-1122
[INFO] [launch]: Default logging verbosity is set to INFO
[ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught multiple exceptions when trying to load file of format [py]:

• SyntaxError: invalid syntax. Perhaps you forgot a comma? (depthai6.launch.py, line 95)
• InvalidFrontendLaunchFileError: The launch file may have a syntax error, or its format is unknown
  russ@raspberrypi:/launch$ nano depthai6.launch.py
  russ@raspberrypi:/launch$ ros2 launch depthai6.launch.py
  [INFO] [launch]: All log files can be found below /home/russ/.ros/log/2026-04-27-20-33-37-311784-raspberrypi-1128
  [INFO] [launch]: Default logging verbosity is set to INFO
  [ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught multiple exceptions when trying to load file of format [py]:
• SyntaxError: invalid syntax. Maybe you meant '==' or ':=' instead of '='? (depthai6.launch.py, line 95)
• InvalidFrontendLaunchFileError: The launch file may have a syntax error, or its format is unknown
  russ@raspberrypi:~/launch$ ros2 launch depthai6.launch.py
  [INFO] [launch]: All log files can be found below /home/russ/.ros/log/2026-04-27-20-35-28-277329-raspberrypi-1134
  [INFO] [launch]: Default logging verbosity is set to INFO
  [ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught multiple exceptions when trying to load file of format [py]:
• SyntaxError: invalid syntax. Maybe you meant '==' or ':=' instead of '='? (depthai6.launch.py, line 95)
• InvalidFrontendLaunchFileError: The launch file may have a syntax error, or its format is unknown

[manankharwar]

Sorry about that, the snippet I gave had conflicting imports. Here's the complete launch file to replace yours entirely, no merging needed:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    IncludeLaunchDescription, TimerAction, EmitEvent, RegisterEventHandler
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import LifecycleNode, Node
from launch_ros.event_handlers import OnStateTransition
from launch_ros.events.lifecycle import ChangeState
from lifecycle_msgs.msg import Transition


def generate_launch_description():
    parameters = [{'frame_id': 'oak-d-base-frame',
                   'subscribe_rgbd': True,
                   'subscribe_odom_info': True,
                   'subscribe_scan': True,
                   'approx_sync': True,
                   'wait_imu_to_init': True}]

    remappings = [('imu', '/imu/data')]

    fc = LifecycleNode(
        package="fusioncore_ros",
        executable="fusioncore_node",
        name="fusioncore",
        namespace="",
        output="screen",
        parameters=["/home/russ/ros2_ws/src/fusioncore/fusioncore_ros/config/wt_robot.yaml"],
        remappings=[
            ("/odom/wheels", "/diff_controller/odom"),
        ]
    )

    configure = TimerAction(
        period=2.0,
        actions=[EmitEvent(event=ChangeState(
            lifecycle_node_matcher=lambda a: a is fc,
            transition_id=Transition.TRANSITION_CONFIGURE,
        ))]
    )

    activate = RegisterEventHandler(OnStateTransition(
        target_lifecycle_node=fc,
        start_state="configuring",
        goal_state="inactive",
        entities=[EmitEvent(event=ChangeState(
            lifecycle_node_matcher=lambda a: a is fc,
            transition_id=Transition.TRANSITION_ACTIVATE,
        ))]
    ))

    return LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory('andino_bringup'), 'launch'),
                '/stereo_inertial_node.launch.py']),
            launch_arguments={
                'depth_aligned': 'false',
                'enableRviz': 'false',
                'cam_pos_x': '0.1',
                'stereo_fps': '30',
                'cam_pitch': '0.08726',
                'cam_pos_z': '-0.06',
                'odom_frame_id': 'diff_controller/odom',
                'monoResolution': '400p'
            }.items(),
        ),

        Node(
            package='rtabmap_odom', executable='icp_odometry', output='screen',
            parameters=[{'odom_frame_id': 'icp_odom',
                        'OdomF2M/ScanSubtractRadius': '0.3',
                        'OdomF2M/ScanMaxSize': '10000'}],
            remappings=[('scan_cloud', '/velodyne_points'),
                        ('odom', 'icp_odom')],
        ),

        Node(
            package='rtabmap_sync', executable='rgbd_sync', output='screen',
            parameters=parameters,
            remappings=[('rgb/image', '/right/image_rect'),
                        ('rgb/camera_info', '/right/camera_info'),
                        ('depth/image', '/stereo/depth')]),

        Node(
            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
            parameters=[{'use_mag': False,
                         'world_frame': 'enu',
                         'publish_tf': False}],
            remappings=[('imu/data_raw', '/imu')]),

        Node(
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=parameters,
            remappings=remappings,
            arguments=['-d']),

        fc,
        configure,
        activate,
    ])

What changed from your original: FusionCore is now a LifecycleNode, and two actions wire up the configure and activate transitions automatically. No nav2 required for this, it uses pure ROS 2 lifecycle events.

One more thing to fix in your YAML config: encoder2.topic: "icp_odom" needs a leading slash so it becomes encoder2.topic: "/icp_odom". Without it the topic name resolves relative to a namespace and might not match what rtabmap_odom is actually publishing.

After launching, run ros2 lifecycle get /fusioncore and you should see active. Then /fusion/odom should be publishing.

[Russ76]

Thank you! I'll try it tomorrow!

[Russ76]

The launch file worked somewhat, but not well. I get my best results throwing out completely the wheel odom, and just using the icp odom. I named the output from Ros2control as unreliable/odom instead of diff_controller/odom, and Slam toolbox did its best work with Rtabmap. Rtabmap seems particular and doesn't work with Fusioncore well in my setup. When RtabmapViz doesn't show the camera image, the slam doesn't publish the map topic, and one can't proceed. This is what I see when Rtabmap isn't working properly, Viz shows nothing. Perhaps I can pair odom with visual odom and try to tune the former better, not using at all for a time icp odom. It's all quite challenging. The robot has two Raspberry Pi 5s, the 4 Gig memory one does Ekumen Andino software, and the 8 Gig memory runs the launch file you worked on. The IMU is actually in the Oak-D camera. Andino has one but it isn't fully supported in the software yet, as far as I can see. The Velodyne VLP16 is turned down to 300 rpm and I'm principally using /scan at this time.

[manankharwar]

Hey Russ.... glad the launch file got it running. From what you're describing, the issues are upstream of FusionCore:

The RTABMAP/camera problem: when RTABMAPViz isn't showing the camera image, that's a topic mismatch or sync issue inside RTABMAP itself. FusionCore only publishes odom → base_link TF and /fusion/odom: it doesn't interact with RTABMAP's image pipeline at all. Worth checking ros2 topic hz /right/image_rect and /stereo/depth to make sure those are actually publishing before RTABMAP starts.

Throwing out wheel odom: that's actually fine and makes sense if your wheel encoder is unreliable. FusionCore runs perfectly on ICP odometry alone as the velocity source via encoder2.topic. If you want to go that route, just leave /odom/wheels unremapped (FusionCore will wait for it but it won't fail) or I can show you how to disable it entirely in the config.

IMU from OAK-D: one thing to check: the OAK-D IMU publishes on its own frame (oak_imu_frame), and you've already set imu.frame_id: "oak_imu_frame" in the config, which is correct. Just make sure there's a TF from oak_imu_frame → base_link published somewhere in your stack (the depthai driver usually handles this).

What does ros2 topic hz /fusion/odom show while running? If FusionCore is active and publishing, the SLAM issues are entirely in the RTABMAP side of things and I'd point you to the RTABMAP community for those. If /fusion/odom isn't publishing, paste the output of ros2 lifecycle get /fusioncore and we'll dig in.

I was able to understand this relatively easy because I’d worked with a similar setup before, and that experience was part of what motivated me to build this sensor fusion system

[manankharwar]

To make things easier for you... I've just added a config file built for exactly your setup: icp_indoor.yaml: IMU + LiDAR ICP, no GPS, no wheel odom required.

Your launch should look like this:

ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/icp_indoor.yaml \
  --ros-args \
  -r /odom/wheels:=/icp_odom \
  -r /imu/data:=/imu

And in the config, set:

imu.frame_id: "oak_imu_frame"

That's it. FusionCore treats ICP odom exactly like wheel odom: same outlier gate, same adaptive noise, same ZUPT when stationary. The noise values in icp_indoor.yaml are already tuned tighter than wheel odom defaults since LiDAR ICP is more accurate.

Also there's a new hardware decision table on the docs page now: if you ever need to know which config to use for a given sensor combination, that's the place to start.

[Russ76]

Thanks for the good work! I'll apply these answers today.

[Russ76]

I tried the changes today and still didn't have good results with Fusioncore. Maybe the Raspberry 5 isn't up to that extra processing?

Running Rtabmap without the (slight) benefit of regular odom makes the icp_odom more brittle. It seems to want the "guess" input to keep "on top of" the robot odom. Otherwise, I have to drive very slowly and rotate slowly. I'm wondering about keeping the wheel odom with its name of "unreliable/odom" but remapping it to odom just for the icp odometry node? If it is under the name of diff_controller/odom, then Slam Toolbox can find it and the robot begins jumping around. Probably I should ask this in the Rtabmap forum!

[manankharwar]

@Russ76, the Raspberry Pi isn't the problem.... FusionCore's UKF is very lightweight, it adds maybe 2-3% CPU.

The real issue is the guess_frame_id you had commented out in your original launch file:

# 'guess_frame_id':'odom',

Uncomment that. This tells rtabmap's icp_odometry to use the odom frame as its starting point for scan matching. Without a guess, ICP has to search a much wider area on every scan, which is why you have to drive slowly and rotate slowly.... it's working much harder than it needs to.

FusionCore publishes the odom frame continuously from IMU + wheel odom. When ICP uses that as its guess, it just needs to refine a small local correction instead of searching from scratch. The two work in a loop:

FusionCore (IMU + wheels) → odom frame → ICP uses as guess → produces icp_odom → FusionCore fuses icp_odom → better odom frame

For the jumping with slam_toolbox: that's a TF conflict. Both FusionCore and something else are publishing odom→base_link. Make sure only FusionCore publishes that TF. Rename diff_controller/odom to something slam_toolbox won't find, feed it into FusionCore only, and point slam_toolbox at /fusion/odom.

Try uncommenting guess_frame_id: 'odom' first and see if ICP becomes stable. That's likely 80% of the brittleness problem.

[Russ76]

There may be something in Fusioncore that kills the rgbd_sync node of Rtabmap. I'm finding that the camera image doesn't come up in Viz because this node is dying. It doesn't die when the launch file is run without Fusioncore.

this is for fusioncore.txt

Check line 117