Added parameter check_min_dist_and_heading_precisely

[cosmicog]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #807)
Primary OS tested on	(Ubuntu 24.04, Jazzy)
Robotic platform tested on	(custom robot, gazebo sim)
Tested with	360 lidar, diff drive, localization, mapping

---

I'm not sure about the 10% correction error parts, removed it while testing to see if the updates work with the given min distance and angle, and then added them back.

[SteveMacenski]

Remove these multipliers Or rather, keep them the same as before

[cosmicog]

Oh, you did put a "👍🏻" on 0.9 here: #545 (comment), so I used it. I thought, the previous 0.8 * min_dist2 was
0.9 * 0.9 = 0.81.

• Okay, for the distance squared, keeping 0.8.
• For the angle, 0.9 or not at all?

In my tests, no multipliers, obviously gave the closest updates according to the given minimum_travel_distance and minimum_travel_heading values. So I was about to push the change with pleasure before you strikethroughed it 🙃

[cosmicog]

Since this fix changes previous behavior, e.g. old test rosbags wouldn't produce the same maps, I think keeping the previous values doesn't make much difference. I even considered adding a new param named allow_pure_rotational_updates with a default value of false, and keeping everything the same as before when it’s set to false.

• People generating new maps with this fix, wouldn't mind (I think)
• People using localization node, would notice a different behaviour, and an occasional small CPU spike, but robots in production usually don't upgrade their packages to avoid these.

I'm new here, wasn't sure if it is really necessary to preserve the previous behaviour. I was also not sure about backporting this to Jazzy or making the change backportable in the first place.

This change is bigger than how it looks. I don't wanna be the person who broke slam_toolbox 🙃 Would implementing the allow_pure_rotational_updates parameter be a better approach?

[SteveMacenski]

Would implementing the allow_pure_rotational_updates parameter be a better approach?

yes please, I think that makes sense. Make sure to document it in the readme's configuration section and I can merge this. I approve of the code, minus that update

[SteveMacenski]

Planning on adding the new parameter?

[cosmicog]

Currently working on it, updated the issue title, because I discovered that the min heading param is actually used only sometimes with the previous magic you did, but that also caused min distance param to behave differently. I also saw that the map wasn't updating after exceeding min dist sometimes.

Now, I created more troubles for myself, even though the previous allow_pure_rotational_updates was approved by you, I changed it to check_minimum_travels_precisely now.

Also started creating it in the wrong place but realised that I could just get the parameter in the slam_toolbox_common.cpp, not Mapper.h:

  /**
  * @note Currently not used in the Mapper class; either of the minimum travel parameters is always
  * checked here before processing a scan. Used by ROS nodes to decide which scans to process.
  *
  * Whether to always check if both MinimumTravelHeading and MinimumTravelDistance are satisfied
  * precisely before processing a scan.
  * When set to false, the MinimumTravelHeading check allows processing a scan only rarely when
  * the ROS nodes consider the MinimumTravelDistance satisfied, but the Mapper class, which uses a
  * stricter tolerance, does not. In such cases, the HasMovedEnough() check approves the update
  * based on the MinimumTravelHeading. This default behavior suits most cases where, for example,
  * rotational odometry is poor.
  * When set to true, scans with pose changes exceeding the MinimumTravelHeading, such as in-place
  * rotational movements, are also forwarded to the mapper to be processed without requiring
  * translational movement to meet the MinimumTravelDistance threshold. In this mode, the ROS
  * nodes' MinimumTravelDistance check always matches the Mapper's strictness.
  * Default value is false.
  */
  Parameter<kt_bool>* m_pCheckMinimumTravelsPrecisely;

Will try to convince you to approve removing the 10% correction multipliers when this param is true.

[cosmicog]

shouldProcessScan() was checking dist2 < 0.8 * min_dist2 and forwarding it to the mapper. And then, HasMovedEnough() was doing

if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)

so sometimes, the difference is approved by SlamToolbox::shouldProcessScan() but not by Mapper::HasMovedEnough(). That causes the min dist param behaving differently part I said above. In the HasMovedEnough(), heading difference is also checked and that's how minimum_travel_heading is used sometimes.

I can see that this magic you did, improves mapping quality, even when users play with min travel params. But I think, when localising, we need the param check_minimum_travels_precisely to make it work predictably without 10% correction.

[cosmicog]

I requested a review but I should add, tested the changes in sim, with Jazzy. Didn't even compile with Rolling, there were some other issues and I just diff-checked the $ git diff outputs with both branches against their pasts to see if there is a typo. Except the new files (multi robot), the differences were just git indexes and line numbers.

I can try to compile, if you have a docker file for testing. But I can't promise a time frame of delivery if you want me to test with Rolling in sim 🙃

[SteveMacenski]

Sorry for the delay, I was at ROSCon and then took my annual post-ROSCon vacation. Back at my desk now. You can just compile in the standard OSRF rolling docker container.

