Costmap Filters (keep out, preferred lanes, slow/safety zones)

[orduno]

Feature request

Feature description

• Enable the capability for setting traffic lanes and/or preferred routes.
• Interface to a UI that allows users to create the traffic flows/lanes, such as this one.

Implementation considerations

It could be implemented using a costmap layer or way-point following #803.

[SteveMacenski]

This is a spiritual duplicate of #404

They're all the same mechanics with different labels

[orduno]

I see a theme around defining navigation constraints (#1263, #404, #401) with the underlying objective of achieving order in the environment.

Maybe we should create a meta-ticket around this theme, and list this and other possibilities.

[SteveMacenski]

I think closing the other 3 and renaming this would be fine.

How I see this: All of these are just costmap filters.

• Costmap Layer
• Reading in from multi-layer map or separate file with filter (so storing map as a png with N channels where N-1 are the filters and 1 is the map itself, or map png, and new png files for each of the filters). Depends if you like having the map separate from the filters or not. I'd go with separate.
• The filters are loaded with some annotation or have that annotation embedded with an enum (load my_map_keepout.png with the keepout type, load my_map_lanes.png with the lane type, ...) and some cost is related to the filter mask. For the case of slow down, when entering it sends a signal to dwb to scale by a certain amount or we change dwb to ask the costmap each update cycle for a scale

[orduno]

I agree with the costmap approach for defining zones. In the case of the keep-out zones, it's straight forward enforcing it, but slow zones might require some though -- scaling is one option, but dwb's trajectory generator could also have a max speed parameter.

Something to keep in mind is that while the values costmap values for these zones might not change, the ones for traffic lanes depends on the robot's position.

Also, I see some users might still prefer to define routes rather than lanes, in which case way-point following could help.

[SteveMacenski]

ut slow zones might require some though -- scaling is one option, but dwb's trajectory generator could also have a max speed parameter.

Scaling parameter is largely static. I think the 2 best routes:

• expose a service in the nav2_controller called /controller_scale and the plugin broadcasts a scale depending on value in the position of the costmap, costmap layers get the robot's position for free.
• expose a new API / layer type CostmapFilters in costmap 2D that has a method getSpeedScale() that will return a scale for the caller to use. Since the costmaps are inside the controller, easy to apply the scale.

I think the traffic lanes are just like the keep outs, its just that instead of applying LETHAL cost across the entire area to avoid, you just make it HIGH cost such that it will not plan there if given other options, and will return as soon as reasonably possible. The general usecase is basically a magnetic tape follower except you want it to leave that lane when required. Those are usually infrastructure based and not changing depending on a particular starting/goal pose

[mkhansenbot]

I would prefer that there is a meta-issue for supporting this category of features, then a list of the individual feature issues (like #401, #404) within that meta-issue.

I think that while there is some overlap in these conceptually and even in the implementation, I would NOT want someone to try to solve all of them at once in some very large PR. I would prefer to see this done incrementally. We could use the meta-ticket to discuss the overall strategy / design and then check off the other tickets one by one.

[SteveMacenski]

I would prefer that there is a meta-issue

Fair enough. When I think of a meta-ticket I think of a ticket encompassing a bunch of tasks to check off and links to closed tickets that were rescoped within it. If you prefer to have the tickets still open, that's totally valid. I generally close them so that someone knows that there's a metaticket covering them, with the ticket open my fear is someone just goes after that ticket in isolation since its open rather than looking at the encompassing theme.

I would NOT want someone to try to solve all of them at once in some very large PR

I... somewhat disagree? I think they should be solved all at once (ei make a design that supports them all and their extensions, and a PR with the first one) but then submit PRs for the individual application differential files independently.

I think we're on the same page just different styles. And that's OK 😄

[AlexeyMerzlyakov]

As far as I understand there are three directions in this task:

1. keep-out zones
2. slow/safety zones
3. preferred lanes

I am suggesting to create three Costmap2D plugins linked with local costmap. It should be enough to use them in Controller only without involvement in Planner Server.
There to be one plugin per each sub-task:

• KeepoutLayer will just mark keep-out zones with LETHAL_OBSTACLE won't allowing robot to move there.
• SlowdownLayer will send a "/controller_server/set_parameters" signal with setting "max_vel*" parameters to Controller Server causing it to restrict/return back the maximum speed of robot each time when it enters/leaves safety zone.
• DirectedLanesLayer will dynamically form and move something like U-shaped figure with costmap gradient in required direction inside and bordered by LETHAL_OBSTACLE values. This approach might have one drawback: robot may try to turn around causing unnecessary jerking/twitching. If robot won't move well, this probably will require some modifications in Controller, which seems not very smooth. But here I can not say for sure until will try. So, it is worth to check.
  Please note that I did not take into consideration non-directed preferred lanes. For this task it should be added entering and exiting gates from lane to normal map. This will cause much more re-work placed apart from overall design.

Next part - is a map_server.
There should be added three types of layers (maps). These layers will be linked with main map in *.yaml-file. The design idea to add a loader for each layer as follows:

• KeepoutLoader:
  Input file - .png/.jpg/.pgm standard map.
  Output topic - OccupancyGrid messages.

• SlowdownLoader:
  Input file should contain the maximum speed value. It might be a PGM-file with encoded areas, like area No.1, area No.2, etc.. Each area could have its own speed limit, specified in connected description yaml-file. E.g. let be main map "map.yaml" and "map.pgm" with the "safe_map.pgm" slowdown layer and "safe_map.yaml" description file. Speed descriptions could be placed into main "map.yaml" file instead.
  Output topic - new type of messages, called SpeedGrid. Something like:

  std_msgs/Header header
  nav_msgs/MapMetaData info
  float32[] data

• DirectedLanesLoader:
  Input file should contain encoded direction. It might be a PNG-file encoded by color like attached RB-colorspace
  
  or by anything else (e.g. direction degree is coded as 3-byte float of consequent bytes of RGB (0x1345FE) but this is less representative in terms of UI).
  Output topic - new type of messages, called DirectionGrid. Like:

  std_msgs/Header header
  nav_msgs/MapMetaData info
  float32[] data

NOTE1: In this case SpeedGrid and DirectionGrid could be merged, since technologically they are the same type of structure. But according to the meaning of the data inside, they are two completely different entities.
NOTE2: Also, I am not finally sure about input file formats for SlowdownLoader and DirectLanesLoader.

Do you have any objections or comments about the design?

[SteveMacenski]

I am suggesting to create three Costmap2D plugins linked with local costmap. It should be enough to use them in Controller only without involvement in Planner Server.

I think it should be the opposite. I think only the global costmap makes sense for most of these, since there is going to be a globally defined set of maps for each of those tasks. Also for the keep out zones, it is necessary for the planner to have that information so that it is unable to path plan through those areas. Additionally, the safety zones need to be aware of the location in the global map frame. The local costmaps operate in the odom frame.

For your 3 plugins: Are you sure they need to be unique plugins? I seems like there could be a single general plugin for CostmapFilters that it itself takes in an plugin algorithm of how to parse a file for its contextual information. For example, the CostmapFilter plugin takes in file keep_out_map and algorithm plugin keep_out. The keep_out_map is read in on configuration, and on activation, the keep_out algorithm will take that data and provide an internal costmap for that plugin instance.

A similar approach I think would also work if we had a base CostmapFilters class that those 3 plugins derived from. There's going to be alot of overlap in how they work, I just want to find a way to minimize the amount of boilerplate needed for updates. At the end of the day, each of these will:

• Load a map (or have a map server do it)
• Have an algorithm for how to use that map's information
• Have an action based on that information.

Ex. keepout:

• Load map
• If values > threshold, cell should be marked LETHAL in local costmap structure
• When an update is called, we update the bounded area with those LETHALs

Ex. SlowZone:

• Load map
• Values 0-100 for percentange of maximum speed to use
• When an update is called, we get the robot's location, and if different % zone than before, we trigger a parameter update to limit the controllers speed.

For the map loaders, I agree that the loader utilities should be in map_server, but should they just be libraries that we use is costmap_2d or actual servers running as part of the map server? I could see them being header libraries for costmap_2d to use to load information on configuration. I could also see having a separate server running to load the files and serve them to the costmap layers. What do you think?

Assuming we have servers: The first 2 loaders just sound like normal map servers loading PGM/jpeg files. Why not just use multiple instances of the map_server in different namespaces?

How have you thought about doing the preferred lanes with orientation information?

[AlexeyMerzlyakov]

I think only the global costmap makes sense for most of these, since there is going to be a globally defined set of maps for each of those tasks.

Agree, that using only the global costmap, path planner will correctly make a path and robot should avoid restricted areas. But I am not sure how robot will move near them: when controller will not see any obstacle on local map, it could move the robot inside restricted area during maneuvers.

For your 3 plugins: Are you sure they need to be unique plugins? I seems like there could be a single general plugin for CostmapFilters that it itself takes in an plugin algorithm of how to parse a file for its contextual information. For example, the CostmapFilter plugin takes in file keep_out_map and algorithm plugin keep_out. The keep_out_map is read in on configuration, and on activation, the keep_out algorithm will take that data and provide an internal costmap for that plugin instance.

A similar approach I think would also work if we had a base CostmapFilters class that those 3 plugins derived from. There's going to be alot of overlap in how they work, I just want to find a way to minimize the amount of boilerplate needed for updates.

Sounds reasonable. It is no problem to prepare one plugin instead. Also, since one plugin may be named whatever it wants, it could be loaded many times with different parameters and could be used for all three sub-tasks handling by these parameters. One thing I am not fully sure of: is that if to make a basic CostmapFilters class and derive all child classes from it, whether this will cause an additional increase in entities and confusion between two mechanisms: plugins deriving from Layer class and filters derived from derived CostmapFilters? As you mentioned, I also could make just one plugin with many if-statements inside for all keepout/speed/lanes cases. But it less scalable solution than CostmapFilters deriving.

Regarding setting a percent of maximum velocity in slow zones. In case if we will use two different robots with different max_vel parameters at the same time, this will cause different speed limits in one zone. E.g. for one robot the limit could be 0.1 meter/s but for another 2 meters/s. However if this is not a real problem case (I mean, we can use different speed percents per each robot in their configs), this solution is much easier and better than invention of new SpeedGrid messages.

How have you thought about doing the preferred lanes with orientation information?

In order to stick to the plan above and not make new message types, I could suggest to use int8 data to store direction map in OccupancyGrid messages. Direction error in this case will be equal to 360degrees/256 =~ 1.4degrees per a segment. If this error is also acceptable, it could make the PR is much easier than I've suggested in previous post.

Regarding re-using of one type of message and map. Do you think - is it a problem if one OccupancyGrid type will represent different by its meaning entities? In one case - a 2D grid map, in another - % velocity map and in third case - direction map.

For the map loaders, I agree that the loader utilities should be in map_server, but should they just be libraries that we use is costmap_2d or actual servers running as part of the map server? I could see them being header libraries for costmap_2d to use to load information on configuration. I could also see having a separate server running to load the files and serve them to the costmap layers. What do you think?

Frankly speaking I have not meet before any usage of map_server as a loadable shared library. I see, it has such ability: libmap_server_core.so or dll for Windows. But I thought that conceptually map_server is a node supporting a variety of different maps for ROS and having topics and services interfaces. Also, StaticLayer plugin is using a create_subscription to get OccupancyGrid messages. Therefore in my opinion, it is reasonable to use map_server as a node publishing filter map topics for CostmapFilters plugin as well. This architecture might be more modular, for example if we want to replace map_server to something else publishing map and filter topics.

Agree, there could be many instances of map_server nodes running simultaneously. Or I can check the code to add some new functionality into map_server to be able to run two loaders at the same time, why not?

[SteveMacenski]

But I am not sure how robot will move near them: when controller will not see any obstacle on local map, it could move the robot inside restricted area during maneuvers.

That is possible. Some of the plugins could be loaded for both. For instance, the speed zones don't, the keep outs probably do. I've usually worked with robots who's controllers I tune to be exact path followers, but that is atypical. Good catch.

this will cause different speed limits in one zone. E.g. for one robot the limit could be 0.1 meter/s but for another 2 meters/s. However if this is not a real problem case (I mean, we can use different speed percents per each robot in their configs), this solution is much easier and better than invention of new SpeedGrid messages.

I don't think the speed zones should contain the absolute maximum speed (ei 5m/s, 0.5m/s) but rather percentages (10%, 100%) so that they generalize for situations with heterogeneous systems and all share the same set of maps. Does that resolve the question?

On the directionality of lanes: I don't really understand your plan there. The others I think if we're not on the same page, then we're getting there. This one I think needs more specific and explicit descriptions from file -> lanes that vary on direction. I thought we'd talked about doing lanes without directional information to start and then building up to that. If you've found a way to handle it on the first try, power to you, but I don't understand. I'm less concerned by how the information is encoded and more concerned about how to make it work reliably/scalably. This one may be an example of a situation where we may need a unique map loader or map encoding method.

I have not meet before any usage of map_server as a loadable shared library.

I'm not sure that the argument of "no one else has done it yet" is strong. If we can make a library that handles loading maps into an object, ei auto occ_grid = map_server::getMap(name), why not use it? The rationale for the map_server to be separate is that the map isn't only used by the costmaps, so it shouldn't belong to it. These will only belong to the costmap. We should publish them latched for debug, but I don't expect other users of it. I could get behind multiple map servers, that just adds more nodes on the network which has performance implications. I suppose on this front, I'm not too picky, but that's the direction I would personally go.

Regarding re-using of one type of message and map. Do you think - is it a problem if one OccupancyGrid type will represent different by its meaning entities? In one case - a 2D grid map, in another - % velocity map and in third case - direction map.

No problem at all. I think that makes our lives considerably easier since we can create then single tools for annotation and debugging that works with all of them (maybe different color schemes at most).

Agree, there could be many instances of map_server nodes running simultaneously. Or I can check the code to add some new functionality into map_server to be able to run two loaders at the same time, why not?

As long as we didn't break that from ROS1 to ROS2, that should just work out of the box without an issue. Almost all the robots I've built exploits this.

Back to the "Multiple plugins vs 1 plugin and subplugins vs 1 base class and many implementations":

whether this will cause an additional increase in entities and confusion between two mechanisms

Take a look at Voxel and Obstacle layers, they derive from each other. That confusion is already there. 1 example is confusing, 2 examples is a model 😆

I also could make just one plugin with many if-statements inside for all keepout/speed/lanes cases

Yeah, don't do that. What you can do instead is to create a plugin for the algorithm to use and load at runtime. No different than how Costmap2D loads costmap layers, but now this layer loads additional plugins. But from the discussion above, I think I prefer to derive from some base class if possible. I think that is less restricting if someone wants to do something weird with this in the future.

[AlexeyMerzlyakov]

On the directionality of lanes: I don't really understand your plan there.

I am talking about directed lanes. There could be direction maps encoded as PGM-files and plugin for Costmsp2D forcing robot to move in specified direction each time when robot enters the directed lane.
The design is just as follows:

Map-server part:

• At the input - PGM-file with encoded lane directions. PGM consists from int8[] bytes. The values in range {1 ... 255} of each byte could be converted to degrees by following rule: 1 - North direction (0 degrees), 65 - East (~90.35 degrees), 129 - South (~180.71 degrees), 192 - West (~269.64 degrees), etc... Zero-value in PGM will be treated as "no lane here".
  Another idea - is to group by 2 bytes sequentially going in PGM into float16[] data. Here we have much more abilities. E.g. values in {0.0 ... 360.0} range may represent direction in degrees, negative values will represent "no lane here".
• Output - just a usual OccupancyGrid map with its data encoded the same rule as above.

Costmap2D plugin part:

• The output map will be read by CostmapFilters plugin and then processed by DirectedLanesFilter class as we agreed, derived from CostmapFilters.
• Each time when robot enters directed lane area DirectedLanesFilter will generate U-shaped figure around the robot in the costmap layer. The borders of U-figure will be a LETHAL_OBSTACLE. Inside U-shaped figure will be a costmap gradient. This should force robot to move in desired direction.
• The U-shaped figure on map will be moving dynamically with the robot on the lane until it will reach the end of directed lane. Thus, I hope, robot will move in required direction once it enters the lane.

Usecase:

• Robot enters a directed lane in any place and start to moving in required direction until the lane will end.

Limitations:

• This approach does not specifying the entering gates. This might cause to be difficult or almost impossible to have two lanes go near each other: robot always will enter nearest lane.
• The omnidirectional or non-directed lanes are out of the scope of this approach. The reason is the same as in previous limitation: it is no mechanism of entering to / exiting from the lane.

I don't think the speed zones should contain the absolute maximum speed (ei 5m/s, 0.5m/s) but rather percentages (10%, 100%) so that they generalize for situations with heterogeneous systems and all share the same set of maps. Does that resolve the question?

I am not finally sure exactly about heterogeneous systems: if the max_vel_percent=10% for all kinds of robots, it will give different absolute values of max_vel_restricted in m/s per each robot. I am just curious because we can compare the situation with speed limits on highways, for example "70 miles/h" for all types of vehicles. Personally, I will prefer the percentage specifying rather than absolute values, since it could be passed through already existing OccupancyGrid structure without introduction something new. But is this an expected behaviour?

Regarding map_server: library or node talks:
As I understand, the situation is - we can use map_server as a node to have a better modularity: the map isn't only used by the costmaps, so it shouldn't belong to it. However it will have performance impact on the whole system when using another instance of map_server running as a separate node only for CostmapFilter plugin. So, you are recommending to use it as a dynamic library. Am I correctly got your point?

On the remaining points I think now we are on the same page:

• Re-using of OccupancyGrid is OK if possible.
• There will be basic CostmapFilter plugin/class with some virtual methods, like processing_algorithm(), costmap_fill() and action() and derived from it KeepoutFilter, SpeedFilter and DirectedLanesFilter child sub-classes having concrete implementations of these methods.
• Plugin will possibly be enabled in global/local costmaps (experiments will finally shown the exact needs).
• We can re-use map_server to for safe-zones maps and probably for speed-limit and direction maps if these maps will technically have the same int8[] data.

[AlexeyMerzlyakov]

Moving lanes-related discussion into separate ticket #1522.

[SteveMacenski]

I think for the naming of things, we can get more pedantic about that in a PR, but the approach sounds reasonable. I think the best option is to create a base class with some of this functionality and then derive from it the specific implementations of methods. I'm imagining the base class taking a map_mask or similar parameter, getting the map from the topic, storing it with get/set functions. Then also implementing the update bounds / costs methods for the costmap_2d layers with the usual boiler plate. Then there's hooks in activate to analyze the maps read in, and in each of the costmap_2d virtual methods as required.

[AlexeyMerzlyakov]

Returning back to zones/lanes activity. I've draw initial high-level design made from our discussion above: CostmapFilter_design_0.1.pdf.

There is a basic CostmapFilter class inherited from CostmapLayer. It is designated to minimize the boilerplate concerned with routine costmap work. There will be three plugins for Costmap2D to be compiled from inherited classes: KeepoutFilter, SpeedFilter and LanesFilter. On first sight, first two filters should be straightforward in implementation. The third one now seems to be some vague. I plan to return back with detailed discussion to it again in #1522. Currently, just a basic concept for it to not miss something important on the start.

Then I suggest to move step-by-step: after design will be agreed, I will switch to implementation of basic CostmalFilter class and first two filters.

Looking forward to any feedback on it.

[SteveMacenski]

So you're suggesting making a plugin interface inside of an implementation of a costmap layer plugin interface? That works. We could also add a plugin interface directly into costmap_2d to do the filters and treat them a little differently than the current layer API. I think both are reasonable.

I think the LanesFilter could use the KeepOutFilter if its truly a binary "you can go here, or not". Another good filter would be a preferred regions filter (something like this), where rather than the Keepout being binary "ok or not ok" this is more of a range of preferred areas, so the embedded information isnt FREE of LETHAL, but anywhere in that range to allow for preferred areas to go, but allow the robot some flexibility to deviate.

I think both could be used to implement lane-like behavior. Any other filters you think would be good to add?

On your API, the loadFilter, what's the input? a string file path? Or do we think the map server will already be launched for this map type so its a topic name? I think a topic makes most sense if we think that its already to be loaded from the semantic map server.

I think process() would be a good one, with the inputs being the robot's position in the pixel coordinates given to it by the base plugin. I think this should be called in updateCosts since that's where the real "work" is done, the updateBounds is just looking for the minimum bounding area for updates so I don't think that's the right spot.

For the speed filter, that process would send a topic with a speed throttling topic or something, so it wouldn't impact the base map (you suggested parameter updates, also reasonable). The lane/keepout/preferred would however need to impact the master grid so I think there would need to be inputs for that pointer and the bounds, really it turns into the updateCosts function pretty quickly for those.

I'm not sure what the action() is intended for, I think you could just have the process() method do the updates to master grid or update parameter itself. Since the filters are passive participants (e.g. they cannot extend the bounds) I don't think any implementation of updateBounds is necessary. It just needs to play with the bounds other sensor data had changed to update accordingly and know the robot's position.

[AlexeyMerzlyakov]

Thank you for the detailed review! I've combined and numbered the themes by their content:

1. Costmap plugin structure

So you're suggesting making a plugin interface inside of an implementation of a costmap layer plugin interface? That works. We could also add a plugin interface directly into costmap_2d to do the filters and treat them a little differently than the current layer API. I think both are reasonable.

Yes, I see the implementation of one basic CostmapFilter class with all Costmap2D-related stuff in it in order to avoid further boilerplate work. And sequentially add derived from CostmapFilter plugins actually having only plugin-related code inside.
About alternative approach: do you mean to change CostmapLayer class by adding new loadFilter() and process() methods into it? If so, I do not think that filters is so big thing that require a Costmap2D API re-work. But I am not sure that's fine understood an alternative method you've mentioned above.

2. API discussion

On your API, the loadFilter, what's the input? a string file path? Or do we think the map server will already be launched for this map type so its a topic name? I think a topic makes most sense if we think that its already to be loaded from the semantic map server.

Since the filters - is feature that is not running on most navigation cases, I personally would not like to prefer to start an additional separate map server process for it. Also for performance reasons. I thought about file path as an input argument (e.g. /file/path/to/filter/map-name within its sematics map-name.XML and actual filter layer map-name.PNG). Filters might directly call MapIO library from map server via API functions. Speed limit structure for SpeedFilter and exits structure for LanesFilter could be obtained from semantic map server with similar API calls or service calls.

However, in dynamic world as we discussed in sematics ticket, map filter and features (e.g. max. speed limit) might be changed during a run-time. In this case filter-map topic + semantic topic seems to be better solution.

I think it depends on one big question: do we put the dynamism into the design or not?

3. Methods naming and code structure

I think process() would be a good one, with the inputs being the robot's position in the pixel coordinates given to it by the base plugin.

Ok, sounds good.

I'm not sure what the action() is intended for, I think you could just have the process() method do the updates to master grid or update parameter itself.

The process() and action() methods are separated only for understanding: process() makes all calcuations and preparations necessary for updating costmap, while the action() makes some follows from process() (e.g. sending a speed restriction limit to a controller server or setting a certain directions to robot wheels also via controller server). Logically, these two methods are made only for separating their intentions but could be easily combined into one process() method. I would rather agree with you here.

I think this should be called in updateCosts since that's where the real "work" is done, the updateBounds is just looking for the minimum bounding area for updates so I don't think that's the right spot.

The lane/keepout/preferred would however need to impact the master grid so I think there would need to be inputs for that pointer and the bounds, really it turns into the updateCosts function pretty quickly for those.

Ok, I agree with your arguments.

Since the filters are passive participants (e.g. they cannot extend the bounds) I don't think any implementation of updateBounds is necessary. It just needs to play with the bounds other sensor data had changed to update accordingly and know the robot's position.

Some simple implementation of updateBounds() might require in order to get and save last robot position for further usage of it in updateCosts() -> process() (e.g. speed filter must know robot position).

4. Speed filter

For the speed filter, that process would send a topic with a speed throttling topic or something, so it wouldn't impact the base map (you suggested parameter updates, also reasonable).

Publishing max.speed topic seems to be a more direct approach, but it requires some support from controller/DWB side (as topic listener), while the parameter updates through a service calls seems to be more tricky but not required any existing controller workout. In terms of performance parameter updates are seems to work faster (parameter update loop seems to be already implemented in a DWB). I also thought that for pointy speed updates service calls fit better than making a permanent topic. So I've selected the second approach.

5. Lanes filter

I think the LanesFilter could use the KeepOutFilter if its truly a binary "you can go here, or not".

Maybe yes, but what is about the case when robot is not on the lane? In this case we need to force it (e.g. by making gradient layer of costmap2d) to move robot to nearest lane. Only after entering lane robot is not allowed to move out from it.
I think, the logic of Lanes and KeepOut filters might have some common, but not be the same.

Another good filter would be a preferred regions filter (something like this: https://github.com/SteveMacenski/weighted_region_layer), where rather than the Keepout being binary "ok or not ok" this is more of a range of preferred areas, so the embedded information isnt FREE of LETHAL, but anywhere in that range to allow for preferred areas to go, but allow the robot some flexibility to deviate.

Great example, that we could align with (if there is no problem with LGPL vs. BSD/Apache licensing). Thanks!

I think both could be used to implement lane-like behavior. Any other filters you think would be good to add?

Hm... Traffic light controlling filter when crossing the lane. Directed lanes/sidewalk ordering. Minimum speed restriction (e.g. when robot is on the lane/highway). Stop for synchronization filter (e.g. before entering crowded area or elevator). Just some brainstorms.

[SteveMacenski]

1. I'm suggesting we could make a new pluginlib CostmapFilter interface into the costmap layers to sit next to the CostmapLayers or create a CostmapFilter plugin-plugin interface as you suggested. I don't care too much one way or another, but something to consider.

2. I think per the existing design, having the map servers running is preferable. We spent a bunch of time refactoring the map server so that we could load multiple instances of it for exactly this purpose. We only want to read these files exactly once so we need to have a server publish to a latched topic in case multiple users want it. When we start merging in the semantic information, what I imagine happening is that the semantic_map_server will load the XML file to find all the filters, then it will use the key-value mapped filepath and topic to load the map file and publish to a topic automatically for the costmap to read in. A very clean solution.

process() makes all calcuations and preparations necessary for updating costmap, while the action() makes some follows from process()

Can you give me an example of preparations that couldn't be done in the main process() method? I don't think we need this two-step update process because these filters should not be impacting the bounds of the update cycle, by nature of being a passive participant to the updateBounds cycle. I think these can just be 1 method, unless there's something you had in mind in particular.

implementation of updateBounds() might require in order to get and save last robot position for further usage

You get the position as an input to process()? UpdateBounds gives a position, you store that and pass it to process() when UpdateCosts rolls back around with the total sized update area. This is a case that you have all the information you need, you should just shuffle it around to make it in the most compact representation for the plugins that will be built on this Filters interface.

4. We would need to enable the reconfiguration of parameters in DWB / TEB. Also document that all controllers are required to expose that parameter and be reconfigurable (and a param in the filter for what the param is on the controller). I'm not sure that any of the controllers have run-time reconfigurable parameters right now.

5. I'm not saying copy paste that work, or that that work is even efficient. I'm just pointing out a potential resource. What is then your suggestion of the lanes that gets over that issue?

[AlexeyMerzlyakov]

1. Costmap plugin structure

I'm suggesting we could make a new pluginlib CostmapFilter interface into the costmap layers to sit next to the CostmapLayers or create a CostmapFilter plugin-plugin interface as you suggested. I don't care too much one way or another, but something to consider.

Great. I've got your point. This also seems to be reasonable approach - new CostmapFilter plugin will be abstracted from Costmap2D classes. On the other hand, if we want to update or change a plugin interface (e.g. in the case when in future it will turn out that some new CostmapFilter plugin cannot be done only by loadFilter() and process() API methods), it will require to change pluginlib API for CostmapFilters which will break compatibility with previous CostmapFilter plugins. So, it seems to be little bit puzzled than just usual class/plugin inheritance.

2. API discussion

OK, let's stick to the topic publishing model for map and features spreading.

3. Methods naming and code structure

Can you give me an example of preparations that couldn't be done in the main process() method? I don't think we need this two-step update process because these filters should not be impacting the bounds of the update cycle, by nature of being a passive participant to the updateBounds cycle. I think these can just be 1 method, unless there's something you had in mind in particular.

There are nothing in my opinion that could principally force a separation. It was supposed only for meaning, but not fundamentally. So, I am OK to leave one process() function instead of two.

You get the position as an input to process()?

I mean robot position could be stored as a protected member of CostmapFilter class during updateBounds() call (when robot position/orientation is available) in order to have an ability to use this information later in a process(). Or we could pass robot position as an input parameter of process().
Both approaches are OK.

4. Speed filter

We would need to enable the reconfiguration of parameters in DWB / TEB. Also document that all controllers are required to expose that parameter and be reconfigurable (and a param in the filter for what the param is on the controller). I'm not sure that any of the controllers have run-time reconfigurable parameters right now.

Yes, it turns out that both features be it topic listener or dynamic parameters update, should be supported from controller plugin side in order to work with costmap filters properly. If you are OK with both approaches, I suppose to remain dymanic parameters update at this stage.

5. Lanes filter

What is then your suggestion of the lanes that gets over that issue?

In my current view, the algorithm might be as follows:
LanesFilter to be a separate costmap filter plugin (not using KeepoutFilter, weighted_region_layer or other plugin).
Plugin inputs might be: filter-map OccupancyGrid topic with lanes marked by pre-defined color and semaptics XML topic having exit gates information.

Robot could be on the lane, or not. If robot is not on the lane, it should be forced to enter the lane as soon as possible (e.g. by applying gradient filters for costmap2d towards to the nearest lane). Robots could enter the lane(s) in any place. Once entered, robot is not allowed to leave the lane until it will reach an exit gate. This might be processed by statically made or dynamically updating costmap around the robot with LETHAL_OBSTACLE out of the lane and FREE_SPACE inside the lane. After robot exists the lane, there are no moving forces applying to it until robot will reach its destination. Just lanes could be rounded by LETHAL_OBSTACLE in order to not allow robot to re-enter the lane again.

Possible shortcomings/issues I see:

• Using only the gradient layer plugins might not be enough to force the robot moving towards to the nearest lane. Here some tricks with giving a direction for controller server might require.
• Not always the robot needs to go on the lane. This might be out of scope of current approach. In worth case, we can invent some flags, given by robot when it "wants" to using lanes.
• Not sure how the problem will be solved if multiple robots are on map: is there the same costmap for all robots, or each one having its own costmap?

It looks like the lanes discussion became to going deep into implementation details. How about to separate it to the #1522 to avoid a confusion?

---

It looks like we are on agreement of most of points. Updating HLD with all information we've discussed here: CostmapFilter_design_0.2.pdf

[AlexeyMerzlyakov]

4. Speed filter

Initially I thought that sending a requests to speed change seemed to be more logical for this kind of task than continuously sending a data through a topic. However, the "safety" argument changed my point of view as most reasonable in favor of using topic. We can imagine an example when a controller did not process an incoming request for some reasons, or even the controller was not online for some reasons/switched on later. When a controller will back online, it immediately will receive a speed restricting info through a topic. (sure we also might organize a request sending loop, but it is better to use already existing mechanisms in ROS when it is more convenient).
So, the topic spreading model is the most safe. I am OK about sending speed restriction through a topic.

For multi-robot case (I mean multi-controllers) I suppose for controller to have an input parameter (like max_speed_topic_name) being configured per each robot to know its required topic.

5. Lanes filter

I think, keep-out plugin will not be suitable for lanes, since it will work only if the robot already is on the lane surrounding by LETHAL_OBSTACLE areas around. But this approach won't allow robot to enter the lane before. Only if there will be bidirectional gates to enter and leave the lane (these gates may be just made as intermittent in LETHAL_OBSTACLE wall).

Using of weighted plugin, with FREE_SPACE on the lane and some costmap higher than 0 (e.g. "100") for out-of-the-lane areas sounds more tempting. But as you previously mentioned non-lethal costs does not guarantee that planner will consider it as encouraging robot to stay on the lane. So, using only this approach I am not sure how it will work robustly.

I also agree, that gradient plugin is not the best way to encourage robot to "gravitate" towards the lane. So, the combination of two plugins, you've mentioned: weighted + keep-out might give a required result. However, the keep-out plugin should be dynamically switched-on when robot enters the lane and switched-off when robot exist the lane. It looks like it is more correct to do it via a separate LanesFilter plugin than making a re-work of Navigation2 plugin loading mechanism to enable dynamically loading-unloading KeepoutFilter plugin.

---

I'm not sure what the "feature_topic" is, can you explain? If that's the speed layer's topic, can that not just be a parameter instead?

map_filter_topic and feature_topic - are std::string names of incoming map filter and sematics data topics accordingly. In this case feature topic may be anything that semantic map server publishes. E.g. for Speed Filter, it could be a decoded structure of speed restriction values; for Lanes Filter - exit gates structure. I am not finally sure, how semantic server will publish this data through a topics. Maybe it will be a decoded key-value structures or something else. Here each filter plugin will define which type of topic it will receive. In any case, if we are are aiming to transferring map and semantics data through a topics, this should be a topic name, I believe.

[SteveMacenski]

4. For multiple-robots, there are multiple controllers per robot so I don't think that's going to be an issue (intuitively, each robot needs their own local costmap and controller so that the rolling window is around the right unit). On the topic update safety, keep in mind that the parameter updates also happen through a topic, its just hidden from your behind some objects, so the mechanics are more or less the same.

5. See image below. Note, this is a 10 minute thought out plan, I'm sure someone doing this professionally could poke some holes in this but for first order approximation. Pencil is the "warehouse" with 3 loading docks, some aisles, a few boxes, and an office. In pen is the lanes marked through a keep out zone. Essentially, the pen is the lanes the robot may traverse and everything is keep out (inverted from probably reality for easier visualization).

As you can see, in the aisles there are thin preferred areas to drive in that are permissible. In practice they might be wider to allow for some deviation, but I'm not an artist. Then in the loading dock region, you note that they're no longer lanes, we now allow the robots to roll around in free space because this is an area where the robots paths might not be well defined because it needs to move some box to 1 of the 3 loading docks. This area might also be the home of the robot charging docks so that on power on, the robot will always be in a valid location.

You see that in this case that there's no situation, except where the robot mistakenly drives out of a lane or becomes delocalized that it would be outside the bounds of the permissible space. The weighted-regions way of doing this (also, please don't use that name, that's a terrible name, I can't name things) would be to have 0 cost in those lanes and then some modest cost outside of it to coax the robot into staying on the better path, maybe with a gradient increasing sharply as the robot deviates more and more from the lanes vornoi diagram or something. That would solve the delocalization jump issue. We could also offer a "turn off" recovery as mentioned above to deal with that situation so the robot could find a way out.

I agree that the issue of "what if robot gets outside of it?" needs to be solved, and maybe that solution involves another layer, but I'm not sure the described layer is that right choice. I think because we're dealing with costmap filters only and we know the semantic information is coming down the line, we don't need to over think this with the gateways. If a person chooses to go this route, they understand that the robot can deviate a bit and that its more "fluid" than a strict navigation graph (e.g. leave when required, if had to leave, then the path will coax it back with higher cost). Maybe there's another way other than those gradients to encourage the robot back onto it if its left and far away. I don't think the gradients will be robust, and if the robot is close to the lane, the lower costs will drive the search based planners into the free space pretty quickly so I don't think it needed any additional help.

feature topic may be anything that semantic map server publishes.

I hadn't thought about that. OK. I had thought that the layer itself would know how to decode it (e.g. the speed layer only works with 1 specific encoding, the XYZ layer has ABC encoding). I think that needs a little more clarification to its implementation, but since it borders the semantics work, I won't push too much for it. Just keep it in the back of your mind and try to formulate some structured way of transmitting semantic information and the message that could be made for all types of masks. Maybe along the way we find we'd rather support 1 specific encoding per filter and it becomes redundant. It depends on how much variety might be had in encoding.

[AlexeyMerzlyakov]

Great. It seems we have fixed on chapter No.4 to use topics for max_speed. BTW, I really even did not guess that parameters update service calls are spread through a topic. Thanks for letting me know. Such portions of information constitute the deeper understanding of ROS2 mechanisms.

Well, it is remained to discuss the item No.5: Lanes Filter.
Thank you for the explanation drawn! This means, that in approach you suggesting the robot never allowed to leave the permitted area or lanes. It fundamentally changes the situation. In this case "loading docks" and lanes are the same for costmap representation having 0 cost in these areas. From this, it turns out that LanesFilter is just a KeepoutFilter (or its inverted state) with some returning force on its bounds (e.g. higher costmaps outside the bounds rolling into LETHAL_OLBSTACLE when robot is far away from permitted area). BTW, it seems that inflation_layer applied after LanesFilter will give the same result.

If I correctly understood the situation and we do not need to make an exit gates and robot never will leave permitted areas in terms of this task, we can move on. I will update the design with v0.3 and continue the work (I already have some working very-basic prototype for KeepoutFilter on synthetics).

Just keep it in the back of your mind and try to formulate some structured way of transmitting semantic information and the message that could be made for all types of masks.

• My vision of SpeedFilter information is presented in the end of Semantic labeling in maps #1595 (comment)
• For KeepoutFilter it is not required to have any than filter map info from semantics map server.
• LanesFilter as we discussed - be something like KeepoutFilter. If there is no exitting gates, here also no additional information is required from semantics map server.

[SteveMacenski]

This means, that in approach you suggesting the robot never allowed to leave the permitted area or lanes.

That might not be the best way to do things, but just my proposal of how you could do the lanes without a unique layer. So I'd ask that you adjust your proposal in light of that for what a Lanes layer might look like and how it differentiates from the keepout or weighted layers.

Either way, the keepout, speed, and weighted (or just a non-trinary keepout mode?) can be started, we agree on that stuff. We can keep chatting about the lanes stuff while that is being completed.

[AlexeyMerzlyakov]

Updated HLD to next version by comments from above: CostmapFilter_design_0.3.pdf

[SteveMacenski]

Slide 2 shows a single map_filter and features topics, if you have multiple of these filters, how do those change? I think maybe those arrows should be message types and have the topics either remappable or reconfigurable. That's mostly just for the diagram, no change on the proposal.

I think you will use the bounds in the updateCosts given to you, you don't show it in that diagram, but assuming you just didn't include it for brevity, that's OK. Both the process and updateCosts will need to interact with these bounds so that we only update a certain set of bounds for instance for a rolling costmap keep out layer (while I don't suggest anyone to use the keepout or lanes layers in the controller rolling costmaps, we can easily enable it).

I'm not sure I agree with the messages (e.g. if you have a non-trinary loaded keepout / speed layer, those messages should just be the value in costmap -> value in speed or percentage mapping, I don't think you need area IDs or any of that stuff. Really, for basic 0-100% cases, you can embed 0-100 in the occupancy grid directly and there's no remapping. For 0 m/s-N m/s, there may be, but its just a linear mapping of ranges from 0-255 or 0-100 to the speed ranges available to the robot. Personally, I think the best solution is 0-100% because its simpler, doesn't require complex encoding, and the designer manually creating these mask files knows the robot's top speeds and can pretty easily divide 2 numbers to make it into a percentage)

[AlexeyMerzlyakov]

Slide 2 shows a single map_filter and features topics, if you have multiple of these filters, how do those change?

Agree. This is incorrectly formed slide, I will update in next version with topic descriptions and types instead. Thanks for noting this.

I think you will use the bounds in the updateCosts given to you, you don't show it in that diagram, but assuming you just didn't include it for brevity, that's OK.

Yes, I've hidden the costmap window in updateCosts() for brevity. If you are OK about this, I won't include it since this is implied to be used by updateCosts() and process(), I think.
By the way, currently I am making the working prototype of KeepoutFilter and discovered that one more CostmapFilter API method is required: resetFilter(). This method is required to be called from inside reset() routine in order to stop Filter's specific subscriptions. This also will be included into next version of HLD.

Really, for basic 0-100% cases, you can embed 0-100 in the occupancy grid directly and there's no remapping

Agree, this is the most simple approach and it will avoid to have some dances with regions marking for developers using SpeedFilter. From other side we have one drawback of percentage model: drawn in % map_filter.pgm can not be re-used for another robot:

I am not finally sure exactly about heterogeneous systems: if the max_vel_percent=10% for all kinds of robots, it will give different absolute values of max_vel_restricted in m/s per each robot. I am just curious because we can compare the situation with speed limits on highways, for example "70 miles/h" for all types of vehicles. Personally, I will prefer the percentage specifying rather than absolute values, since it could be passed through already existing OccupancyGrid structure without introduction something new. But is this an expected behaviour?

If you are OK about it, I will update the proposal with percentage using model and remove from the proposal nav2_msgs/msg/SpeedLimits messages.

[SteveMacenski]

OK.

OK. Why the reset - what's the unsubscribe-resubscribe do for this node? I'm not sure that's required. I think that's more an issue with data streams in case there's an issue rather than map servers. We can add it, I don't mind, but seems like it might not be required. Reset flexibility for designers might be nice though.

I think for the most part people are working with homogeneous fleets, but that is a good point for heterogeneous fleets. But if you had 2 types of robots in the same environment, wouldn't you think that the controller max speed would both be set as the same for consistency? In that case, the % would be the same. It would be great to support the absolute speeds too. The point I was making in that comment is that I think your filter speed message is overly complex. It just needs to be a linear map of costmap values to [something] (%, speed, etc). If we want to support both then we need to have a message to enable that. Ideally, the mapping message is consistent for all types, not specialized for speed limit or keep out or something - but a single message they all use (if possible)

[AlexeyMerzlyakov]

Why the reset - what's the unsubscribe-resubscribe do for this node? I'm not sure that's required. I think that's more an issue with data streams in case there's an issue rather than map servers.

I considering to have some filter unloading or filter resetting function for symmetry to loadFilter(). I think, there should be a mechanism of stopping filter work/subscriptions/publishers/etc... This is related to case when Layer::deactivate() or Layer::reset() is called.

Regarding a % speed restrictions, OK, we can stop on it until it will be required absolute speed restriction values in the system to be added. In this case, overall picture became to look much more simple - there is no need in any information from semantic map server for today: all 3 Filter plugins might work using only a OccupancyGrid messages.

[SteveMacenski]

Lets add the reset then.

I think you misinterpreted me. I'm saying we should support absolute speed, but we shouldn't have a specific SpeedLimit message, we should have a SemanticEmbeddedInfoMap (whatever, some name) message that all masks that the CostmapFilters use translates the 0-255 of the occ grid into some other number space. Then it can be used for the speed limit layers (1:1 map for %, actual masked values for absolute speed) as well as other things like keep out (different mapping of cost) or other things. That way the inputs to all CostmapFilters are the same: 2 topics occpuancy grid and this standard number-space-remapping message.

For simple prototyping, we can just do the % for now, but we should enable this before too long

[AlexeyMerzlyakov]

Thank you for the OccGrid data -> space conversion idea! This is pretty universal, I think. I've updated HLD to the next version CostmapFilter_design_0.4.pdf with all changes we've discussed.

[SteveMacenski]

For the space converter, I think what you want to set up is some linear mapping.

If the occupancy grid information can be stored as 0-255 (and remember, it could actually be anything as an image, but the occupancy grid message only understands 0-255), then I think the 2 parameters you need is an offset and a scale, ex. y = 1.5 x + 32. You need both the scale multiplier for scaling 0-255 into the new number space and an offset in case you need to remap 0-255 to N-M where N != 0.

You may also not really need the type - what do you imagine that being used for? What I think might actually be useful is to have that have a topic name for the layer's occupancy grid. E.g. the costmap filters will only know about this metadata topic, and it will use the topic embedded in it to subscribe to the map. That reduces the number of parameters for each filter to 1 for metadata instead of 2 (metadata + map). Also lets the semantic information set the topic in the semantics.xml file.

If we can show that this space conversion is general, we could also just make a custom message that contains the metadata + map into 1 message.

[AlexeyMerzlyakov]

I think the 2 parameters you need is an offset and a scale

Yes, I forgot to add a offset base to multiplier to have a complete linear transformation. Thanks for nothing!

You may also not really need the type - what do you imagine that being used for?

The type field is required for filter plugin to understand - what does it work with. In particular, speed filter should know which scale (percentage or absolute values) should it read from OccupancyGrid filter layer topic and which type of speed restriction (% or abs.) should it publish to MaxSpeed.msg for controller (controller initially also does not know anything about speed restriction type).

Another option if you want to avoid a type field in topic messages - to set type value as ROS2 node parameter for speed filter only. I think, this will also work.

What I think might actually be useful is to have that have a topic name for the layer's occupancy grid. E.g. the costmap filters will only know about this metadata topic, and it will use the topic embedded in it to subscribe to the map. That reduces the number of parameters for each filter to 1 for metadata instead of 2 (metadata + map).

Agree. Since each semantic map info is connected with its own map filter topic, filter map topic name could be specified in semantic messages as well. In this case we need to rename FilterSpaceConverter.msg to something like FilterSemanticInfo.msg.

If we can show that this space conversion is general, we could also just make a custom message that contains the metadata + map into 1 message.

Initially, linear transformation was thought to be used only in costmap filters. If think general, it might be useful if we want to transform the OccupancyGrid into some value to be spatially-dependent and linear. But all of this (linear maps complementing main OccupancyGrid-s) one way or another related to filters. So, I find this to be quite universal primarily for filters (costmap or non-costmap based). However, I do not know in current navigaiton2 stack any other filter usage other than costmap filters. So, I have some doubts that we really need to extend this model to be universal.

[SteveMacenski]

Ah, so you imagine that the type of filter is defined in the map itself, which makes sense. I figured that in the configuration file for costmap, we would have to add the SpeedLayer KeepoutLayer XYZLayer to the layered costmap, and therefore, they already know what the "type" is, because you just instantiated a SpeedLayer instance with the topic map_server/keepout/map or whatever for the occupancy grid / features topic. I think communicating the type would largely be redundant, but I agree it would be useful for introspection and if other things want to look at all these topics and make some decisions based on it without knowing specifically what plugin its going to. We're in agreement. More independent information is good.

XYZSemanticInfo.msg is a good name. I'm not sure I agree with Filter for XYZ, but I think semantic info is a much better name. Maybe just SemanticInfo? My fear is that Filter isn't descriptive enough or is confusing for a state estimation filter or something.

Not sure I understand your last comment. I was just suggesting that rather than having a OccupancyGrid and a XYZSemanticFilter message to relay the info, we just have 1 SemanticInformation message containing the info and the occ grid both. I don't see too much benefit from having them separated other than being able to more easily use the rviz plugins.

[AlexeyMerzlyakov]

I think SemantiInfo name might be quite universal to use only in costmap filters. Whether this will hinder us in the future to introduce semantic inforamation topic for all maps (OccGrid non-filter maps)? Or do you have some plans to extend this message in the future for all kind of OccupancyGrid maps?

Not sure I understand your last comment. I was just suggesting that rather than having a OccupancyGrid and a XYZSemanticFilter message to relay the info, we just have 1 SemanticInformation message containing the info and the occ grid both. I don't see too much benefit from having them separated other than being able to more easily use the rviz plugins.

Ok, got your point. If encapsulate the OccupancyGrid into semantic info message, how about to call this message something like: CostmapFilterSemanticInfo or even just CostmapFilterMap bacause it is just a map + semantic infomation about it in one topic?

[SteveMacenski]

SemanticInfo would imply all semantic information. This is only for the costmap filters, we'll need to make other custom messages to translate the XML semantic information into ROS messages which are more appropriate to be named SemanticInfo than this.

Lets do 2 messages for right now (semantic and map) so that we can use the default rviz plugins. No need to make more issues for ourselves until after we have things working. Having rviz to visualize will be useful for development and initial testing (especially for maps that are offset origins from each other)

[AlexeyMerzlyakov]

Ok, I would like to agree that on current stage it is better to remain two topics: one for OccupancyGrid map, another for SemanticInfo in order to have less problems with visualization and compatibility.

Updated HLD to v0.5 with comments above: CostmapFilter_design_0.5.pdf

[SteveMacenski]

Looks good, I think the minor things left are name of SemanticInfo and a list of enums for the type unit8 of plugins we support so far / general needs, that can expand over time

[AlexeyMerzlyakov]

Added WIP PR for Costmap Filters currently covering Keep-out zones and Preferred Lanes in industries use-cases. Filter for maximum speed-limiting areas is in progress.
Also, updated HLD to current discussion stage: CostmapFilter_design_0.7.pdf.

[SteveMacenski]

Keepout ones were merged and ready for use, documentation pending

[AlexeyMerzlyakov]

Now, I am making the tests for SpeedFilter (SpeedFilter itself is ready for use). The question is about terminology:
Currently the topic name and everything around it is called as "MaxSpeed" (nav2_msgs/msg/MaxSpeed.msg). In DWB, max_speed name is already used as parameter name for KinematicsHandler. So, in order to avoid a confusion, I suggest to rename MaxSpeed in our terminology to SpeedLimit value (expressed in a % from DWB max_speed or in absolute values). @SteveMacenski, please let me know, if you are OK with it.

[SteveMacenski]

Sure, works for me.

[SteveMacenski]

Speed filter merging imminent, closing out ticket

[Pran-Seven]

Is something similar available for ROS1? I am looking for a function to pass coordinates and have them represented as keep out zones in a layer of the costmap?