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

do not depend on velocity for approach scaling #3047

Merged

Conversation

Aposhian
Copy link
Contributor


Basic Info

Info Please fill out this column
Ticket(s) this addresses #3045
Primary OS tested on Ubuntu
Robotic platform tested on Not yet

Description of contribution in a few bullet points

Depend only on the path length left on whether to scale velocity on approach, since depending on lookahead distance results in a cyclic dependency

Description of documentation updates required from your changes

Added a new parameter.


Future work that may be required in bullet points

Need to test this and build it still.

For Maintainers:

  • Check that any new parameters added are updated in navigation.ros.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

@mergify
Copy link
Contributor

mergify bot commented Jun 27, 2022

@Aposhian, please properly fill in PR template in the future. @SteveMacenski, use this instead.

  • Check that any new parameters added are updated in navigation.ros.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

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.

Beyond the obvious points, this seems good to me!

@mergify
Copy link
Contributor

mergify bot commented Jun 28, 2022

@Aposhian, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@SteveMacenski
Copy link
Member

@Aposhian any update? It would be great to have this in before the next sync & close out #3045

@Aposhian
Copy link
Contributor Author

Aposhian commented Jul 7, 2022

I may be able to get to this on Saturday, but I have been a little swamped.

@Aposhian
Copy link
Contributor Author

Aposhian commented Jul 8, 2022

Here is how it ends up performing with the default 1m threshold. I think that is probably not a good default. I will try to figure out how to set this parameter to have roughly the same behavior as before.
image

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 8, 2022

Sounds good! Toggling since CI didn't trigger again after the changes

TODO

  • Default for similar behavior
  • Add docs to navigation.ros.org
  • Add comment in readme (and navigation.ros.org) about sizing of the new parameter w.r.t. the costmap size

@Aposhian
Copy link
Contributor Author

Aposhian commented Jul 8, 2022

Ok I just updated the default to be one costmap cell less than the forward costmap extent, so that should approximate the old behavior (where scaling was enabled when lookahead distance was shorter than requested). Here is the behavior with that now on my robot sim:
image
One problem with looking at integrated distance of the transformed plan is that it jumps by the robot jumping between poses (this is with a sparse path). However, I think this is a far better behavior than what is existing, and this is also an issue that the velocity smoother could help remedy, correct?

@Aposhian Aposhian marked this pull request as ready for review July 8, 2022 21:29
@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 9, 2022

Ok I just updated the default to be one costmap cell less than the forward costmap extent, so that should approximate the old behavior

Uh, wasn't it before that it started to slow down at the lookahead distance away, e.g. by default about 0.6m? This is much higher than that if its 1/2 the costmap size. The added comment in the readme also doesn't say that you should never set it to be higher than that -- what I meant by my comments in threads above was we need a warning not to exceed, not that this is the recommended value.

But maybe its a better value, but I suppose you didn't mention that you tried the old setting (~0.6) and found that the longer distance was better. Maybe I need a clarification here on what you tried to come to that conclusion (and still need that warning in the readme / in the code that it checks and never exceeds that + logs a warning about it and sets to the max valid possible value if they tried to exceed it)

Yes, the vel smoother will work out the jaggedness.

@Aposhian
Copy link
Contributor Author

Uh, wasn't it before that it started to slow down at the lookahead distance away, e.g. by default about 0.6m

Well, I figured there wasn't a good way to recreate the average lookahead distance, since the velocity is unknown. But maybe the default could be calculated based off lookahead time and desired linear velocity?

what I meant by my comments in threads above was we need a warning not to exceed, not that this is the recommended value.

Ok, I misunderstood, but that makes sense. I think I can add a bounds check for that.

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 12, 2022

Well, I figured there wasn't a good way to recreate the average lookahead distance, since the velocity is unknown. But maybe the default could be calculated based off lookahead time and desired linear velocity?

Why not just use the value of the parameter lookahead_dist as the default? Though, if Adaptive behavior is being used, I suppose just max_lookahead_dist would be fine too, but if I had to pick one, I'd pick lookahead_dist since that should be no matter what type of PP was being used. Just needs to be documented that it defaults to lookahead_dist unless otherwise overridden (and should never exceed costmap size / 2).

