Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add preferred lanes CostmapFilter #1522

Closed
2 tasks
AlexeyMerzlyakov opened this issue Feb 14, 2020 · 14 comments
Closed
2 tasks

Add preferred lanes CostmapFilter #1522

AlexeyMerzlyakov opened this issue Feb 14, 2020 · 14 comments

Comments

@AlexeyMerzlyakov
Copy link
Collaborator

AlexeyMerzlyakov commented Feb 14, 2020

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:
    Lanes_U
  • 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
Copy link
Member

SteveMacenski commented Feb 14, 2020

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

@SteveMacenski
Copy link
Member

SteveMacenski commented Feb 14, 2020

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
Copy link

amburkoff commented Feb 27, 2020

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

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 1, 2020

@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
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 7, 2020

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
Copy link
Member

SteveMacenski commented Jul 7, 2020

... 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
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 13, 2020

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):
Lanes

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 13, 2020

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
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 14, 2020

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
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 14, 2020

Closed by mistake. Re-opening.

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 14, 2020

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
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 15, 2020

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
Copy link
Member

SteveMacenski commented Jul 15, 2020

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
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 16, 2020

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.

AlexeyMerzlyakov added a commit to AlexeyMerzlyakov/navigation2 that referenced this issue Jul 20, 2020
AlexeyMerzlyakov added a commit to AlexeyMerzlyakov/navigation2 that referenced this issue Jul 27, 2020
AlexeyMerzlyakov added a commit to AlexeyMerzlyakov/navigation2 that referenced this issue Sep 10, 2020
AlexeyMerzlyakov added a commit to AlexeyMerzlyakov/navigation2 that referenced this issue Sep 28, 2020
SteveMacenski pushed a commit that referenced this issue Sep 29, 2020
* Add costmap filters with KeepoutFilter realization

* Cover preferred lanes in industries by KeepoutFilter (#1522)

* * Resolve review questions
* Add new OccupancyGrid class into nav2_util - API wrapper for original OccupancyGrid
* Add declareParameter() with one argument to Layer API

* Resolve 2nd review questions
* Remove OccupancyGrid class wrapper
* Add Costmap2D constructor with OccupancyGrid instead
* Add launch-file to help running CostmapFilterInfo publisher

* Code clean-up

* Resolve 3rd review questions (partially)

* Fix hidden problems during the shared handling of copstmap_ resource

* Cast into double during the linear conversion of OccupancyGrid data

* Add keepout_filter system test

* Add error message when base or multiplier is not set in ConstmapFilterInfo message

* Add comment to dummy info publisher launch-file

* Add costmap_conversion unit test

* Add declareParameter() unit test

* Add keepout zone entering check in FiltersTester

* Fix upper window boundary calculation in KeepoutFilter

* Correct mutex header in costmap_filters

* Added KeepoutFilter::isActive() method for filter readiness check

* Add KeepoutFilter unit test

* Correct the code to consider node_ as a weak pointer

* Enhance KeepoutFilter unit test coverage by adding different surfaces

* Made CostmapFilters and minor documentation updates

* Fixed typo

* Move CostmapFilterInfo publisher to Map Server

* Add costmap_filter_info.launch.py into tb3_simulation_launch.py

* Remove hard-coded bar areas from test_keepout_filter

* Add CostmapFilters HLD

* Fix doc after review

* Fix mask parameter initialization in bringup_launch.py

* Improve tests and remove costmap-filters from nav2_bringup

* Update map mask to filter mask in HLD

* Small fix-ups

* Fix error message
ruffsl pushed a commit to ruffsl/navigation2 that referenced this issue Jul 2, 2021
* Add costmap filters with KeepoutFilter realization

* Cover preferred lanes in industries by KeepoutFilter (ros-planning#1522)

* * Resolve review questions
* Add new OccupancyGrid class into nav2_util - API wrapper for original OccupancyGrid
* Add declareParameter() with one argument to Layer API

* Resolve 2nd review questions
* Remove OccupancyGrid class wrapper
* Add Costmap2D constructor with OccupancyGrid instead
* Add launch-file to help running CostmapFilterInfo publisher

* Code clean-up

* Resolve 3rd review questions (partially)

* Fix hidden problems during the shared handling of copstmap_ resource

* Cast into double during the linear conversion of OccupancyGrid data

* Add keepout_filter system test

* Add error message when base or multiplier is not set in ConstmapFilterInfo message

* Add comment to dummy info publisher launch-file

* Add costmap_conversion unit test

* Add declareParameter() unit test

* Add keepout zone entering check in FiltersTester

* Fix upper window boundary calculation in KeepoutFilter

* Correct mutex header in costmap_filters

* Added KeepoutFilter::isActive() method for filter readiness check

* Add KeepoutFilter unit test

* Correct the code to consider node_ as a weak pointer

* Enhance KeepoutFilter unit test coverage by adding different surfaces

* Made CostmapFilters and minor documentation updates

* Fixed typo

* Move CostmapFilterInfo publisher to Map Server

* Add costmap_filter_info.launch.py into tb3_simulation_launch.py

* Remove hard-coded bar areas from test_keepout_filter

* Add CostmapFilters HLD

* Fix doc after review

* Fix mask parameter initialization in bringup_launch.py

* Improve tests and remove costmap-filters from nav2_bringup

* Update map mask to filter mask in HLD

* Small fix-ups

* Fix error message
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants