I recently purchased a ZED 2i camera and am using it with my autonomous robot running ROS 2 (Humble). I’m currently integrating the ZED 2i with the robot_localization package to perform sensor fusion using an EKF.
Here’s my setup:
Odometry from the robot’s wheel encoders (/diffbot_base_controller/odom)
IMU data from the ZED 2i (/zed2i/zed_node/imu/data)
Visual odometry from the ZED 2i (/zed2i/zed_node/odom)
The issue arises when I try to fuse all of these inputs together. My robot starts behaving erratically — it looks like it’s jumping between two positions in RViz, almost as if the fused data is conflicting or lagging behind. This causes the overall localization to become very unstable.
I’d like to ask:
What is the correct way to fuse ZED 2i’s visual odometry and IMU with wheel odometry in ROS 2 using robot_localization?
Are there any important parameters in the zed_ros2_wrapper or ekf_node that I should tweak to prevent TF/frame mismatches or overconfidence in a particular source?
To help with troubleshooting, I’m attaching:
My EKF configuration
My ZED 2i launch file
My URDF
Video of issue
Ekf Params
# robot_localization EKF configuration for DiffBot with ZED camera and IMU
local_ekf_filter_node:
ros__parameters:
frequency: 20.0
two_d_mode: true
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: /diffbot_base_controller/odom
odom0_config: [true, true, false,
false, false, true,
true, false, false,
false, false, true,
false, false, false]
odom0_differential: false
odom1: /zed/zed_node/odom
odom1_config: [true, true, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
odom1_differential: false
imu0: /zed/zed_node/imu/data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
Launch file
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import RegisterEventHandler, IncludeLaunchDescription, DeclareLaunchArgument, TimerAction
from launch.event_handlers import OnProcessExit
from launch.substitutions import Command, PathJoinSubstitution, LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Get package directories
diffdrive_zlac_dir = get_package_share_directory('diffdrive_zlac')
ldlidar_node_dir = get_package_share_directory('ldlidar_node')
# Launch arguments
use_sim_time = LaunchConfiguration('use_sim_time')
enable_sensor_fusion = LaunchConfiguration('enable_sensor_fusion')
enable_slam = LaunchConfiguration('enable_slam')
camera_model = LaunchConfiguration('camera_model')
# Declare launch arguments
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true')
declare_enable_sensor_fusion_cmd = DeclareLaunchArgument(
'enable_sensor_fusion',
default_value='true',
description='Enable robot_localization EKF sensor fusion')
declare_enable_slam_cmd = DeclareLaunchArgument(
'enable_slam',
default_value='true',
description='Enable SLAM toolbox')
declare_camera_model_cmd = DeclareLaunchArgument(
'camera_model',
default_value='zed2i',
description='ZED camera model',
choices=['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'zedxonegs', 'zedxone4k'])
# Get URDF directly (no xacro processing needed)
robot_description_content = Command(
[
"cat",
" ",
PathJoinSubstitution(
[FindPackageShare("diffdrive_zlac"), "urdf", "diffbot.urdf"]
),
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[
FindPackageShare("diffdrive_zlac"),
"config",
"diffbot_controllers.yaml",
]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("diffdrive_zlac"), "rviz", "diffbot.rviz"]
)
# Robot localization EKF parameters
robot_localization_params = PathJoinSubstitution(
[
FindPackageShare("diffdrive_zlac"),
"config",
"robot_localization_params.yaml",
]
)
# Robot control and state publisher nodes
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output="both",
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description, {'use_sim_time': use_sim_time}],
remappings=[
("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[{'use_sim_time': use_sim_time}]
)
# Joint state broadcaster
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
# Robot controller
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["diffbot_base_controller", "--controller-manager", "/controller_manager"],
)
# Static transform from camera mount to ZED camera link
camera_static_transform = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="camera_mount_to_zed_transform",
arguments=["0", "0", "0", "0", "0", "0", "camera_mount_link", "zed_camera_link"],
output="log",
parameters=[{'use_sim_time': use_sim_time}]
)
# Include ZED camera launch
zed_camera_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare("zed_wrapper"),
"launch",
"zed_camera.launch.py"
])
]),
launch_arguments={
'camera_model': camera_model,
'publish_urdf': 'true',
'publish_tf': 'false', # Let EKF handle odometry TF
'publish_map_tf': 'false', # Let SLAM handle map frame
'camera_name': 'zed',
'use_sim_time': use_sim_time
}.items()
)
# Single front LiDAR launch
front_lidar_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
ldlidar_node_dir,
"launch",
"ldlidar_with_mgr.launch.py"
])
]),
launch_arguments={
'use_sim_time': use_sim_time,
'node_name': 'front_ldlidar_node',
}.items()
)
# Robot localization EKF node for sensor fusion
ekf_local_node = Node(
condition=IfCondition(enable_sensor_fusion),
package='robot_localization',
executable='ekf_node',
name='local_ekf_filter_node',
output='screen',
parameters=[
robot_localization_params,
{'use_sim_time': use_sim_time}
]
)
# SLAM Toolbox Node
slam_params_file = PathJoinSubstitution([
diffdrive_zlac_dir,
'config',
'slam_toolbox_online_async.yaml'
])
start_async_slam_toolbox_node = Node(
condition=IfCondition(enable_slam),
parameters=[
slam_params_file,
{'use_sim_time': use_sim_time}
],
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen',
remappings=[
('scan', '/ldlidar_node/scan'), # Use single front LiDAR
('odom', 'odometry/filtered') # Use EKF filtered odometry instead of raw
]
)
# Delay start of robot_controller after joint_state_broadcaster
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[robot_controller_spawner],
)
)
# Start camera and LiDAR after joint_state_broadcaster
delay_sensors_after_joint_state_broadcaster = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[camera_static_transform, zed_camera_launch, front_lidar_launch],
)
)
# Start EKF after sensors are running (with delay)
delay_ekf_after_sensors = TimerAction(
period=5.0,
actions=[ekf_local_node]
)
# Start SLAM after EKF is running (with delay)
delay_slam_after_ekf = TimerAction(
period=8.0,
actions=[start_async_slam_toolbox_node]
)
# Add a TimerAction to delay rviz_node start after EKF
joy_node = Node(
package="joy",
executable="joy_node",
name="joy_node",
output="screen"
)
teleop_node = Node(
package="teleop_twist_joy",
executable="teleop_node",
name="teleop_node",
output="screen",
remappings=[
('cmd_vel', '/diffbot_base_controller/cmd_vel_unstamped')
]
)
delay_rviz_after_ekf = TimerAction(
period=10.0,
actions=[rviz_node, joy_node, teleop_node]
)
nodes = [
# Launch arguments
declare_use_sim_time_cmd,
declare_enable_sensor_fusion_cmd,
declare_enable_slam_cmd,
declare_camera_model_cmd,
# Core robot nodes
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
# Event-driven startup
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
delay_sensors_after_joint_state_broadcaster,
# Sensor fusion and SLAM with timing delays
delay_ekf_after_sensors,
delay_slam_after_ekf,
# Start rviz after EKF with delay
delay_rviz_after_ekf,
]
return LaunchDescription(nodes)
URDF Extract of Camera
<joint name="camera_mount_joint" type="fixed">
<parent link="base_link"/>
<child link="camera_mount_link"/>
<origin xyz="0.1815 0 0.010" rpy="0 0 0"/>
</joint>
<link name="camera_mount_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 1.5708"/>
<geometry>
<box size="0.175 0.037 0.03"/>
</geometry>
<material name="Purple"/>
</visual>
</link>
Video of issue
https://drive.divyessh.my/l.webm
Any insights or suggestions would be really appreciated — especially from anyone who has done multi-sensor fusion with the ZED 2i in ROS 2.
Thanks in advance!