Its imperfect, but it sounds reasonable. The other alternative is to just make them unrelated by default to anything else and just set it to 0.6 like it was before. I suppose there's no specific reason we need to make the default tied to the lookahead distance anymore as long as the default behavior is similar. I don't care very much, I think both are rational options, so I leave it to implementer's choice 😆

@Aposhian
Copy link
Contributor Author

Docs PR: ros-navigation/docs.nav2.org#336

@Aposhian
Copy link
Contributor Author

I decided to go with just making it an independent default, but going to 0.6 to match previous behavior. Unit tests are updated. I'm just going to do one more test in simulation with this and then I will be happy with it.

@Aposhian
Copy link
Contributor Author

With some tuning this is what the approach velocity scaling ends up looking like (on a sparse path)
image
I think we can probably do better, but this at least removes the oscillating behavior because of the cyclic dependency.

@Aposhian
Copy link
Contributor Author

@SteveMacenski I think all tasks should be addressed. Let me know if there is anything else.

@Aposhian
Copy link
Contributor Author

If you weren't using adaptive lookahead distance before, then this change will be a regression: it will result in a less smooth scaling. I think we can tweak this algorithm to make it scale linearly, regardless of path density. I just need to brainstorm for a bit.

@SteveMacenski
Copy link
Member

Toggling for CI, Circle's been flaky lately. Will merge once builds

@SteveMacenski SteveMacenski merged commit 9706878 into ros-navigation:main Jul 21, 2022
@Aposhian
Copy link
Contributor Author

Here is what it looks like with the smoothed version:
image

SteveMacenski pushed a commit that referenced this pull request Aug 24, 2022
* do not depend on velocity for approach scaling

* add approach_velocity_scaling_dist to README

* do not pass references to shared ptr

* fixup! do not pass references to shared ptr

* fix approach velocity scaling compile issues

* default approach_velocity_scaling_dist based on costmap size

* change default approach_velocity_scaling_dist to 0.6 to match previous behavior

* update tests for approach velocity scaling

* remove use_approach_linear_velocity_scaling from readme

