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 <[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]>
ros-navigation · Feb 5, 2025 · 0aee198 · 0aee198
1 parent 44896e9
commit 0aee198
Show file tree

Hide file tree

Showing 75 changed files with 817 additions and 389 deletions.
diff --git a/nav2_amcl/package.xml b/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

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/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();
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/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();
 

diff --git a/nav2_behavior_tree/package.xml b/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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>

diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/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;

diff --git a/nav2_behaviors/package.xml b/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="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_bringup/package.xml b/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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_bt_navigator/doc/parallel_w_recovery.png b/nav2_bt_navigator/doc/parallel_w_recovery.png
diff --git a/nav2_bt_navigator/package.xml b/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_bt_navigator</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Nav2 BT Navigator Server</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/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="[email protected]">Alexey Merzlyakov</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_common/package.xml b/nav2_common/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_common</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/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_constrained_smoother</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="[email protected]">Matej Vargovcik</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_controller/package.xml b/nav2_controller/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_controller</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Controller action interface</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_core/package.xml b/nav2_core/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_core</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">Carl Delsey</maintainer>

diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/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_costmap_2d</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/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>opennav_docking</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>A Task Server for robot charger docking</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
@@ -144,7 +144,10 @@ bool Controller::isTrajectoryCollisionFree(
   }
 
   // Generate path
-  for (double t = 0; t < projection_time_; t += simulation_time_step_) {
+  double distance = std::numeric_limits<double>::max();
+  unsigned int max_iter = static_cast<unsigned int>(ceil(projection_time_ / simulation_time_step_));
+
+  do{
     // Apply velocities to calculate next pose
     next_pose.pose = control_law_->calculateNextPose(
       simulation_time_step_, target_pose, next_pose.pose, backward);
@@ -177,7 +180,10 @@ bool Controller::isTrajectoryCollisionFree(
       trajectory_pub_->publish(trajectory);
       return false;
     }
-  }
+
+    // Check if we reach the goal
+    distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
+  }while(distance > 1e-2 && trajectory.poses.size() < max_iter);
 
   trajectory_pub_->publish(trajectory);
 

diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py
@@ -19,23 +19,37 @@
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import TransformStamped, Twist
 from launch import LaunchDescription
+# from launch.actions import SetEnvironmentVariable
 from launch_ros.actions import Node
 import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import launch_testing.markers
+import launch_testing.util
 from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot
 import pytest
 import rclpy
-from rclpy.action import ActionClient, ActionServer
+from rclpy.action.client import ActionClient
+from rclpy.action.server import ActionServer
 from sensor_msgs.msg import BatteryState
 from tf2_ros import TransformBroadcaster
 
 
 # This test can be run standalone with:
 # python3 -u -m pytest test_docking_server.py -s
 
+# If python3-flaky is installed, you can run the test multiple times to
+# try to identify flaky ness.
+# python3 -u -m pytest --force-flaky --min-passes 3 --max-runs 5 -s -v test_docking_server.py
+
 @pytest.mark.rostest
+# @pytest.mark.flaky
+# @pytest.mark.flaky(max_runs=5, min_passes=3)
 def generate_test_description():
 
     return LaunchDescription([
+        # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        # SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
         Node(
             package='opennav_docking',
             executable='opennav_docking',
@@ -74,21 +88,26 @@ class TestDockingServer(unittest.TestCase):
     @classmethod
     def setUpClass(cls):
         rclpy.init()
-        # Latest odom -> base_link
-        cls.x = 0.0
-        cls.y = 0.0
-        cls.theta = 0.0
-        # Track charge state
-        cls.is_charging = False
-        # Latest command velocity
-        cls.command = Twist()
-        cls.node = rclpy.create_node('test_docking_server')
 
     @classmethod
     def tearDownClass(cls):
-        cls.node.destroy_node()
         rclpy.shutdown()
 
+    def setUp(self):
+        # Create a ROS node for tests
+        # Latest odom -> base_link
+        self.x = 0.0
+        self.y = 0.0
+        self.theta = 0.0
+        # Track charge state
+        self.is_charging = False
+        # Latest command velocity
+        self.command = Twist()
+        self.node = rclpy.create_node('test_docking_server')
+
+    def tearDown(self):
+        self.node.destroy_node()
+
     def command_velocity_callback(self, msg):
         self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z))
         self.command = msg
@@ -175,24 +194,32 @@ def test_docking_server(self):
             'navigate_to_pose',
             self.nav_execute_callback)
 