You mentioned that this degraded performance in one of your intermediate comments. Did you resolve that? Obviously reducing performance isn't something I want...

[SteveMacenski]

Other option: Doesn't Karto itself check if enough distance/heading change has occurred? If so, you could have this totally bypassed and use the karto limits from the parameterizations. I think this was done here to get around the 'work' done prior to that check as an optimization.

But I could be remembering wrong, this was almost 10 years ago

[cosmicog]

I guessed the same, asked to myself "why check twice", for an optimization maybe, so I wanted to keep it same.

There are other things being done before checking HasMovedEnough in Karto, I hadn't dug enough to see what are those, how of much this 'work' prior is helpful when the robot hasn't moved enough, so I didn't continue digging.

Adding a new check_if_moved_enough method parameter to Process and ProcessLocalization methods in Karto and not checking HasMovedEnough if this parameter is true would effectively bypass the second check:

Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance, bool check_if_moved_enough = false)

Since checking in the toolbox before Karto, is presumably better for optimisation.

I can add the new toolbox parameter to Karto too, add a commented out serialisation of the parameter like below and just check it instead of using a method parameter.

...
    ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty);
    ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion);
// NOTE: the following two lines are commented out to avoid breaking the serialization of already existing maps
//    ar & BOOST_SERIALIZATION_NVP(m_pMinPassThrough); 
//    ar & BOOST_SERIALIZATION_NVP(m_pOccupancyThreshold);
//    ar & BOOST_SERIALIZATION_NVP(m_pCheckIfMovedEnough); <--- new one

But I would prefer this bypassing operation to be a separate PR since it is a work on another thing which was here before this PR. And also honestly, so I can delay doing it and put it in my TODO list, by opening a shiny issue about it. You can ping me later in that issue.

[cosmicog]

Sorry for the delay, I was at ROSCon and then took my annual post-ROSCon vacation. Back at my desk now. You can just compile in the standard OSRF rolling docker container.

Welcome back, hope you rested well. Currently dealing with it. I had Rolling installed on my machine natively when I wrote this fix and decided not to build it because of the Qt5 errors. Now I’m seeing the same error in the docker image as well. I’m testing with the RViz plugin disabled in CMakeLists.txt, since this PR doesn’t even touch that part. Also had to add back the deprecated (and now removed) geometry_msgs/Pose2D to common_interfaces. Still getting a few errors, but I’ll deliver it with a build test. Maybe it isn’t worth the trouble here, but if I get it running and share the result, it might help you later when working on Lyrical.

You mentioned that this degraded performance in one of your intermediate comments. Did you resolve that? Obviously reducing performance isn't something I want...

The "occasional small CPU spike" I mentioned was about the natural, expected behaviour. Right now, if minimum_travel_distance is 0.5, sometimes the processing happens after moving 0.5 meters, sometimes after 1.5 meters, only when the minimum heading is also satisfied in Karto (as far as I can tell—I haven’t tested it that much in simulation). If this new parameter is true, always processing after exactly 0.5 meters will introduce that small spike, which wasn’t there before, but it’s natural and only applies when the new param is true.

I will answer the review comments now.

[cosmicog]

I was finally able to build it after commenting out slam_toolbox_common.cpp:24:

//#include "rclcpp/rclcpp/qos.hpp"

Ofc this and other changes I mentioned, to be able to build it, are not pushed.

[SteveMacenski]

LGTM - any other adjustments before merge? This does, obviously, need to compile on Rolling though as the ros2 main branch :-)

[cosmicog]

LGTM - any other adjustments before merge?

Nope, will open the same PR for jazzy

This does, obviously, need to compile on Rolling though as the ros2 main branch :-)

I'd open an ✨issue✨ that it currently doesn't 🙃 but that seems unnecessary;

• There is this one for the deprecated geometry_msgs/Pose2D message: ROS2 Rolling pose2d deprecated #811 with its PR already merged.

• I can open another PR to the main branch with the removal of slam_toolbox_common.cpp:24:

  #include "rclcpp/rclcpp/qos.hpp"

  My favourite type, one line change, easier to review.

• About the Qt5, rviz_common issues, I'm afraid I don't have time for them, probably needs upgrading to Qt6. I saw Qt6 in rviz_common's CMakeLists.txt 🙃

• About bypassing the double check problem we talked above, I can open another issue.

edit (this is not related to the compiling, so should've been separated):

About bypassing the double check problem we talked above, I can open another issue. Because it was present before this PR.

[SteveMacenski]

I'm a bit unclear. Are you saying this PR, due to your changes, does not compile? Or that it does not compile in general?

(I really need to setup CI for this repo; I have no idea why I haven't thus far. A bit ridiculous if you ask me ... about me)

[cosmicog]

The main branch was not compiling already, before my changes. To be sure that there is nothing wrong with my changes, I did the things I said above to be able to build. With Jazzy, same changes, no problem at all.

Yeah CI would be great.

[SteveMacenski]

Got it - another problem for another time