* smooth approach velocity scaling
SteveMacenski added a commit that referenced this pull request Aug 24, 2022
* Fix size_t format specifier (#3003)

* clear up params file (#3004)

Signed-off-by: zhenpeng ge <[email protected]>

* Bt test fix (#2999)

* fixed tests

* undo

* linting fix (#3007)

Signed-off-by: zhenpeng ge <[email protected]>

* Add nav2_constrained_smoother to metapackage

* adding humble release to table (#3008)

* Fix for costmap nodes lifecycle status (#3009)

Lifecycle status for global and local cost nodes not correct.
ros2 lifecycle/service commands  shows unconfigured for these two.
This is due to directly calling on_configure/on_activate/on_cleanup
calls in parent node.  This PR to replace on_xxxxxx() to
configure()/activate()/cleanup() calls of lifecycle base.

Signed-off-by: Arshad <[email protected]>

* Get parameters on configure transition addressing #2985 (#3000)

* Get parameters on configure transition

Signed-off-by: MartiBolet <[email protected]>

* Remove past setting of parameters

Signed-off-by: MartiBolet <[email protected]>

* Expose transition functions to public for test

Signed-off-by: MartiBolet <[email protected]>

* fix: wrong input type in navigate_to_pose_action.hpp and navigate_to_… (#2994)

* fix: wrong input type in navigate_to_pose_action.hpp and navigate_to_pose_action.hpp

* Update navigate_through_poses_action.hpp

Co-authored-by: Steve Macenski <[email protected]>

* Nav2 Velocity Smoother (#2964)

* WIP velocity smoother with ruckig

* a few comments

* vel smoother prototype

* updating defaults

* adding defaults to readme

* removing note from readme

* updates to velocity smoother TODO items

* adding unit tests

* finishing system tests

* adding failure to change parameters tests

* fix last bits

* fixing negative sign bug

* lint

* update tests

* setting defaults

* Adding warning

* Update velocity_smoother.cpp

* Fixing rebase issue

* adding plugin type for static in local + removing unused print (#3021)

* removed extra includes (#3026)

* remove extra sub (#3025)

* Fix missing dependency on nav2_velocity_smoother (#3031)

* adding timeout for action client initialization (#3029)

* adding timeout for action client initialization

Signed-off-by: Vinny Ruia <[email protected]>

* adding constant 1s timeout, catching exception

Signed-off-by: Vinny Ruia <[email protected]>

* cleanup warnings (#3028)

* cleanup warnings

* removed referenc

* installing plugins folder of nav2_behaviors package (#3051)

Co-authored-by: Srijanee Biswas <[email protected]>

* Completed PR 2929 (#3038)

* accepting empty yaml_filename if no initial map is available

* invalid load_map-request does not invalidate existing map, added Testcase

* style

* finish PR 2929

* finish linting

* removing change

* removing change

* Update test_map_server_node.cpp

* Update test_map_server_node.cpp

Co-authored-by: Nikolas Engelhard <[email protected]>

* zero z values in rpp transformed plan (#3066)

* removing get node options default for nav2 utils (#3073)

* adding looping timeout for lifecycle service clients + adding string name of action for BT action nodes (#3071)

* adding looping timeout for lifecycle service clients + adding string name of action for BT action nodes

* fix linting

* remove inline comment

* adding goal updated controller node to test

* Smac Planner 2D changes (#3083)

* removing 4-connected option from smac; fixing 2D heuristic and traversal function from other planner's changes

* fix name of variable since no longer a neighborhood

* partial test updates

* ported unit tests fully

* revert to no costmap downsampling

* Collision monitor (#2982)

* Add Collision Monitor node

* Meet review items

* Fix next review items

* Code cleanup

* Support dynamic footprint. More optimizations.

* Switch to multiple footprints. Move variables.
Remove odom subscriber. Add 0-velocity optimization

* Update nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/params/collision_monitor_params.yaml

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/params/collision_monitor_params.yaml

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/params/collision_monitor_params.yaml

Co-authored-by: Steve Macenski <[email protected]>

* Meet smaller review items

* Add fixes found during unit test development

* Fix uncrustify issues

* Add unit tests

* Fix number of polygons points

* Move tests

* Add kinematics unit test

* Minor tests fixes

* Remove commented line

* Add edge case checking testcase and references

* Update comment

* Add README.md

* Fixed table

* Minor changes in README.md

* Fix README.md for documentation pages

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Meet review items

* Meet review items (part 2)

* Update polygons picture for README

* Change simulation_time_step to 0.1

* Fix bounding boxes to fit the demo from README.md

* Terminology fixes

Co-authored-by: Steve Macenski <[email protected]>

* removing the extra argument in class dec (#3084)

* Fix for #3078, fix image path in YAML (#3082)

* Fix for #3078, fix image path in YAML

* Update map_io.cpp

* Update map_io.cpp

* Update map_io.cpp

* Update map_io.cpp

* do not depend on velocity for approach scaling (#3047)

* do not depend on velocity for approach scaling

* add approach_velocity_scaling_dist to README

* do not pass references to shared ptr

* fixup! do not pass references to shared ptr

* fix approach velocity scaling compile issues

* default approach_velocity_scaling_dist based on costmap size

* change default approach_velocity_scaling_dist to 0.6 to match previous behavior

* update tests for approach velocity scaling

* remove use_approach_linear_velocity_scaling from readme

* smooth approach velocity scaling

* Use correct timeout when checking future (#3087)

* Adds missing commas so default plugin names are not stuck together (#3093)

Signed-off-by: Aaron Chong <[email protected]>

Signed-off-by: Aaron Chong <[email protected]>

* Fix Costmap Filters system tests (#3120)

* Fix Costmap Filters system tests

* Update map_io.cpp

Co-authored-by: Alexey Merzlyakov <[email protected]>

* Finding distance H if obstacle H is <= 0 (#3122)

* adding checks on goal IDs in results for waypoint follower (#3130)

* ComputePathToPose Sets empty path on blackboard if action is aborted or cancelled (#3114)

* set empty path on black on failure

* docs

* switched to correct message type

* set empty path for compute_path_through_poses

* Ignore feedback from old goals in waypoint follower (#3139)

* apply joinchar in pathify (#3141)

Co-authored-by: kevin <[email protected]>

* Log level (#3110)

* added log level for navigation launch

* localization log level

* slam log level

* revert use_comp

* added log level to components

* linting and uncrusitfy fixes for CI (#3144)

* start is now added to the path (#3140)

* start is now added to the path

* lint fix

* Update README.md (#3147)

Current example doesn't work with the updates.

* fixing linting problem

* Setting map map's yaml path to empty string, since overridden in launch (#3123)

* Update nav2_params.yaml

* Update nav2_params.yaml

* Update nav2_params.yaml

* bumping to 1.1.1 for release

Signed-off-by: zhenpeng ge <[email protected]>
Signed-off-by: Arshad <[email protected]>
Signed-off-by: MartiBolet <[email protected]>
Signed-off-by: Vinny Ruia <[email protected]>
Signed-off-by: Aaron Chong <[email protected]>
Co-authored-by: M. Mostafa Farzan <[email protected]>
Co-authored-by: Zhenpeng Ge <[email protected]>
Co-authored-by: Joshua Wallace <[email protected]>
Co-authored-by: Arshad Mehmood <[email protected]>
Co-authored-by: MartiBolet <[email protected]>
Co-authored-by: shoufei <[email protected]>
Co-authored-by: hodnajit <[email protected]>
Co-authored-by: Vinny Ruia <[email protected]>
Co-authored-by: SrijaneeBiswas <[email protected]>
Co-authored-by: Srijanee Biswas <[email protected]>
Co-authored-by: Nikolas Engelhard <[email protected]>
Co-authored-by: Adam Aposhian <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: Pradheep Krishna <[email protected]>
Co-authored-by: nakai-omer <[email protected]>
Co-authored-by: Samuel Lindgren <[email protected]>
Co-authored-by: Aaron Chong <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>
Co-authored-by: 정찬희 <[email protected]>
Co-authored-by: kevin <[email protected]>
Co-authored-by: Austin Greisman <[email protected]>
jwallace42 pushed a commit to jwallace42/navigation2 that referenced this pull request Dec 14, 2022
* do not depend on velocity for approach scaling

* add approach_velocity_scaling_dist to README

* do not pass references to shared ptr

* fixup! do not pass references to shared ptr

* fix approach velocity scaling compile issues

* default approach_velocity_scaling_dist based on costmap size

* change default approach_velocity_scaling_dist to 0.6 to match previous behavior

* update tests for approach velocity scaling

* remove use_approach_linear_velocity_scaling from readme

* smooth approach velocity scaling
borongyuan added a commit to borongyuan/navigation2 that referenced this pull request Dec 16, 2022
* do not depend on velocity for approach scaling

* add approach_velocity_scaling_dist to README

* do not pass references to shared ptr

* fixup! do not pass references to shared ptr

* fix approach velocity scaling compile issues

* default approach_velocity_scaling_dist based on costmap size

* change default approach_velocity_scaling_dist to 0.6 to match previous behavior

* update tests for approach velocity scaling

* remove use_approach_linear_velocity_scaling from readme

* smooth approach velocity scaling

(cherry picked from commit 9706878)
borongyuan added a commit to borongyuan/navigation2 that referenced this pull request Jan 4, 2023
* do not depend on velocity for approach scaling

* add approach_velocity_scaling_dist to README

* do not pass references to shared ptr

* fixup! do not pass references to shared ptr

* fix approach velocity scaling compile issues

* default approach_velocity_scaling_dist based on costmap size

* change default approach_velocity_scaling_dist to 0.6 to match previous behavior

* update tests for approach velocity scaling

* remove use_approach_linear_velocity_scaling from readme

* smooth approach velocity scaling

(cherry picked from commit 9706878)
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