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 in range sensor costmap layer #1888

Merged

Conversation

Michael-Equi
Copy link
Contributor

Basic Info

Info Please fill out this column
Ticket(s) this addresses #1830
Primary OS tested on Ubuntu 20.04
Robotic platform tested on Bytes Robot

Description of contribution in a few bullet points

Added in new costmap2d layer based on the range sensor layer in ROS1.

Description of documentation updates required from your changes

Added parameters and costmap layer to docs/parameters. Added layer info to navigation2.ros.org.


Future work that may be required in bullet points

  • Add unit and integration tests for the range layer

@codecov
Copy link

codecov bot commented Jul 26, 2020

Codecov Report

Merging #1888 into master will increase coverage by 0.22%.
The diff coverage is 79.77%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master    #1888      +/-   ##
==========================================
+ Coverage   75.00%   75.23%   +0.22%     
==========================================
  Files         222      224       +2     
  Lines       10712    10971     +259     
==========================================
+ Hits         8035     8254     +219     
- Misses       2677     2717      +40     
Impacted Files Coverage Δ
nav2_costmap_2d/src/layered_costmap.cpp 88.57% <ø> (ø)
nav2_costmap_2d/plugins/range_sensor_layer.cpp 78.54% <78.54%> (ø)
..._2d/include/nav2_costmap_2d/range_sensor_layer.hpp 100.00% <100.00%> (ø)
nav2_costmap_2d/plugins/inflation_layer.cpp 99.37% <100.00%> (ø)
nav2_costmap_2d/test/testing_helper.hpp 80.00% <100.00%> (+1.42%) ⬆️
nav2_controller/src/nav2_controller.cpp 86.63% <0.00%> (+0.46%) ⬆️
nav2_costmap_2d/src/costmap_2d.cpp 77.88% <0.00%> (+1.44%) ⬆️
nav2_recoveries/plugins/spin.cpp 94.73% <0.00%> (+5.26%) ⬆️
... and 1 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 34d6184...18fa0cc. Read the comment docs.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

Also some unit tests are necessary

nav2_costmap_2d/plugins/range_sensor_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/range_sensor_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/range_sensor_layer.cpp Outdated Show resolved Hide resolved

if (!tf_->canTransform(
global_frame_, in.header.frame_id, in.header.stamp,
rclcpp::Duration(1e8)))
Copy link
Member

Choose a reason for hiding this comment

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

1e8?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Do you have a suggestion for a different timeout value or are you looking for a chrono literal to denote the units?

Copy link
Member

Choose a reason for hiding this comment

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

@Michael-Equi what do other layers do for the transform timeouts? Do that for consistency.

nav2_costmap_2d/plugins/range_sensor_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/range_sensor_layer.cpp Outdated Show resolved Hide resolved
@Michael-Equi
Copy link
Contributor Author

Also some unit tests are necessary

@simutisernestas Do you want to help write some of the tests?

@SteveMacenski
Copy link
Member

Should be pretty easy because this one is simple. Make an instance of the plugin and feed in some data and check the values in the layer's costmap, feed in some more data, see how it changes. Its easier than others because its a 1D sensor so you can just make 1 sensor to make sure the marking / clearing / probabilistic values are correct for 3-4 update cycles of some representative use-case


if (!tf_->canTransform(
global_frame_, in.header.frame_id, in.header.stamp,
rclcpp::Duration(1e8)))
Copy link
Member

Choose a reason for hiding this comment

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

@Michael-Equi what do other layers do for the transform timeouts? Do that for consistency.

@SteveMacenski
Copy link
Member

After those, LGTM. Only big thing to discuss if the TF timeouts. We spent a great deal of time making all the TF calls in nav2 configurable with networking timeouts so this layer should match

@Michael-Equi
Copy link
Contributor Author

After those, LGTM. Only big thing to discuss if the TF timeouts. We spent a great deal of time making all the TF calls in nav2 configurable with networking timeouts so this layer should match

The only other example of a transformation timeout in a costmap plugin layer I can find is here: https://github.com/ros-planning/navigation2/blob/master/nav2_costmap_2d/plugins/static_layer.cpp#L384-L393

Should I just copy that for the range layer?

@SteveMacenski
Copy link
Member

Well use the transform_tolerance_, but yeah

@SteveMacenski
Copy link
Member

Your test is seg faulting

@Michael-Equi
Copy link
Contributor Author

Your test is seg faulting

Currently debugging that

