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

[BtActionNode] [BtServiceNode] clear between calls #4887

Merged
merged 1 commit into from
Jan 30, 2025

Conversation

doisyg
Copy link
Contributor

@doisyg doisyg commented Jan 30, 2025


Basic Info

Info Please fill out this column
Ticket(s) this addresses
Primary OS tested on Ubuntu 24.04
Robotic platform tested on Dexory robot
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

In BtActionNode and BtServiceNode, the goal_, result_ and request_ objects were not cleared between 2 calls. Hence keeping an old value if any field of these was not explicitly set by a BT node inheriting BtActionNode or BtServiceNode.
I realized it while using ComputePathToPose and not understanding why the action was using a previous start pose when actually making a new call without a start parameter (where it is expected that the current pose of the robot will be used):

getInput("goal", goal_.goal);
getInput("planner_id", goal_.planner_id);
if (getInput("start", goal_.start)) {
goal_.use_start = true;
}
}

This PR reinitializes goal_, result_ and request_ between calls to solve this issue.

Description of documentation updates required from your changes

Description of how this change was tested

  • Appropriate behavior of ComputePathToPose when calling it successively with and without a valid input start

Future work that may be required in bullet points

For Maintainers:

  • Check that any new parameters added are updated in docs.nav2.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.

Nice! I checked and see no problems with it. I thought maybe some node maintained state, but it doesn't appear so

@SteveMacenski SteveMacenski merged commit 27fdcb7 into ros-navigation:main Jan 30, 2025
9 of 10 checks passed
SteveMacenski pushed a commit that referenced this pull request Feb 5, 2025
Signed-off-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
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 <[email protected]>

* nav2_smac_planner: use goal orientation when path is made of one point

Signed-off-by: Dylan De Coeyer <[email protected]>

* nav2_smac_planner: publish raw path also when start and goal are on the same cell

Signed-off-by: Dylan De Coeyer <[email protected]>

* 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 <[email protected]>

---------

Signed-off-by: Dylan De Coeyer <[email protected]>

