nav2_smac_planner: handle corner case where start and goal are on the same cell#4793
Merged
SteveMacenski merged 4 commits intoros-navigation:mainfrom Dec 17, 2024
Conversation
5030af6 to
2f13bf1
Compare
Codecov ReportAttention: Patch coverage is
|
… same cell This case was already properly handled in the smac_planner_2d, but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case. This commit fixes issue ros-navigation#4792. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
2f13bf1 to
a912059
Compare
SteveMacenski
requested changes
Dec 13, 2024
Member
SteveMacenski
left a comment
There was a problem hiding this comment.
Otherwise, LGTM. Make sure to do these same comments in the State Lattice planner & add into the unit tests a plan where the start and goal are the same bin to exercise the change in unit testing
Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
…he same cell Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
f820898 to
09eec6a
Compare
Contributor
|
@DylanDeCoeyer-Quimesis, your PR has failed to build. Please check CI outputs and resolve issues. |
09eec6a to
728054f
Compare
SteveMacenski
requested changes
Dec 13, 2024
Member
SteveMacenski
left a comment
There was a problem hiding this comment.
Otherwise, LGTM. Test just needs to actually test the result of the exercised bit
5b44e9e to
eba884f
Compare
Contributor
|
@DylanDeCoeyer-Quimesis, your PR has failed to build. Please check CI outputs and resolve issues. |
Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
eba884f to
81ee425
Compare
masf7g
pushed a commit
to quasi-robotics/navigation2
that referenced
this pull request
Dec 19, 2024
… same cell (ros-navigation#4793) * nav2_smac_planner: handle corner case where start and goal are on the same cell This case was already properly handled in the smac_planner_2d, but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case. This commit fixes issue ros-navigation#4792. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: use goal orientation when path is made of one point Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: publish raw path also when start and goal are on the same cell Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: add corner case to unit tests Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> --------- Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
SteveMacenski
pushed a commit
that referenced
this pull request
Feb 5, 2025
… same cell (#4793) * nav2_smac_planner: handle corner case where start and goal are on the same cell This case was already properly handled in the smac_planner_2d, but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case. This commit fixes issue #4792. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: use goal orientation when path is made of one point Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: publish raw path also when start and goal are on the same cell Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: add corner case to unit tests Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> --------- Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
SteveMacenski
added a commit
that referenced
this pull request
Feb 5, 2025
* nav2_smac_planner: handle corner case where start and goal are on the same cell (#4793) * nav2_smac_planner: handle corner case where start and goal are on the same cell This case was already properly handled in the smac_planner_2d, but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case. This commit fixes issue #4792. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: use goal orientation when path is made of one point Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: publish raw path also when start and goal are on the same cell Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: add corner case to unit tests Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> --------- Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * creating auto-transition option for nav2_util::LifecycleNode (#4804) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix trajectory generation bug in docking controller (#4807) * Fix trajectory in docking controller Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Ceil and remove resolution Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update nav2_docking/opennav_docking/src/controller.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update nav2_docking/opennav_docking/src/controller.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * graceful_controller: implement iterative selection of control points (#4795) * initial pass at iterative Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * add v_angular_min_in_place Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * add orientation filter, fix remaining TODOs Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * try to increase coverage, fixup minor test issues Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * address review comments Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * review comments * update defaults * rename to in_place_collision_resolution Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * revert change in default Signed-off-by: Michael Ferguson <mfergs7@gmail.com> --------- Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * fix bug in use of v_angular_min_in_place (#4813) Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * publish motion target as pose (#4839) Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * nav2_behaviors: drive_on_heading: return TIMEOUT error code when exceeding time allowance (#4836) Until now, the NONE error code was returned when exceeding the time allowance. Let's return the more appropriate TIMEOUT error code instead. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * fix bug in orientation filtering (#4840) * fix bug in orientation filtering some global planners output all zeros for orientation, however the plan is in the global frame. when transforming to the local frame, the orientation is no longer zero. Instead of comparing to zero, we simply check if all the orientations in the middle of the plan are equal Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * account for floating point error Signed-off-by: Michael Ferguson <mfergs7@gmail.com> --------- Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * Adapt GoalUpdater to update goals as well (#4771) * Add IsStoppedBTNode Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add topic name + reformat Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix comment Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix abs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove log Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add getter functions for raw twist Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove unused code Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use odomsmoother Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * update groot Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reset at success Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * FIX velocity_threshold_ Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix stopped Node Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add tests to odometry_utils Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Adapt goalUpdater to modify goals as well Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * change name of msg Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Make input goals be Goals again for compatibility Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "fix" This reverts commit 8303cdc. Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * refactoring Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * ament Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * ignore if no timestamps Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * facepalm Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * update groot nodes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Use PoseStampedArray Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use geometry_msgs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use geometry_msgs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * more fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * . Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * revert Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * . Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add common_interfaces Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use PoseStampedArray Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reformat Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * revert Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * revert Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix warn msg Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * improve Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix format Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * change to info Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix simple smoother failing during final approach (#4817) * new test case for end of path approach Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> * modify tests to match the more permissive smoother policy Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> * implement steve's suggestions Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> --------- Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> * Add acc limit consideration to avoid overshooting in RotationShimController (#4864) * Add acc limit consideration to avoid overshoot in RotationShimController Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add acceleration limit tests to RotationShimController Signed-off-by: RBT22 <rozgonyibalint@gmail.com> --------- Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Fix flaky ness of opennav_docking test_docking_server (#4878) (#4879) Call publish() (odom -> base_link tf) at startup to kick things off and spin 10 times(1 second) TF, so that it has a chance to propogate to the docking_server so that it will accept an action request. Previously it was only spinning once, hoping the timer would fire and call publish fast enough for it to propogate to the docking_server so that it is able to accept the first 'dock_robot' action request Signed-off-by: Mike Wake <macwake@gmail.com> * [BtActionNode] [BtServiceNode] clear between calls (#4887) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * dwb_critics flaky test - lineCost coordinates must be within costmap (#4889) (#4884) There is no protection/checks in the pathway from lineCost to costmap_2d::getIndex(mx, my) for grid coordinates that exceed the of bounds of the allocated costmap. (presumably for speed) This test was triggering an off by one error attempting to read the the 2500 byte costmap at byte 2503 costmap size 50x50. getIndex(3, 50) = my * size_x_ + mx; = 50 * 50 + 3; = 2503 Signed-off-by: Mike Wake <macwake@gmail.com> * Add option to use open-loop control with Rotation Shim (#4880) * Initial implementation Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * replace feedback param with closed_loop Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Reset last_angular_vel_ in activate method Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add closed_loop parameter to dynamicParametersCallback Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add tests Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Override reset function Signed-off-by: RBT22 <rozgonyibalint@gmail.com> --------- Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Fix unstable test in nav2 util (#4894) * Fix unstable test in nav2 util Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Change 5s to 1s Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update bt2img syntax and bt pics (#4900) Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com> * bumping to 1.3.5 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Revert "Adapt GoalUpdater to update goals as well (#4771)" This reverts commit 55d7387. --------- Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: Michael Ferguson <mfergs7@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> Signed-off-by: RBT22 <rozgonyibalint@gmail.com> Signed-off-by: Mike Wake <macwake@gmail.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: Alberto Tudela <ajtudela@gmail.com> Co-authored-by: Michael Ferguson <mfergs7@gmail.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Ray Ferric <63957587+rayferric@users.noreply.github.com> Co-authored-by: Balint Rozgonyi <43723477+RBT22@users.noreply.github.com> Co-authored-by: ewak <ewak@users.noreply.github.com> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: mini-1235 <58433591+mini-1235@users.noreply.github.com>
stevedanomodolor
pushed a commit
to stevedanomodolor/navigation2
that referenced
this pull request
Apr 29, 2025
… same cell (ros-navigation#4793) * nav2_smac_planner: handle corner case where start and goal are on the same cell This case was already properly handled in the smac_planner_2d, but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case. This commit fixes issue ros-navigation#4792. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: use goal orientation when path is made of one point Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: publish raw path also when start and goal are on the same cell Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: add corner case to unit tests Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> --------- Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This case was already properly handled in the smac_planner_2d (since PR #2488), but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case.
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: