Add preferred lanes CostmapFilter

[AlexeyMerzlyakov]

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.

The ticket is a branch from #1263 meta-ticket. It contains all background and work related to "lanes" features only.

Implementation considerations

The suggested implementation implies usage of 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 {0 ... 240} of each byte could be converted to degrees by following rule: 0 - North direction (0 degrees), 60 - East (90 degrees), 120 - South (180 degrees), 180 - West (270 degrees), etc... values above 240 in PGM might 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". However, this idea is not much friendly in terms of UI when making direction maps manually.
• 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 mentioned in Costmap Filters (keep out, preferred lanes, slow/safety zones) #1263, derived from basic CostmapFilters class.
• 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 like presented in the picture below:
  
• 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.

Possible issues:

• Using such a U-shaped approach may cause robot to turn around causing unnecessary jerking/twitching during the path planning when the final goal is in opposite from required on line direction. If robot won't move well, this probably will require some modifications in Controller, which seems not very smooth. So, it is worth to try U-shaped approach at first.

[SteveMacenski]

CC @DLu, who may have some experience or ideas to share, @ruffsl who suggested it in the original discourse thread

[SteveMacenski]

That's an interesting approach, I hadn't thought of something like that. It seems a little non-general but it might function just as well.

If you know the lane boundaries (ei you know the locations of the sides widthwise of the lane) why not draw a straight line of lethal cost across it, lets say, 1 meter behind the robot? That would push the robot forward in the lane without the sides that make up the U-shape.

That seems less optimal but functional once in the lane, but how do you track paths long term through a facility to route through lanes in their proper direction? ie I'm in a warehouse and in order to get from point A to B, I need to go through 4 different lanes and then navigate a little bit to and from them on either end to reach my target positions?

It seems with the directionality aspect, it would require less information embedded in just an image but also work in the planner itself. Something like a sparse graph of nodes from the start of the lane and each of the valid exit locations to create paths to connect to, and then the usual dense plans from A*/Dij. after leaving a lane to get to the positions. The paths inside of the lanes are just straight lines in the direction for the controller to track.

With that suggestion, its lesser a discussion of costmap plugins and more of a discussion on semantic labeling of the map and creating a sparse-graph planner.

I'm not sure if that's in scope for what you want to be working on -- but certainly that will generalize better and deal with the on-ramp and off-ramps with multiple segments + free-space planning.

Let me know your thoughts. If nothing else, I think still implementing a lanes layer without directionality as a costmap layer has alot of value. This is because it can be both for lanes for the robot to follow, but also if the information is encoded not as "free" and "occupied" but with variable cost from 0-254, then we can create suggested paths and let them deviate. Or create areas of higher cost to ideally stay out of, but are not strictly required to. This can be thought of as the keepouts but with variable cost. But perhaps that can be properly represented in the keepouts layer itself.

[amburkoff]

Добрый день, @AlexeyMerzlyakov!
Приглашаю вас выступить с докладом на ROS Meetup 18 апреля в Москве.
https://sberbank-robotics.timepad.ru/event/1271640/ - анонс.
https://habr.com/ru/post/476436/ - видео записи с первого ROS Meetup.

[SteveMacenski]

@AlexeyMerzlyakov what are your thoughts now on the preferred lanes for costmap filters? I think we've decided at most to have it be non-directional, and some discussion on rolling it into another layer with an inverted use.

Do you think we should do that or have a different unique layer for it? I like the idea of a unique layer to highlight it, but if its functionally the same, there's alot of overlap

[AlexeyMerzlyakov]

According to latest discussion in connected #1263, we have agreed that lanes should be preferred and omni-directional. Based on loaded map, LanesFilter plugin should compose Costmap2D with FREE_SPACE on the lanes and inside permitted areas and higher costs rolling into LETHAL_OBSTACLE outside the border of the lanes and permitted areas. The main difference between LanesFilter and KeepoutFilter plugin is this rolling of costmap on lane's boundary. I think, we may try to use keepout filter with applied "InflationLayer" plugin after it. One restriction - is that lane width marked on map in this case should be wider on 2 x inflation_radius than real width we are willing to have in the end.

[SteveMacenski]

... is this rolling of costmap on lane's boundary

What? I don't understand your comment at all, what does rolling costmaps or inflation have to do with anything here?

[AlexeyMerzlyakov]

As we discussed, the lane's boundary could have a cost smoothly increasing from FREE_SPACE up to LETHAL_OBSTACLE to coax the robot into staying on the better path inside the lane, but having some devination. In other words, I mean that cost is rolling from 0 to LETHAL on lane's boundary.

I've draw some picture of how costmap might look for the lanes from #1263 (comment) comment (I am not an artist, too, but hoping this will make a little clearer the meaning):

[SteveMacenski]

Sure - but how does this functionally differ from a inverted keepout map? I think that was the initial question I posed; do we need a specifically separate lanes layer or do we provide instruction for using it both ways?

The gradient, at the end of the day, is up to the application / company to make in the file, so I'm not sure we need to discuss it further than "sure, someone could do that", but if the gradient is in the file, then from our perspective, nothing is different. Read file -> advertise it -> read topic -> set mapped values in internal costmap

[AlexeyMerzlyakov]

Yes, I also agree that this might covered by KeepoutFilter plugin. The gradient on the edge might be drawn by developer, but I rather think, it is better to perform automatically just by applying an inflation_layer Costmap2D plugin going after lanes filter (=keepout filter).

I'll depict it in a new separate "Costmap Filters" section in nav2_costmap_2d/README.md file.

[AlexeyMerzlyakov]

Closed by mistake. Re-opening.

[SteveMacenski]

I disagree with any automatic gradient adding. That should be up to the application designer, mostly because I don't think that gradient is likely all that useful. The issue its trying to solve should be considered more carefully on an application by application basis if you want any deviation, and how much to give (likely far more leeway that isn't easily represented as an automatically generated gradient).

[AlexeyMerzlyakov]

OK, I've added a support of intermediate states from [0..100] OccupancyGrid data range in KeepoutFilter so it should work (previously KeepoutFilter worked only with two discrete states: occupied (=100 in OccupancyGrid data) and free (=0 in OccupancyGrid data) on filter maps).
In this case drawing of gradients or something else will be on developer's side.

[SteveMacenski]

OK. So any reason to have a separate lanes layer at this point or just close this particular ticket and document the lanes usecase of keep out?

[AlexeyMerzlyakov]

I see no other reason to keep this ticket opened futhermore: inversion of input map-filter could be either made by ROS2 application develops, e.g. by using colors inversion in favourite graphics redactor. So, I think, LanesFilter functionality for preferred non-directional lanes is covered by KeepoutFilter plugin.