Version: ROS2 Humble

I am trying to map the lab space with my ackermann robot. I have the incoming scan data and the odometry twist field with linear.x and linear.y input from an optical flow sensor, which is calibrated and outputs reliable distance measurements form which i compute linear velocities in x and y (tested).

The output is then fed to ekf which publishes transforms (odom to base_link).


When I start mapping with slam toolbox in async mode, I observe that for completely straight movement, the robot in rviz starts facing orientation issues. The map is able to build and then at the time of NAV2 launch and Smac Planner Hybrid generates a path for a straight goal, again while moving there is the same drift which makes the robot in rviz to enter into a unknown space and NAV2 stops sending cmd_vel (which is obvious).

There might be issue that no yaw info is provided in odometry twist field to the EKF. But, for a straight line goal, yaw should bother to much. Also, verified with the sensor data, it outputs nearly 0.001 m/s in Y direction, meaning only outputting linear.x.

Which makes me think, that is there any tuning required with slam_toolbox params or something with EKF params?

My sensor is Depth cam with max range of 10m and using pointcloud to laserscan it behaves ok for slam_toolbox.The only draw back is due to less FOV, have to do initial manual drive to be in map space for NAV2. I have tested whole stack including NAV2 but the robot drifts even with straight line goals. Sometimes I can see to goal success but with oriented in rviz Just wanted to have suggestion, that slam_toolbox works with sensors with 0.5 as resolution, any idea what should I set for intel RGB-D D435?

Any suggestions will be very helpful @StevenMackenski

I have attached a bag file which includes odometry topics from both sensor and EKF, map topic, /tf , /tf_static, /scan, and some low level motor speed topics.

Bag file: https://github.com/Rak-r/ROS1-ROS2-contents/tree/main/odometry

One more thing, I am trying to make output of cmd_vel either from Joystick or NAV2 and odometry twist from sensor same, to avoid any wrong feed back into NAV2. I calibrated it to some extent, still doing some more tests.


1 Answer 1


My suggestions after examining your bag file and information.

1) Include IMU Source: You did not add an IMU source in your EKF fusion right? Well this sensor is important for you to get a more accurate location of orientation, you should integrate it if possible, otherwise, how does the EKF algorithm foresee your current pose (previous pose) correctly to serve as input for predicting the next pose (where your robot is moving): https://en.wikipedia.org/wiki/Extended_Kalman_filter

It could significantly improve orientation estimates. This would involve adjusting the sensor fusion configuration to incorporate IMU data.

2) Accuracy of Static Trasform: Your odometry source is coming from this optical_flow_link as you said. Check static transform values are correct in terms of translation and rotation ? , from the base_link, otherwise, your initial state input for ekf will be wrong and in consequence, you will get cumulative errors as the output of the ekf localization algorithm.

You need a precise transform from these frames to correctly map base_link to odom to map, finally having a good outcome from base_link relative displacement from the map (linear and angular).

3)Adjustment of Covariance Matrix: Noise in real robots must be handled:

You must adjust your covariance matrix, try changing the diagonal values and check the behavior. IT worked for me: Erratic Robot Movement in RViz2 with Nav2: Back and Forth Motion and Small Jumps

4) Consideration of Robot Model: Differential vs Omnidirectional vs Ackermann model for State Estimation

According to documentation, the EKF was designed to work properly with omnidirectional robots, not Ackermann: http://docs.ros.org/en/melodic/api/robot_localization/html/state_estimation_nodes.html

Maybe you could try on nav2_params.yaml amcl block switch for omnidirectional if you are using differential...

Despite I believe that does not influenciates directly your issue, it at least reduces a little bit the accuracy.

5) Parameter fine-tuning in SLAM Toolbox: Consider modify one by one the mapper_params_online_async.yaml file and check if it improves your localization:

An output of ChatGpt:

  • transform_publish_period: This parameter controls how often the odometry transform is published. If set too high or too low, it might not align well with the actual movement of the robot, causing discrepancies in the observed and actual positions.resolution: This parameter affects the granularity of the map. An incorrect resolution might lead to a less accurate representation of the environment, affecting the robot's ability to localize itself accurately.

    scan_matching and use_scan_barycenter: These parameters control how scans are matched and integrated into the map. Incorrect settings here could lead to poor scan alignment, contributing to drift.

    minimum_travel_distance and minimum_travel_heading: These parameters define the minimum travel distance and change in heading before a scan is considered for integration into the map. If set too high, small but significant movements might be ignored, potentially contributing to drift.

    Loop Closure Parameters: Parameters like loop_match_minimum_response_fine, loop_search_space_dimension, and loop_search_space_resolution are critical for loop closure, which helps correct accumulated drift over time. Incorrect settings can prevent effective loop closure, allowing drift to persist.

Based on the issue described, I would suggest focusing on the following adjustments:

Tuning transform_publish_period: Ensure this is set to a value that matches the rate at which your robot's pose can reliably be estimated. Too frequent or too sparse updates can both be problematic.

Adjusting Resolution: If the map resolution does not match the scale of environmental features well, consider adjusting it to better fit the environment's complexity.

Loop Closure Adjustments: Review and possibly adjust loop closure parameters to ensure that the system can effectively identify and correct for drift over time. This might involve tuning parameters related to the search space and response thresholds for loop closure.

Scan Matching and Barycenter Use: Verify that scan matching is effectively reducing drift by ensuring scans are accurately aligned before integration. Adjusting use_scan_barycenter might also help if the scans are not being effectively centralized.

Hope one of the suggested solutions work! Good Luck!


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.