* creating auto-transition option for nav2_util::LifecycleNode (#4804)

Signed-off-by: Steve Macenski <[email protected]>

* Fix trajectory generation bug in docking controller (#4807)

* Fix trajectory in docking controller

Signed-off-by: Alberto Tudela <[email protected]>

* Ceil and remove resolution

Signed-off-by: Alberto Tudela <[email protected]>

* Update nav2_docking/opennav_docking/src/controller.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Update nav2_docking/opennav_docking/src/controller.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

---------

Signed-off-by: Alberto Tudela <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

* graceful_controller: implement iterative selection of control points (#4795)

* initial pass at iterative

Signed-off-by: Michael Ferguson <[email protected]>

* add v_angular_min_in_place

Signed-off-by: Michael Ferguson <[email protected]>

* add orientation filter, fix remaining TODOs

Signed-off-by: Michael Ferguson <[email protected]>

* try to increase coverage, fixup minor test issues

Signed-off-by: Michael Ferguson <[email protected]>

* address review comments

Signed-off-by: Michael Ferguson <[email protected]>

* review comments

* update defaults
* rename to in_place_collision_resolution

Signed-off-by: Michael Ferguson <[email protected]>

* revert change in default

Signed-off-by: Michael Ferguson <[email protected]>

---------

Signed-off-by: Michael Ferguson <[email protected]>

* fix bug in use of v_angular_min_in_place (#4813)

Signed-off-by: Michael Ferguson <[email protected]>

* publish motion target as pose (#4839)

Signed-off-by: Michael Ferguson <[email protected]>

* 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 <[email protected]>

* 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 <[email protected]>

* account for floating point error

Signed-off-by: Michael Ferguson <[email protected]>

---------

Signed-off-by: Michael Ferguson <[email protected]>

* Adapt GoalUpdater to update goals as well (#4771)

* Add IsStoppedBTNode

Signed-off-by: Tony Najjar <[email protected]>

* add topic name + reformat

Signed-off-by: Tony Najjar <[email protected]>

* fix comment

Signed-off-by: Tony Najjar <[email protected]>

* fix abs

Signed-off-by: Tony Najjar <[email protected]>

* remove log

Signed-off-by: Tony Najjar <[email protected]>

* add getter functions for raw twist

Signed-off-by: Tony Najjar <[email protected]>

* remove unused code

Signed-off-by: Tony Najjar <[email protected]>

* use odomsmoother

Signed-off-by: Tony Najjar <[email protected]>

* fix formatting

Signed-off-by: Tony Najjar <[email protected]>

* update groot

Signed-off-by: Tony Najjar <[email protected]>

* Add test

Signed-off-by: Tony Najjar <[email protected]>

* reset at success

Signed-off-by: Tony Najjar <[email protected]>

* FIX velocity_threshold_

Signed-off-by: Tony Najjar <[email protected]>

* Fix stopped Node

Signed-off-by: Tony Najjar <[email protected]>

* Add tests  to odometry_utils

Signed-off-by: Tony Najjar <[email protected]>

* fix linting

Signed-off-by: Tony Najjar <[email protected]>

* Adapt goalUpdater to modify goals as well

Signed-off-by: Tony Najjar <[email protected]>

* fix formatting

Signed-off-by: Tony Najjar <[email protected]>

* fixes

Signed-off-by: Tony Najjar <[email protected]>

* change name of msg

Signed-off-by: Tony Najjar <[email protected]>

* Make input goals be Goals again for compatibility

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* Revert "fix"

This reverts commit 8303cdc.

Signed-off-by: Tony Najjar <[email protected]>

* refactoring

Signed-off-by: Tony Najjar <[email protected]>

* ament

Signed-off-by: Tony Najjar <[email protected]>

* ignore if no timestamps

Signed-off-by: Tony Najjar <[email protected]>

* facepalm

Signed-off-by: Tony Najjar <[email protected]>

* update groot nodes

Signed-off-by: Tony Najjar <[email protected]>

* Use PoseStampedArray

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* fixes

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* use geometry_msgs

Signed-off-by: Tony Najjar <[email protected]>

* fix import

Signed-off-by: Tony Najjar <[email protected]>

* use geometry_msgs

Signed-off-by: Tony Najjar <[email protected]>

* more fixes

Signed-off-by: Tony Najjar <[email protected]>

* .

Signed-off-by: Tony Najjar <[email protected]>

* revert

Signed-off-by: Tony Najjar <[email protected]>

* .

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* add common_interfaces

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* use PoseStampedArray

Signed-off-by: Tony Najjar <[email protected]>

* reformat

Signed-off-by: Tony Najjar <[email protected]>

* revert

Signed-off-by: Tony Najjar <[email protected]>

* revert

Signed-off-by: Tony Najjar <[email protected]>

* fix warn msg

Signed-off-by: Tony Najjar <[email protected]>

* fix test

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

* improve

Signed-off-by: Tony Najjar <[email protected]>

* fix format

Signed-off-by: Tony Najjar <[email protected]>

* change to info

Signed-off-by: Tony Najjar <[email protected]>

---------

Signed-off-by: Tony Najjar <[email protected]>

* fix simple smoother failing during final approach (#4817)

* new test case for end of path approach

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

* modify tests to match the more permissive smoother policy

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

* implement steve's suggestions

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

---------

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

* Add acc limit consideration to avoid overshooting in RotationShimController (#4864)

* Add acc limit consideration to avoid overshoot in RotationShimController

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

* Add acceleration limit tests to RotationShimController

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

---------

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

* 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 <[email protected]>

* [BtActionNode] [BtServiceNode] clear between calls (#4887)

Signed-off-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

* 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 <[email protected]>

* Add option to use open-loop control with Rotation Shim (#4880)

* Initial implementation

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

* replace feedback param with closed_loop

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

* Reset last_angular_vel_ in activate method

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

* Add closed_loop parameter to dynamicParametersCallback

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

* Add tests

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

* Override reset function

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

---------

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

* Fix unstable test in nav2 util (#4894)

* Fix unstable test in nav2 util

Signed-off-by: mini-1235 <[email protected]>

* Fix linting

Signed-off-by: mini-1235 <[email protected]>

* Change 5s to 1s

Signed-off-by: mini-1235 <[email protected]>

---------

Signed-off-by: mini-1235 <[email protected]>

* Update bt2img syntax and bt pics (#4900)

Signed-off-by: Maurice-1235 <[email protected]>

* bumping to 1.3.5

Signed-off-by: Steve Macenski <[email protected]>

* Revert "Adapt GoalUpdater to update goals as well (#4771)"

This reverts commit 55d7387.

---------

Signed-off-by: Dylan De Coeyer <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Michael Ferguson <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
Signed-off-by: rayferric <[email protected]>
Signed-off-by: RBT22 <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Guillaume Doisy <[email protected]>
Signed-off-by: mini-1235 <[email protected]>
Signed-off-by: Maurice-1235 <[email protected]>
Co-authored-by: DylanDeCoeyer-Quimesis <[email protected]>
Co-authored-by: Alberto Tudela <[email protected]>
Co-authored-by: Michael Ferguson <[email protected]>
Co-authored-by: Tony Najjar <[email protected]>
Co-authored-by: Ray Ferric <[email protected]>
Co-authored-by: Balint Rozgonyi <[email protected]>
Co-authored-by: ewak <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: mini-1235 <[email protected]>
masf7g pushed a commit to quasi-robotics/navigation2 that referenced this pull request Feb 5, 2025
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.

2 participants