Jazzy sync 5: Feb 4, 2025 (#4902)

* 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>

Author: 11 people

nav2_amcl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -194,6 +194,10 @@ class BtActionNode : public BT::ActionNodeBase
       // reset the flag to send the goal or not, allowing the user the option to set it in on_tick
       should_send_goal_ = true;
 
+      // Clear the input and output messages to make sure we have no leftover from previous calls
+      goal_ = typename ActionT::Goal();
+      result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
+
       // user defined callback, may modify "should_send_goal_".
       on_tick();
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -138,6 +138,9 @@ class BtServiceNode : public BT::ActionNodeBase
       // allowing the user the option to set it in on_tick
       should_send_request_ = true;
 
+      // Clear the input request to make sure we have no leftover from previous calls
+      request_ = std::make_shared<typename ServiceT::Request>();
+
       // user defined callback, may modify "should_send_request_".
       on_tick();
 

nav2_behavior_tree/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Nav2 behavior tree wrappers, nodes, and utilities</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -102,7 +102,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       RCLCPP_WARN(
         this->logger_,
         "Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");
-      return ResultStatus{Status::FAILED, ActionT::Result::NONE};
+      return ResultStatus{Status::FAILED, ActionT::Result::TIMEOUT};
     }
 
     geometry_msgs::msg::PoseStamped current_pose;

nav2_behaviors/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Nav2 behavior server</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bt_navigator/doc/parallel_w_recovery.png

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Nav2 BT Navigator Server</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

nav2_bt_navigator/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Collision Monitor</description>
   <maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>