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

Costmap Filters #1882

Merged
merged 32 commits into from Sep 29, 2020
Merged

Conversation

AlexeyMerzlyakov
Copy link
Collaborator

@AlexeyMerzlyakov AlexeyMerzlyakov commented Jul 20, 2020

Codemap Filters feature PR for keep-out zones, speed-limit areas and preferred lanes use-cases.


Basic Info

Info Please fill out this column
Ticket(s) this addresses #1263, #1522
Primary OS tested on Ubuntu 20.04 with ROS2 Foxy onboard
Robotic platform tested on TB3 Gazebo simulation

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 20, 2020

@AlexeyMerzlyakov pull in master or rebase. You're missing a number of necessary updates to run CI

Please also go back to the PR template and properly fill it out. You most certainly have documentation and future work to discuss.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Good first draft - some issues to resolve though

nav2_costmap_2d/CMakeLists.txt Outdated Show resolved Hide resolved
nav2_costmap_2d/CMakeLists.txt Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Outdated Show resolved Hide resolved
@AlexeyMerzlyakov
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 27, 2020

Thank you very much for detailed review! I've re-based the branch over master and added one fcf3375 commit resolving review questions.

@codecov
Copy link

codecov bot commented Jul 27, 2020

Codecov Report

Merging #1882 into main will increase coverage by 0.18%.
The diff coverage is 92.38%.

Impacted file tree graph

@@            Coverage Diff             @@
##             main    #1882      +/-   ##
==========================================
+ Coverage   79.23%   79.42%   +0.18%     
==========================================
  Files         221      226       +5     
  Lines       10620    10809     +189     
==========================================
+ Hits         8415     8585     +170     
- Misses       2205     2224      +19     
Flag Coverage Δ
#project 79.42% <92.38%> (+0.18%) ⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files Coverage Δ
..._costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp 100.00% <ø> (ø)
nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp 100.00% <ø> (+20.00%) ⬆️
nav2_map_server/src/map_io.cpp 86.85% <75.00%> (ø)
...tmap_2d/plugins/costmap_filters/costmap_filter.cpp 75.75% <75.75%> (ø)
nav2_costmap_2d/src/layer.cpp 77.27% <85.71%> (+1.59%) ⬆️
...tmap_2d/plugins/costmap_filters/keepout_filter.cpp 95.40% <95.40%> (ø)
...nav2_costmap_2d/costmap_filters/costmap_filter.hpp 100.00% <100.00%> (ø)
nav2_costmap_2d/src/costmap_2d.cpp 79.37% <100.00%> (+1.48%) ⬆️
...costmap_filter_info/costmap_filter_info_server.cpp 100.00% <100.00%> (ø)
nav2_map_server/src/costmap_filter_info/main.cpp 100.00% <100.00%> (ø)
... and 14 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 369e62d...19b3e11. Read the comment docs.

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 27, 2020

Please test as well that this works for both rolling and non-rolling costmaps (and some screen grabs would be great)

@AlexeyMerzlyakov
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 30, 2020

Made an update resolving second review questions: cfeeb1a.
There is a screenshots for Keep-out Filter testing when it enabled in global planner:
keep-out_global

And in local as well:
keep-out_local

In both cases map mask has different from basic map origin, scale and size. As seen from pictures above, both cases works well.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Getting alot closer. Big items now:

  • Test coverage / testing
  • Documentation
  • How to get users to be able to use this once merged

nav2_costmap_2d/CMakeLists.txt Show resolved Hide resolved
nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/src/costmap_2d.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/src/costmap_2d.cpp Show resolved Hide resolved
nav2_costmap_2d/src/costmap_2d.cpp Show resolved Hide resolved
@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 30, 2020

Those gifs will be great for documentation. Though we might want to find a more compelling map to show it in so that its clear the uses to actually block out a region / mark a lane. Thanks for sharing them!

There appears to be a problem in your rolling costmap gif, there's a clear offset between the map and the marked areas that are highlighted after the reset. Maybe its that +0.5 you add? Some aliasing is to be expected, but that's a pretty consistent offset over all the regions

@AlexeyMerzlyakov
Copy link
Collaborator Author

AlexeyMerzlyakov commented Jul 31, 2020

I've added a commit resolving small questions in latest review. There are remaining 3 big items: documentation, testing and SpeedFilter to commit. I'll make a further updates on this PR once they will be done.

There appears to be a problem in your rolling costmap gif, there's a clear offset between the map and the marked areas that are highlighted after the reset. Maybe its that +0.5 you add? Some aliasing is to be expected, but that's a pretty consistent offset over all the regions

