[nav2_map_server] add launcher for map_server

[adivardi]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	-
Primary OS tested on	Ubuntu
Robotic platform tested on	Simulation with rviz
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

This PR fixes a bug when loading .pgm maps in the trinary mode. In this mode, the occupied_thresh and free_thresh specified in the .yaml file are meaningless, since the values of the image pixels are constants determined when the map was saved. Thus the threshold used when loading should correspond to these pixels values.
Without this fix, a map whose free_thresh is above 0.196 will show all UNKNOWN pixels as FREE.

Example using nav2_bringup/.../depot.yaml:

Without the fix:

With the fix:

negated:

Also added a standalone launcher for the map_server so it can be tested

Description of documentation updates required from your changes

None?

Description of how this change was tested

• Visual test using the launcher
• Performed linting validation using colcon test

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[SteveMacenski]

@vickzk can you review? This seems like it might fix a gap in your previous PR and I want to make sure you agree with it

[SteveMacenski]

Remove and use load_parameters.occupied_thresh directly please - we do so lots of places in the code below so changing just the one instance below this isn't consistent

[SteveMacenski]

I dont' see this as consistent with the previous behavior https://github.com/ros-navigation/navigation2/blob/jazzy/nav2_map_server/src/map_io.cpp#L254-L260

[adivardi]

The behavior in Jazzy has the same issue. I was planning to fix it for Jazzy, even before the new Eigen PR

[SteveMacenski]

Can you point to where this is in ROS 1 then? I'm looking for validation that this change is needed. I know we made some changes here from ROS 1 to ROS 2, so if its in ROS 1 and I can map that change as clearly missing in ROS 2, then that makes my approval path here easier :-)

[adivardi]

Loading: https://github.com/ros-planning/navigation/blob/noetic-devel/map_server/src/image_loader.cpp
Saving pixels: https://github.com/ros-planning/navigation/blob/noetic-devel/map_server/src/map_saver.cpp#L77-L82
Saving yaml file: https://github.com/ros-planning/navigation/blob/noetic-devel/map_server/src/map_saver.cpp#L96-L116

In ROS1 they use the 2nd option I mentioned, i.e. always saving occupied_thresh: 0.65, free_thresh: 0.196 regardless of the thresholds used to threshold the map (given by the user with --occ and --free

[SteveMacenski]

In ROS 2 we use the thresholds the user set already though https://github.com/ros-navigation/navigation2/blob/main/nav2_map_server/src/map_saver/map_saver.cpp

[adivardi]

Yes, but those are used to threshold the probability map. They are also used in ROS 1 here
But ROS1 doesn't save these to file, instead always saving 0.196 & 0.65.

[SteveMacenski]

But isn't that supposed to be represented in the thresholds in the yaml file? That is kind of the point of the yaml file.

I don't know about occupied is always 1.0 and free is always 0.039. Where is that coming from?

[adivardi]

I am assuming the images used were saved by the map_saver, not by hand. If so, the image created can only have 3 pixel values (0 205 254), from here
When loading, the values are normalized and complemented to 1 (I think by Magick). So you can only have the 3 values in the comment.

Regarding thresholds: the yaml file now records the thresholds used when saving the file. Those still make sense if the map being saved has graduation and are used to threshold it. Only after the shareholding to Trinary (so when loading), they become meaningless.
Another option could be to overwrite the th thresholds when saving a trinary map. I like this idea less because if someone modifies the yaml file by hand it will break again. Also, specifying thresholds values when saving a map and the discovering different values in the yaml file would look like a bug.

[SteveMacenski]

I don't think the assumption that it is always the map saver is sound, unfortunately. Others have their own map saving utilities so we should look at the yaml values stored (which should correspond properly to those 3 values, right?)

[adivardi]

Right now the map_saver will save to yaml the thresholds used to threshold the probability map being saved.
These are not correct when loading Trinary map.
For example, if you use the default map_saver thresholds of free=0.25, occupied=0.65, it will save a nice map properly. But when loading later on, it won't work since the UNKNOWN value is 0.19 and doesn't lie between the 2 thresholds.

If that assumption is wrong, so maybe the other option is better? When saving with the map_saver, save the thresholds for loading instead of the thresholds for saving. Like I mentioned, it will break if someone change the file by hand and may also be a bit confusing, but imo still better than nothing.

[SteveMacenski]

Why is it not correct? Aren't these all user defined settings they can set to their needs in Nav2?

I'm not trying to be obtuse here, but I'm also very cautious about any changes to map server or AMCL since these are mega-legacy packages that have alot of annoying subtle implications so I prefer to not touch them or their behavior unless we have extremely good reason. I don't really understand the issue.

[adivardi]

What I mean by not correct is:
Say you have a probability map. You want to threshold it to a Trinary map, so you choose 2 thresholds. These 2 thresholds really depend on the probability map (e.g. how it was created and how noisy it is). So it makes sense to let the user choose these thresholds (which he can now - all good).

Once the map is saved and you load it back, it won't load correctly - all the UNKNOWN values will become FREE. This is what I mean by "not correct".

[vickzk]

@vickzk can you review? This seems like it might fix a gap in your previous PR and I want to make sure you agree with it

Sure. I'll check.

[vickzk]

Suggest passing use_sim_time as a launch argument

[vickzk]

@adivardi @SteveMacenski

Right now, when users don’t provide threshold values while saving a map with map_saver, it defaults to a free threshold of 0.25. For trinary maps, it would make more sense to use 0.196 instead, since that’s a more appropriate default for this mode. So, we can update this to use 0.196 when saving in trinary mode.
Note: Users can also achieve this manually by setting the free_thresh_default parameter

If users still want to customize the thresholds for their own needs or when saving from different sources, they absolutely can. The map_server will continue to read the thresholds from the YAML file and use those when publishing the map.

This small change doesn’t affect existing behavior but makes the defaults more aligned with how trinary maps are typically used.

[adivardi]

@adivardi @SteveMacenski

Right now, when users don’t provide threshold values while saving a map with map_saver, it defaults to a free threshold of 0.25. For trinary maps, it would make more sense to use 0.196 instead, since that’s a more appropriate default for this mode. So, we can update this to use 0.196 when saving in trinary mode. Note: Users can also achieve this manually by setting the free_thresh_default parameter

If users still want to customize the thresholds for their own needs or when saving from different sources, they absolutely can. The map_server will continue to read the thresholds from the YAML file and use those when publishing the map.

This small change doesn’t affect existing behavior but makes the defaults more aligned with how trinary maps are typically used.

I think this hides the problem more than solves it.
The threshold values used when saving should be appropriate to the input probability map. If it is very noisy, users may need to increase the threshold. Once they change it, the yaml file will contain their new threshold, and the map won't load correctly anymore. They would need to manually change the yaml file value back to 0.196

[SteveMacenski]

Looking over the code in the PR, I don't know what necessarily the right answer is from this thread, but I know just hardcoding the values as this PR is currently implemented is not something I would be open to merging. It should be configurable, respecting YAML configurations, parameter configurations, or at least something. I would not assume that every map saved in Nav2 is from the map saver as some SLAM algorithms export occupancy grid maps themselves.

Can you adjust the PR to be more configurable while meeting the need / issue you present ?

[adivardi]

How about the ROS1 solution where the yaml values are hard coded when saving a map? (together with the hard coded pixels values)

That will only affect future maps created by the map_saver

[SteveMacenski]

I went back to the base premise of this PR

This PR fixes a bug when loading .pgm maps in the trinary mode. In this mode, the occupied_thresh and free_thresh specified in the .yaml file are meaningless, since the values of the image pixels are constants determined when the map was saved. Thus the threshold used when loading should correspond to these pixels values.

I don't understand how these values are 'meaningless' in Trinary mode. These are set as configurations anyone can set to align with their needs when they save a map from their SLAM vendor. If the normalized value is > 0.65 is occupied or < 0.25 its free, for example. The values in the image are constants, but they can be interpreted differently. A map saved in a continuous probability format like from Cartographer can later be served as trinary where we set thresholds for what we consider on the 0-99% scale to be occupied vs free in this situation.

The 'unknown' in the middle from 0.25-0.65 is a bit odd but also not what that map was saved to do (and makes alot of sense in scale mode). -1 and 255 are intepreted as unknown which is typically what is used.

In short, I'm really not seeing an issue, but if there is an issue, its due to the need for more configurability not less. I wouldn't accept a PR that would hardcode values.

[adivardi]

I think that having the discussion in several comments may have made it quite confusing to follow. I will try communicate my meaning across more clearly:

The issue right now, is that if you use both map_saver and map loader from nav2, you have to manually edit the yaml file for the map to load properly.

I originally assumed that all maps loaded are created by the map_saver. With this assumption, the thresholds are meaningless because pixels can only have 3 values.
When we allow other map sources, this is not true anymore and I agree that the solution implemented in this PR is not good.

Instead, I suggest changing the thresholds in the yaml file when saving using the map_saver. Since the map saver always save pixels with only 3 constant values, the threshold saved could be constants.
This is what was done in ROS1 and would only affect maps saved by the map_saver.

What are your thoughts on this solution? (replacing the current PR change, not in addition)

---

If that's not acceptable, I'm open to other ideas that will remove the manual editing of the yaml file between saving and loading.

[SteveMacenski]

Instead, I suggest changing the thresholds in the yaml file when saving using the map_saver. Since the map saver always save pixels with only 3 constant values, the threshold saved could be constants. This is what was done in ROS1 and would only affect maps saved by the map_saver.

But what about other folks that are saving them as non-Trinary? There are already Trinary values set for saving based on the configured values in the saving service (and cases for each of the others as well). The Map Saver, even if a user does use it, is not static in its values. They are configurations set by the user in the save request. You can adjust these in calling the saving service for whatever is appropriate for your needs.

I don't see any situation in which removing configurability and hardcoding values in IO is the right solution. Maybe you're asking for an update to default values used in the Request fields or not aware you can save with different parameters? But keep in mind that things aren't just Trinary, all settings need to be considered when adjusting defaults.

[adivardi]

I am not sure I follow. The configured values you linked are for the thresholds. I am not suggesting we change these thresholds at all, nor removing their configurability.
I cannot solve the issue by adjusting the thresholds in the request, because this affects the thresholding of the input map. If I always set it below 0.196, then the saved map will be noisier then when using 0.25 for example.

The part which causes the issue, is that the pixel values saved are always hardcoded (lines 542, 544, 546 & 548 in here).

The way this was handled in ROS 1, is that the threshold values saved to yaml were hard-coded, after the thresholding, so the thresholding is not affected and can be configured by the user as now. We can limit this change to saving Trinary map.
This means it should affect only Trinary yaml files saved by the map_saver after the new code is introduced. It won't affect any already existing maps, not any other map format. It also doesn't limit configurability, it simply write to file other values than the values used for thresholding (which are anyway not applicable anymore for Trinary maps as the hard-coded pixels values' are not guaranteed to lie between them)

If we really don't want to hard-code these values, I could think of adding 2 more params for the saved values (so they are de-coupled from the thresholds used for thresholding, only saved to yaml file). But I can't think of any case where it would make sense to change these, since they must lie in between the pixel values, which are hardcoded. It would also break the API since the yaml format will change.

[SteveMacenski]

it simply write to file other values than the values used for thresholding

It looks like ROS 1 deals with this from a parameter for loading to override when desired. I think that makes sense here to do.

I would not accept a PR that adds in hardcoded values generally speaking in the codebase, but configurable values to override the yaml settings when the parameters are set (otherwise use yaml values) seems reasonable and in line with previous behavior in ROS 1.

---

Also, what if negate is set?

[adivardi]

it simply write to file other values than the values used for thresholding

It looks like ROS 1 deals with this from a parameter for loading to override when desired. I think that makes sense here to do.

This is the map loading function. While my PR originally modified this code, I already accepted that modifying the map loading is not a good idea, and suggested that we modify the map saving.

I think that because we are discussing in a PR and not in an issue, we keep referring back to what this PR does. I would like us to forget about it, and concentrate on solving the underlying issue.
So I will first close this PR.

---

Also, what if negate is set?

negate only exist in map loading.

[adivardi]

Now that we forgot about this PR's bad solution 😅 :

I am proposing a similar solution to that of ROS 1, only modifying the saved yaml file, from here, which hard-codes the saved yaml file. However, instead of hard-coding it fully, we will only hardcode occupied_thresh and free_thresh and only if the map_mode is Trinary. negate is already hard-coded in nav2

I know you are against hard-coded values, and I totally get it. However, the saved pixel values are already hard-coded in Trinary mode, so I really do not see why the thresholds should not be as well.

We could of course make the saved thresholds values into prams of the map_saver. That would work. But there would be no reason to modify them (e.g. change the free threshold between 0 and 0.196 won't have any effect, setting it higher than 0.196 will just cause UNKNOWN pixels to become FREE when the yaml+map are loaded)

Do let me know if you prefer moving the discussion to a new issue

[SteveMacenski]

OK - please open a PR for the differed values for the map saver trinary mode. I'm OK with that. Sorry we went the long way around the block on this one 😅

negate is already hard-coded in nav2

That actually is legit to hardcode because the map saver doesn't invert the image pixel values. Its a non-feature of the map saver to support.