ros-navigation
diff --git a/‎nav2_amcl/package.xml
+1-1 b/‎nav2_amcl/package.xml
+1-1
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+4 b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+4
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
+3 b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
+3
diff --git a/‎nav2_behavior_tree/package.xml
+1-1 b/‎nav2_behavior_tree/package.xml
+1-1
diff --git a/‎nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
+1-1 b/‎nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
+1-1
diff --git a/‎nav2_behaviors/package.xml
+1-1 b/‎nav2_behaviors/package.xml
+1-1
diff --git a/‎nav2_bringup/package.xml
+1-1 b/‎nav2_bringup/package.xml
+1-1
diff --git a/‎nav2_bt_navigator/doc/parallel_w_recovery.png
70.7 KB b/‎nav2_bt_navigator/doc/parallel_w_recovery.png
70.7 KB
diff --git a/‎nav2_bt_navigator/package.xml
+1-1 b/‎nav2_bt_navigator/package.xml
+1-1
diff --git a/‎nav2_collision_monitor/package.xml
+1-1 b/‎nav2_collision_monitor/package.xml
+1-1
diff --git a/‎nav2_common/package.xml
+1-1 b/‎nav2_common/package.xml
+1-1
diff --git a/‎nav2_constrained_smoother/package.xml
+1-1 b/‎nav2_constrained_smoother/package.xml
+1-1
diff --git a/‎nav2_controller/package.xml
+1-1 b/‎nav2_controller/package.xml
+1-1
diff --git a/‎nav2_core/package.xml
+1-1 b/‎nav2_core/package.xml
+1-1
diff --git a/‎nav2_costmap_2d/package.xml
+1-1 b/‎nav2_costmap_2d/package.xml
+1-1
diff --git a/‎nav2_docking/opennav_docking/package.xml
+1-1 b/‎nav2_docking/opennav_docking/package.xml
+1-1
diff --git a/‎nav2_docking/opennav_docking/src/controller.cpp
+8-2 b/‎nav2_docking/opennav_docking/src/controller.cpp
+8-2
diff --git a/‎nav2_docking/opennav_docking/test/test_docking_server.py
+58-20 b/‎nav2_docking/opennav_docking/test/test_docking_server.py
+58-20
diff --git a/‎nav2_docking/opennav_docking_bt/package.xml
+1-1 b/‎nav2_docking/opennav_docking_bt/package.xml
+1-1
diff --git a/‎nav2_docking/opennav_docking_core/package.xml
+1-1 b/‎nav2_docking/opennav_docking_core/package.xml
+1-1
diff --git a/‎nav2_dwb_controller/costmap_queue/package.xml
+1-1 b/‎nav2_dwb_controller/costmap_queue/package.xml
+1-1
diff --git a/‎nav2_dwb_controller/dwb_core/package.xml
+1-1 b/‎nav2_dwb_controller/dwb_core/package.xml
+1-1
diff --git a/‎nav2_dwb_controller/dwb_critics/package.xml
+1-1 b/‎nav2_dwb_controller/dwb_critics/package.xml
+1-1
diff --git a/‎nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp
+3-1 b/‎nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp
+3-1
diff --git a/‎nav2_dwb_controller/dwb_msgs/package.xml
+1-1 b/‎nav2_dwb_controller/dwb_msgs/package.xml
+1-1
diff --git a/‎nav2_dwb_controller/dwb_plugins/package.xml
+1-1 b/‎nav2_dwb_controller/dwb_plugins/package.xml
+1-1
@@ -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
 
@@ -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();
 
 
@@ -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();
 
 
@@ -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>
 
@@ -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;
 
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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
 
@@ -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>
 
@@ -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);
 
 
@@ -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)
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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>
 
@@ -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
 }
 
@@ -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>
 
@@ -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
Original file line number	Diff line number	Diff line change
`@@ -102,7 +102,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>`
`102`	`102`	`RCLCPP_WARN(`
`103`	`103`	`this->logger_,`
`104`	`104`	`"Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");`
`105`		`- return ResultStatus{Status::FAILED, ActionT::Result::NONE};`
	`105`	`+ return ResultStatus{Status::FAILED, ActionT::Result::TIMEOUT};`
`106`	`106`	`}`
`107`	`107`
`108`	`108`	`geometry_msgs::msg::PoseStamped current_pose;`