nav2_costmap_2d/plugins/range_sensor_layer.cpp Outdated Show resolved Hide resolved
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
printMap(*(layers.getCostmap()));

int lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
Copy link
Member

Choose a reason for hiding this comment

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

Is is possible to check which cell is marked?

layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
printMap(*(layers.getCostmap()));

int lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
Copy link
Member

Choose a reason for hiding this comment

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

As well here

layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
printMap(*(layers.getCostmap()));

int lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
Copy link
Member

Choose a reason for hiding this comment

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

As well here.

None of the tests also test clearing, just marking. You could make a test that you send it values at some range, 3, see that the right cell is marked, then a few readings at let say 7, and check that 3 is now clear and 7 is now marked

@Michael-Equi
Copy link
Contributor Author

I pushed an update to include clearing tests but I am not able to get the general case of clearing a marked spot with a longer range reading to work in the testing framework (hence one of the tests fails) despite it clearly working fine on my robot. I cannot figure out what I am doing wrong.

@SteveMacenski
Copy link
Member

Last bits before merge:

@Michael-Equi
Copy link
Contributor Author

Doesnt that first link already mention sonars? Is there something I am missing there?

@SteveMacenski
Copy link
Member

Adding that this exists now, this is new.

@Michael-Equi
Copy link
Contributor Author

Can you confirm this is the correct probabilistic model to be using. I am not great with statistics but when I put values through there seems to be something that is not right which may explain why my tests are failing. For example having a prior of 0.5 and a prob_occ of 0.3 I get the value I get a new_prob of 0.57. Shouldn't the posterior probability in this case be smaller not larger?

    double prior = to_prob(getCost(x, y));
    double prob_occ = sensor * prior;
    double prob_not = (1 - sensor) * (1 - prior);
    double new_prob = prob_occ / (prob_occ + prob_not);

@SteveMacenski
Copy link
Member

I haven't read the stats. I assumed you copied them directly from the ROS1 versions, did you change them?

@Michael-Equi
Copy link
Contributor Author

I have not.

@Michael-Equi
Copy link
Contributor Author

Michael-Equi commented Aug 3, 2020

I am also not sure what the sensor model is supposed to do so maybe that is part of the problem. It seems a lot more complicated than it needs to be but I am sure there is a reason.

@Michael-Equi
Copy link
Contributor Author

@DLu Do you think you could provide some help here? Perhaps also an explanation of the sensor_model code?

@SteveMacenski
Copy link
Member

If you didn't change the math and we can assume it works, you can back out the values from the math

@Michael-Equi
Copy link
Contributor Author

Ok, I just went ahead and removed that one test then since the others are sufficient for checking that.

@SteveMacenski
Copy link
Member

SteveMacenski commented Aug 6, 2020

Anything else you want to do before merging?

I see you have a conflict you need to resolve, and a couple things in docs (migration guide)

@Michael-Equi
Copy link
Contributor Author

Just gave it another spin in the simulation. Probabilistic model marking and clearing works. Restarting works. I think its ready to merge if you are good with it.

@SteveMacenski SteveMacenski merged commit 988b2d3 into ros-navigation:master Aug 7, 2020
@SteveMacenski
Copy link
Member

Awesome, thanks for the help and tests!

@Michael-Equi Michael-Equi deleted the range-sensor-costmap branch August 7, 2020 22:55
@DLu
Copy link
Contributor

DLu commented Aug 8, 2020

FWIW, I dug through my old notes and textbook and can't find the exact equation that I implemented. The best I could find was this original paper but the variables don't match.

SteveMacenski pushed a commit that referenced this pull request Aug 11, 2020
* range costmap building

* range sensor layer working

* nav2 costmap pass linter and uncrustify tests

* Added back semicolon to range class

* Added docs

* Added angles dependency

* Added BSD license

* Added BSD license

* Made functions inline

* revmoed get_clock

* added input_sensor_type to docs

* Made defualt topic name empty to cause error

* using chorno literal to denote time units

* Added small initial test

* Fixed linter error, line breaks, enum, logger level, and transform_tolerance issue

* fixed segmentation fault in test and added transfrom tolerances to tf_->transform

* Got test to pass

* Added more tests

* removed incorrect parameter declaration

* Improved marking while moving tests and added clearing tests

* removed general clearing test

