The parameter minimum_travel_heading has little to no effect (jazzy/rolling) #807
Description
Required Info:
- Operating System:
- Ubuntu 24.04
- Installation type:
- deb
- ROS Version
- Jazzy / almost definitely Rolling too
- Version or commit hash:
- Tested only with 2.8.3 on Jazzy
- Laser unit:
- ANY, but the issue hides itself a little when mapping with a 360° lidar, but easily seen even with a 360° lidar when localising
Steps to reproduce issue
Start any localisation / mapping node, rotate the robot in-place.
Expected behavior
minimum_travel_heading=0.1 param getting the respect it deserves 🙃
Actual behavior
No processing, Even after a 360° in-place rotation
Additional information
Hello @SteveMacenski
This is an important issue that we're still experiencing in jazzy🎺 , and the main ⁝⁝⁝2 branch looks the same. slam_toolbox_localization_node advertises parameter minimum_travel_heading but it is not respected.
I thought the mapping nodes did with the Mapper::HasMovedEnough(...) function but checked the node codes again, the laser scan is still discarded, shouldProcessScan doesn't let the mapper process the laser scan with a pure rotation within the minimum_travel_heading.
My robot has a 360° lidar, but to test it, I narrowed its sensor to a smaller FOV in sim, and rotating the robot 360° in-place multiple times, has absolutely no effect, with this parameter having a small value of 0.1.
A related issue #499 was closed by you, probably due to an unlucky name with an EOL distribution(foxy), even though @destogl said that this occured in humble too: #499 (comment).
You're doing a lot and it is easy to miss these things. Also there are PRs #451, #545, and #709 addressing this, but they seem like they have been buried by time, aimed at the wrong branch etc. I cannot re-open the issue so I wanted to open this one with ✨ rolling ✨ in its title.
I was surprised that this issue survived this long and even questioned myself when reading the code. Maybe, If not a lot of companies use Slam Toolbox for localisation, that might be the reason it survived enough?
This is a problem for i.e. robots which has vx_min: 0.0 for mppi and don't use the default nav2_behaviors::BackUp behavior plugin simply because they don't have enough sensors to cover their backs. On a TurtleBot3 with a depth camera facing front, backing up like that would be insecure. I restricted mppi from going back but I remember using the default navigate_to_pose_w_replanning_and_recovery.xml and that this backup plugin doesn't care 🙃 So in a bt without a Backup, a Spin recovery should be enough to trigger re-localisation.
Or, let's not go out of the scope of this repo, still with the default guy; TurtleBot3, almost all the diff drive robots have more problems with the rotational part of the odometry rather than the linear. You know, that's where usually the IMU weighs in. Currently minimum_travel_heading not being respected 'bug' helps this problem as a 'feature', but at what cost 🙃 If some robot has a bad rotational odometry and no IMU, they should not start mapping via in-place rotation or increase decrease this parameter if they have enough processing power etc.
Fixing this issue would also improve mapping quality with lidars with any FOV, even with 360°. Just try running mapping (you can imagine, no need to run) with this change and by just rotating your robot in-place only at the beginning. Without odometry accumulating errors by just travelling, starting mapping with an in-place rotation allows different hit points in the same walls (by just rotating the lidar) and helps creating more dependable initial hits to build on, where probably the docking station will reside.
My humble suggestion would be to go with the @981213's PR #545 since it's the simplest one, aimed at the correct branch and you already reviewed it, before someone else (probably me) opens another PR.
Or if you think, pinging the guy from years ago would take time,
The change to fix, is simple. I can open a PR with this change in src/slam_toolbox_common.cpp:
bool SlamToolbox::shouldProcessScan(
...
- static double min_dist2 =
+ static double min_dist2 = 0.81 * // within 10% for correction error
smapper_->getMapper()->getParamMinimumTravelDistance() *
smapper_->getMapper()->getParamMinimumTravelDistance();
+ static double min_rotation = 0.9 * // within 10% for correction error
+ math::DegreesToRadians(smapper_->getMapper()->getParamMinimumTravelHeading());
...
- // check moved enough, within 10% for correction error
+ if (scan_ctr < 5) {
+ return false;
+ }
+
+ // check if the movement is enough
const double dist2 = last_pose.SquaredDistance(pose);
- if (dist2 < 0.8 * min_dist2 || scan_ctr < 5) {
+ const double heading_diff = fabs(pose.GetHeading() - last_pose.GetHeading());
+ if (dist2 < min_dist2 && heading_diff < min_rotation) {
return false;
}final:
bool SlamToolbox::shouldProcessScan(
...
static double min_dist2 = 0.81 * // within 10% for correction error
smapper_->getMapper()->getParamMinimumTravelDistance() *
smapper_->getMapper()->getParamMinimumTravelDistance();
static double min_rotation = 0.9 * // within 10% for correction error
math::DegreesToRadians(smapper_->getMapper()->getParamMinimumTravelHeading());
...
if (scan_ctr < 5) {
return false;
}
// check if the movement is enough
const double dist2 = last_pose.SquaredDistance(pose);
const double heading_diff = fabs(pose.GetHeading() - last_pose.GetHeading());
if (dist2 < min_dist2 && heading_diff < min_rotation) {
return false;
}
last_pose = pose;
last_scan_time = scan->header.stamp;
return true;tested with jazzy but should work with rolling.
If you don't like Radians -> Degrees -> Radians, I can add this in Mapper.cpp:
double Mapper::getParamMinimumTravelHeadingInRadians()
{
return static_cast<double>(m_pMinimumTravelHeading->GetValue());
}If the concern here is, "fixing this bug would introduce an extra load in existing robots where it didn't process scans before", I'd like to ask, should that be the slam_toolbox's concern? Again, I'm still surprised that this bug survived long enough, just trying to make sense here.