The +0.5 of cell calculations are made to fit the middle of the (0,0) cell. These are also corresponding with https://github.com/ros-planning/navigation2/blob/master/nav2_costmap_2d/src/costmap_2d.cpp#L222.
Anyway, I've disabled the whole optimization (cfeeb1a#diff-f67a16397bb5979b7cbf4729ea53a451R124-R170) remaining only update loop cycle. However this produces still the same picture when robot in some places on the map.
Also, there was not a reset, just layers re-showing.

I think, this is rather AMCL error, because the same shift is applying on the whole map, not only mask one. The picture below confirms an assumption: all underlined in yellow shifts are looks to be the same size:
amcl_error3

// Linear conversion from OccupancyGrid data range [OCC_GRID_FREE..OCC_GRID_OCCUPIED]
// to costmap data range [FREE_SPACE..LETHAL_OBSTACLE]
costmap_[it] = std::round(
1.0 * data * (LETHAL_OBSTACLE - FREE_SPACE) /
Copy link
Member

@SteveMacenski SteveMacenski Jul 31, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is that 1.0 necessary? If you're trying to cast into doubles, you should do so explicitly with a cast

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 31, 2020

this is rather AMCL error

I'm suspect of that given that its only in 1 direction (X) in your image. There's no error in the other direction and that environment is pretty symmetric, the localization error should be more proportional in both primary axes. If you spawn the robot in a known pose and give it that pose as the initial pose then there should be no error - in that case, do you still see the shift?

I agree though it could be from that; trust but verify

@AlexeyMerzlyakov
Copy link
Collaborator Author

AlexeyMerzlyakov commented Aug 18, 2020

I agree though it could be from that; trust but verify

Totally agree with this. I've disabled the Keepout Filter in a plugins at all and then put robot into known point on map. Then I've manually published initial pose message on that point.
And here what I saw:
Map_shifting_issue
Shift between local map built by laser scan and map is persist, which seem really odd.

I've tried to make my own map of turtle-labyrinth, but results did not changed.
Maybe, I should fill a new issue on this.

Steps to reproduce:

  1. Change initial robot pose on (0.5, 1.5):
--- a/nav2_bringup/bringup/worlds/waffle.model
+++ b/nav2_bringup/bringup/worlds/waffle.model
@@ -51,7 +51,8 @@
     </model>
 
     <model name="turtlebot3_waffle"> 
-      <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose>
+      <pose>0.5 1.5 0.01 0.0 0.0 0.0</pose>
 
       <link name="base_footprint"/>

  1. Start TB3 simulation:
ros2 launch nav2_bringup tb3_simulation_launch.py
  1. Publish geometry_msgs/msg/PoseWithCovarianceStamped to (0.5, 1.5) point and see local map <-> map shift:
ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
{
header: {
  stamp: {
    sec: 1588272932,
    nanosec: 798746418,
    },
  frame_id: map
  },
pose: {
  pose: {
    position: {
      x: 0.5,
      y: 1.5,
      z: 0.0
      },
    orientation: {
      x: 0.0,
      y: 0.0,
      z: 0.0,
      w: 1.0
      }
    },
  covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  }
}
"

@SteveMacenski
Copy link
Member

SteveMacenski commented Aug 18, 2020

Can you show that this aliasing moves as related to localization and not having any static offsets?

@AlexeyMerzlyakov
Copy link
Collaborator Author

AlexeyMerzlyakov commented Aug 19, 2020

Can you show that this aliasing moves as related to localization and not having any static offsets?

This is definitely is not a static offset. I've recorded robot long moving scenario (75 seconds GIF) with Costmap Filters disabled:
sc3

From it (and other scenarios I've not included here, but if you want I can share): in most cases the shift depends on robot position on map. Sometimes, randomly appears, but for most cases, it depends on the position of the robot in space.
I've tried to draw windmap of local costmap <-> map shear (this not always true, but often this way):
directions

@SteveMacenski
Copy link
Member

SteveMacenski commented Aug 19, 2020

@AlexeyMerzlyakov, I understand the rolling costmap doesn't have a static offset. I'm asking about the filter enabled showing that it also "vibrates" similarly to the rolling costmap error to show that it doesn't have a static offset. Since we parse a map and then transform it into a new frame using daisy chained worldToMap -> mapToWorld's in potentially offset frames, I want to know that there's not something wrong with that process in the keepout layer.

You could do that by pretty much the same experiment you showed but with the costmap layer enabled and show that: "in most cases the shift depends on robot position on map", as you mention, and that when the positioning in the map is relatively accurate, that there is no or little aliasing.

We know the rolling costmap probably works, we're trying to validate that the rolling costmap filter given the offset frames and transformations are correct 😄

@AlexeyMerzlyakov AlexeyMerzlyakov changed the title [WIP] Costmap Filters Costmap Filters Sep 9, 2020
@AlexeyMerzlyakov
Copy link
Collaborator Author

AlexeyMerzlyakov commented Sep 10, 2020

I've made next test+doc update. Unfortunately, since master branch was renamed to main I've rebased all changed over this branch and force-pushed the commits.

3 unit tests and 1 system test were made for CostmapFilters/KeepoutZones:

  • nav2_costmap_2d/test/unit/costmap_conversion_test.cpp - unit test for OccupancyGrid -> Costmap2D converstion (new Costmap2D(OccGrid) constructor)
  • nav2_costmap_2d/test/unit/declare_parameter_test.cpp - unit test for Layer::declareParameter() with no default value
  • nav2_costmap_2d/test/unit/keepout_filter_test.cpp - unit test for KeepoutFilter class
  • nav2_system_tests/src/costmap_filters/test_keepout_filter.cpp - system test for checking robot can go though valid plan bypassing all keepout-zones and won't go on invalid plan when goal belongs to keepout-zone.

During unit test writing I've discovered, that some boundary checks in optimizations were incorrect. This was indeed the lost on one cell in upper boundary. Thanks to @SteveMacenski that you insisted to check this! Fixed this issue in 27afdab commit.

Also, documentation update was made for doc/parameters/param_list.md parameters description and nav2_costmap_2d/README.md files. Also, fixed along the way some outdated parameters there.
Documentation update for web-site will be made in a separate PR for https://github.com/ros-planning/navigation.ros.org shortly.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

I fail to see how the tests test anything in costmap 2d. You hardcode the "bar" locations rather than actually using the costmap or costmap filter you actually built. If its a system test, it should be testing the system with a keepout in the global and/or local costmaps and showing the robot path / robot itself doesn't enter a zone in the way. If its just testing the costmap filter plugins themselves, which appears to be the case, then its a costmap_2d integration test. However it looks like you're simplifying the use of the costmap away, so I don't actually see much if any value in those tests.

doc/parameters/param_list.md Show resolved Hide resolved
doc/parameters/param_list.md Show resolved Hide resolved
nav2_costmap_2d/README.md Outdated Show resolved Hide resolved
nav2_costmap_2d/README.md Outdated Show resolved Hide resolved
nav2_costmap_2d/README.md Outdated Show resolved Hide resolved
nav2_system_tests/src/costmap_filters/filters_tester.cpp Outdated Show resolved Hide resolved
@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 14, 2020

How can users use this once merged in? Seems like the semantic info publisher should be a node for actual use until we have other options (or build it into the map server). Or built into the map server? I'm not sure, but this feels like we need to simplify this for people or provide documentation about how to actually set this up for use. The tutorial could be that, but it needs some real clean up.

@AlexeyMerzlyakov
Copy link
Collaborator Author

AlexeyMerzlyakov commented Sep 18, 2020

Test failures appeared because of uninitialized mask parameter in bringup_launch.py. To be fixed shortly.

@AlexeyMerzlyakov
Copy link
Collaborator Author

AlexeyMerzlyakov commented Sep 28, 2020

Good. I've rebased all changes over main branch. Let's look how it finishes.

How did you build that locally? I've never been able to figure that out.

I've enabled GCC coverage in cmake flags, run test locally and then used LCOV to convert all the data produced by GCOV to be web-readable. For costmap_filter_info_server.cpp unit test this was as follows:

1. Build with GCOV enabled
$ cd ~/navigation2_ws/
$ colcon build --symlink-install --packages-select nav2_map_server --cmake-args -DCMAKE_CXX_FLAGS='--coverage' -DCMAKE_C_FLAGS='--coverage'

2. Run testcase manually
$ ./build/nav2_map_server/test/unit/test_costmap_filter_info_server

3. Run LCOV to get html-file
$ lcov --capture --directory build/nav2_map_server/CMakeFiles/map_server_core.dir/src/costmap_filter_info --output-file coverage.info
(in order to get what directory to specify to LCOV I've found required *.gcda file from all file list:
  $ find "build/nav2_costmap_2d/" -name "*.gcda")
$ genhtml coverage.info --output-directory out
$ firefox out/index.html

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 28, 2020

Passes, last thing is just that last open comment #1882 (comment) and we can merge

@SteveMacenski SteveMacenski merged commit 4c8334f into ros-planning:main Sep 29, 2020
4 checks passed
ruffsl pushed a commit to ruffsl/navigation2 that referenced this pull request 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

Successfully merging this pull request may close these issues.

None yet

2 participants