-        # Spin once so that TF is published
-        rclpy.spin_once(self.node, timeout_sec=0.1)
+        # Publish transform
+        self.publish()
+
+        # Run for 1 seconds to allow tf to propogate
+        for _ in range(10):
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            time.sleep(0.1)
 
         # Test docking action
         self.action_result = []
-        self.dock_action_client.wait_for_server(timeout_sec=5.0)
+        assert self.dock_action_client.wait_for_server(timeout_sec=5.0), \
+               'dock_robot service not available'
+
         goal = DockRobot.Goal()
         goal.use_dock_id = True
         goal.dock_id = 'test_dock'
         future = self.dock_action_client.send_goal_async(
             goal, feedback_callback=self.action_feedback_callback)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal_handle not accepted'
         result_future_original = self.goal_handle.get_result_async()
 
         # Run for 2 seconds
-        for i in range(20):
+        for _ in range(20):
             rclpy.spin_once(self.node, timeout_sec=0.1)
             time.sleep(0.1)
 
@@ -201,7 +228,8 @@ def test_docking_server(self):
             goal, feedback_callback=self.action_feedback_callback)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal_handle not accepted'
         result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self.node, result_future)
         self.action_result.append(result_future.result())
@@ -216,7 +244,7 @@ def test_docking_server(self):
         self.node.get_logger().info('Goal preempted')
 
         # Run for 0.5 seconds
-        for i in range(5):
+        for _ in range(5):
             rclpy.spin_once(self.node, timeout_sec=0.1)
             time.sleep(0.1)
 
@@ -230,7 +258,8 @@ def test_docking_server(self):
             goal, feedback_callback=self.action_feedback_callback)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal_handle not accepted'
         result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self.node, result_future)
         self.action_result.append(result_future.result())
@@ -247,10 +276,19 @@ def test_docking_server(self):
         future = self.undock_action_client.send_goal_async(goal)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal_handle not accepted'
         result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self.node, result_future)
         self.action_result.append(result_future.result())
 
         self.assertEqual(self.action_result[3].status, GoalStatus.STATUS_SUCCEEDED)
         self.assertTrue(self.action_result[3].result.success)
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+
+    def test_exit_code(self, proc_info):
+        # Check that all processes in the launch exit with code 0
+        launch_testing.asserts.assertExitCodes(proc_info)
diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/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>opennav_docking_bt</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>A set of BT nodes and XMLs for docking</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/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>opennav_docking_core</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>A set of headers for plugins core to the opennav docking framework</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>The costmap_queue package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/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>dwb_core</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>DWB core interfaces package</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>The dwb_critics package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp
@@ -242,7 +242,9 @@ TEST(ObstacleFootprint, LineCost)
   costmap_ros->getCostmap()->setCost(4, 3, 100);
   costmap_ros->getCostmap()->setCost(4, 4, 100);
 
-  ASSERT_EQ(critic->lineCost(3, 3, 0, 50), 50);   // all 50
+  auto max_y_in_grid_coordinates = costmap_ros->getCostmap()->getSizeInCellsY() - 1;
+  ASSERT_EQ(max_y_in_grid_coordinates, 49);
+  ASSERT_EQ(critic->lineCost(3, 3, 0, max_y_in_grid_coordinates), 50);   // all 50
   ASSERT_EQ(critic->lineCost(4, 4, 0, 10), 100);  // all 100
   ASSERT_EQ(critic->lineCost(0, 50, 3, 3), 100);  // pass 50 and 100
 }

diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/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>dwb_msgs</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>1.3.4</version>
+  <version>1.3.5</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core