* changed testing helper import in test
SteveMacenski added a commit that referenced this pull request Aug 11, 2020
* Add slam toolbox as exec dep for nav2_bringup (#1827)

* Add slam toolbox as exec dep

* Added slam toolbox

* Changed version of slam toolbox to foxy-devel

* Added modifications to allow for exec depend of slam toolbox without breaking docker / CI

* reformatted skip keys

* Added tabs to skip key arguments

* Removed commented out code block

* Add unit tests for follow path, compute path to pose, and navigate to pose BT action nodes (#1844)

* Add compute path to pose unit tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add follow path action unit tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add navigate to pose action unit tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* adding foxy build icons to readme (#1853)

* adding foxy build icons

* adding dividers

* sync master from foxy version updates (#1852)

* sync master from foxy version updates

* syncing foxy and master

* Add unit tests for clear entire costmap and reinitialize global localization BT service nodes (#1845)

* Add clear entire costmap service unit tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add reinitialize global localization service unit test

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Temporarily disable dump_params test (#1856)

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* commenting out unused validPointPotential (#1854)

* Adding ROS2 versions to issue template (#1861)

* reload behavior tree before creating RosTopicLogger to prevent nullptr error or no /behavior_tree_log problem (#1849)

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* Add citation for IROS paper (#1867)

* Add citation

We'll want to add the DOI once published

* Bump citation section above build status

* Fix line breaks

* Changes RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED to RCUTILS_LOGGING_BUFFERED_STREAM (#1868)

* Added parameters to configure amcl laser scan topic (#1870)

* Added parameters to configure amcl topic

* changed parameter to scan_topic and added to all the configuration files

* added scan_topic amcl param to docs

* Satisfied linter

* Move dwb goal/progress checker plugins to nav2_controller (#1857)

* Move dwb goal/progress checker plugins to nav2_controller

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Move goal/progress checker plugins to nav2_controller

Address review comments

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Move goal/progress checker plugins to nav2_controller

Use new plugin declaration format and doc update

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Update bringup.yaml for new plugins in nav2_controller

Also remove redundent file from dwb_plugins
Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Fix doc errors and update remaining yaml files

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Remove mention of goal_checker from dwb docs

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Add .plugin params to doc

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Tests for progress_checker plugin

declare .plugin only if not declared before

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Tweak plugin names/description in doc

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Follow pose (#1859)

* Truncate path finished

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Follow Pose finished

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Change names. Add test and doc

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Change names and check atan2 values

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Checking Inf/NaNs and trucate path changes

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Revert changes in launcher

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Documenting Tree

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Update nav2_bt_navigator/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_bt_navigator/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Change TruncatePath from decorator to action node

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Update nav2_bt_navigator/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Remove Deprecated Declaration (#1884)

* Remove deprecated rclcpp::executor::FutureReturnCode::SUCCESS in favor of rclcpp::FutureReturnCode::SUCCESS

Signed-off-by: Hunter L. Allen <hunter.allen@ghostrobotics.io>

* Update nav2_util/include/nav2_util/service_client.hpp

Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Added new costmap buffer to resize map safely (#1837)

* Added new costmap buffer to resize map safely

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Update map if the layer is not being processed.

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Updated bool name and added buffer initialization

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Fix dump params tests (#1886)

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Move definitions in tf_help to separate src cpp file (#1890)

* Move definitions of functions in tf_help to separate src cpp file to avoid multiple definition error

* Fix linting issues

* Make uncrustify happier

* More format fixing

* resolved the simulated motion into its x & y components (#1887)

Signed-off-by: Marwan Taher <marokhaled99@gmail.com>

* Fix nav2_bt_navigator cleanup (#1901)

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Adding some more test coverage (#1903)

* more tests

* removing old cmake

* remove on_errors in specific servers in favor of a general lifecycle warning (#1904)

* remove on_errors in favor of a general lifecycle warning

* adding removed thing

* simply nodes to remove lines (#1907)

* Bunches of random new tests (#1909)

* trying to get A* to work again

* make flake 8 happy

* adding cancel and preempt test

* planner tests use A*

* adding A* note

* test with topic

* Adding failure to navigate test (#1912)

* failures tests

* adding copyrights

* cancel test in recoveries

* Costmap lock while copying data in navfn planner (#1913)

* acquire the costmap lock while copying data in navfn planner

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* add costmap lock to dwb local plannger

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* Added map_topic parameters to static layer and amcl (#1910)

* see if spin some in cancel will allow result callback (#1914)

* Adding near-complete voxel grid test coverage and more to controller server (#1915)

* remove erraneous handling done by prior

* adding a bunch of voxel unit tests

* retrigger

* adding waypoint follower failure test, voxel layer integration tests, etc (#1916)

* adding waypoint follower failure test

* adding voxel, more logging

* reset -> clear

* minor changes to lower redundent warnings (#1918)

* Costmap plugins declare if not declared for reset capabilities (#1919)

* fixing #1917 on declare if not declared

* fix API

* adding CLI test (#1920)

* More coverage in map server tests (#1921)

* adding CLI test

* adding a bunch of new coverages for map_server

* Add in range sensor costmap layer (#1888)

* range costmap building

* range sensor layer working

* nav2 costmap pass linter and uncrustify tests

* Added back semicolon to range class

* Added docs

* Added angles dependency

* Added BSD license

* Added BSD license

* Made functions inline

* revmoed get_clock

* added input_sensor_type to docs

* Made defualt topic name empty to cause error

* using chorno literal to denote time units

* Added small initial test

* Fixed linter error, line breaks, enum, logger level, and transform_tolerance issue

* fixed segmentation fault in test and added transfrom tolerances to tf_->transform

* Got test to pass

* Added more tests

* removed incorrect parameter declaration

* Improved marking while moving tests and added clearing tests

* removed general clearing test

* changed testing helper import in test

* [Test sprint] push nav2_map_server over 90% (#1922)

* adding CLI test

* adding tests for more lines to map_server

* fix last test

* adding out of bounds and higher bounds checks

* adding planner tests for plugins working

* add cleanup

* working

* ping

* [testing sprint] final test coverage and debug logging in planner/controller servers (#1924)

* adding CLI test

* adding tests for more lines to map_server

* fix last test

* adding out of bounds and higher bounds checks

* adding planner tests for plugins working

* add cleanup

* working

* ping

* adding more testing and debug info messages

* [testing sprint] remove dead code not used in 10 years from navfn (#1926)

* remove dead code not used in 10 years from navfn

* ping

* inverting period to rate

* removing debug statement that could never be triggered by a single non-looping action server

* type change

* adding removed files

* bump to 0.4.2

* add another missing file

* add missing files

* remove some tests

* lint

* removing nav2_bringup from binaries

Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com>
Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com>
Co-authored-by: Ruffin <roxfoxpox@gmail.com>
Co-authored-by: Siddarth Gore <siddarth.gore@gmail.com>
Co-authored-by: Francisco Martín Rico <fmrico@gmail.com>
Co-authored-by: Hunter L. Allen <hunter.allen@ghostrobotics.io>
Co-authored-by: Aitor Miguel Blanco <aitormibl@gmail.com>
Co-authored-by: Joe Smallman <ijsmallman@gmail.com>
Co-authored-by: Marwan Taher <marokhaled99@gmail.com>
SteveMacenski pushed a commit that referenced this pull request Aug 11, 2020
* range costmap building

* range sensor layer working

* nav2 costmap pass linter and uncrustify tests

* Added back semicolon to range class

* Added docs

* Added angles dependency

* Added BSD license

* Added BSD license

* Made functions inline

* revmoed get_clock

* added input_sensor_type to docs

* Made defualt topic name empty to cause error

* using chorno literal to denote time units

* Added small initial test

* Fixed linter error, line breaks, enum, logger level, and transform_tolerance issue

* fixed segmentation fault in test and added transfrom tolerances to tf_->transform

* Got test to pass

* Added more tests

* removed incorrect parameter declaration

* Improved marking while moving tests and added clearing tests

* removed general clearing test

* changed testing helper import in test
ruffsl pushed a commit to ruffsl/navigation2 that referenced this pull request Jul 2, 2021
* range costmap building

* range sensor layer working

* nav2 costmap pass linter and uncrustify tests

* Added back semicolon to range class

* Added docs

* Added angles dependency

* Added BSD license

* Added BSD license

* Made functions inline

* revmoed get_clock

* added input_sensor_type to docs

* Made defualt topic name empty to cause error

* using chorno literal to denote time units

* Added small initial test

* Fixed linter error, line breaks, enum, logger level, and transform_tolerance issue

* fixed segmentation fault in test and added transfrom tolerances to tf_->transform

* Got test to pass

* Added more tests

* removed incorrect parameter declaration

* Improved marking while moving tests and added clearing tests

* removed general clearing test

* changed testing helper import in test
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.

3 participants