Merge branch 'ros-navigation:main' into main

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v29\
+              - "<< parameters.key >>-v31\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v29\
+              - "<< parameters.key >>-v31\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v29\
+            key: "<< parameters.key >>-v31\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_behavior_tree/plugins/control/recovery_node.cpp

@@ -37,83 +37,83 @@ BT::NodeStatus RecoveryNode::tick()
     throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
   }
 
+  if (retry_count_ > number_of_retries_) {
+    halt();
+    return BT::NodeStatus::FAILURE;
+  }
   setStatus(BT::NodeStatus::RUNNING);
 
-  while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) {
-    TreeNode * child_node = children_nodes_[current_child_idx_];
-    const BT::NodeStatus child_status = child_node->executeTick();
-
-    if (current_child_idx_ == 0) {
-      switch (child_status) {
-        case BT::NodeStatus::SKIPPED:
-          // If first child is skipped, the entire branch is considered skipped
-          halt();
-          return BT::NodeStatus::SKIPPED;
-
-        case BT::NodeStatus::SUCCESS:
-          // reset node and return success when first child returns success
-          halt();
-          return BT::NodeStatus::SUCCESS;
-
-        case BT::NodeStatus::RUNNING:
-          return BT::NodeStatus::RUNNING;
-
-        case BT::NodeStatus::FAILURE:
-          {
-            if (retry_count_ < number_of_retries_) {
-              // halt first child and tick second child in next iteration
-              ControlNode::haltChild(0);
-              current_child_idx_++;
-              break;
-            } else {
-              // reset node and return failure when max retries has been exceeded
-              halt();
-              return BT::NodeStatus::FAILURE;
-            }
-          }
-
-        default:
-          throw BT::LogicError("A child node must never return IDLE");
-      }  // end switch
-
-    } else if (current_child_idx_ == 1) {
-      switch (child_status) {
-        case BT::NodeStatus::SKIPPED:
-          {
-            // if we skip the recovery (maybe a precondition fails), then we
-            // should assume that no recovery is possible. For this reason,
-            // we should return FAILURE and reset the index.
-            // This does not count as a retry.
-            current_child_idx_ = 0;
-            ControlNode::haltChild(1);
+  TreeNode * child_node = children_nodes_[current_child_idx_];
+  const BT::NodeStatus child_status = child_node->executeTick();
+
+  if (current_child_idx_ == 0) {
+    switch (child_status) {
+      case BT::NodeStatus::SKIPPED:
+        // If first child is skipped, the entire branch is considered skipped
+        halt();
+        return BT::NodeStatus::SKIPPED;
+
+      case BT::NodeStatus::SUCCESS:
+        // reset node and return success when first child returns success
+        halt();
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::FAILURE:
+        {
+          if (retry_count_ < number_of_retries_) {
+            // halt first child and tick second child in next iteration
+            ControlNode::haltChild(0);
+            current_child_idx_ = 1;
+            return BT::NodeStatus::RUNNING;
+          } else {
+            // reset node and return failure when max retries has been exceeded
+            halt();
             return BT::NodeStatus::FAILURE;
           }
-        case BT::NodeStatus::RUNNING:
-          return child_status;
-
-        case BT::NodeStatus::SUCCESS:
-          {
-            // halt second child, increment recovery count, and tick first child in next iteration
-            ControlNode::haltChild(1);
-            retry_count_++;
-            current_child_idx_ = 0;
-          }
-          break;
-
-        case BT::NodeStatus::FAILURE:
-          // reset node and return failure if second child fails
-          halt();
+        }
+
+      default:
+        throw BT::LogicError("A child node must never return IDLE");
+    }  // end switch
+
+  } else if (current_child_idx_ == 1) {
+    switch (child_status) {
+      case BT::NodeStatus::SKIPPED:
+        {
+          // if we skip the recovery (maybe a precondition fails), then we
+          // should assume that no recovery is possible. For this reason,
+          // we should return FAILURE and reset the index.
+          // This does not count as a retry.
+          current_child_idx_ = 0;
+          ControlNode::haltChild(1);
           return BT::NodeStatus::FAILURE;
+        }
+      case BT::NodeStatus::RUNNING:
+        return child_status;
+
+      case BT::NodeStatus::SUCCESS:
+        {
+          // halt second child, increment recovery count, and tick first child in next iteration
+          ControlNode::haltChild(1);
+          retry_count_++;
+          current_child_idx_ = 0;
+          return BT::NodeStatus::RUNNING;
+        }
 
-        default:
-          throw BT::LogicError("A child node must never return IDLE");
-      }  // end switch
-    }
-  }  // end while loop
+      case BT::NodeStatus::FAILURE:
+        // reset node and return failure if second child fails
+        halt();
+        return BT::NodeStatus::FAILURE;
 
-  // reset node and return failure
+      default:
+        throw BT::LogicError("A child node must never return IDLE");
+    }  // end switch
+  }
   halt();
-  return BT::NodeStatus::FAILURE;
+  throw BT::LogicError("A recovery node has exactly 2 children and should never reach here");
 }
 
 void RecoveryNode::halt()

nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp

@@ -105,6 +105,7 @@ TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child)
   first_child_->changeStatus(BT::NodeStatus::IDLE);
   EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   second_child_->changeStatus(BT::NodeStatus::IDLE);
   EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
 }
@@ -119,8 +120,9 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry)
   first_child_->returnSuccessOn(1);
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
   second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
-  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS);
   EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
   EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
 }
@@ -130,15 +132,17 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry)
   // first child fails, second child fails
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
   second_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
-  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
   EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
 
   // first child fails, second child succeeds, then first child fails (one retry)
   first_child_->returnFailureOn(1);
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
   second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
@@ -158,6 +162,7 @@ TEST_F(RecoveryNodeTestFixture, test_skipping)
   // first child fails, second child skipped
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
   second_child_->changeStatus(BT::NodeStatus::SKIPPED);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -220,6 +220,7 @@ class Tester : public ::testing::Test
 Tester::Tester()
 {
   cm_ = std::make_shared<CollisionMonitorWrapper>();
+  cm_->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
 
   footprint_pub_ = cm_->create_publisher<geometry_msgs::msg::PolygonStamped>(
     FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());

nav2_docking/opennav_docking/test/test_docking_server.py

@@ -17,7 +17,7 @@
 import unittest
 
 from action_msgs.msg import GoalStatus
-from geometry_msgs.msg import TransformStamped, Twist
+from geometry_msgs.msg import TransformStamped, Twist, TwistStamped
 from launch import LaunchDescription
 from launch_ros.actions import Node
 import launch_testing
@@ -90,8 +90,8 @@ def tearDownClass(cls):
         rclpy.shutdown()
 
     def command_velocity_callback(self, msg):
-        self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z))
-        self.command = msg
+        self.node.get_logger().info('Command: %f %f' % (msg.twist.linear.x, msg.twist.angular.z))
+        self.command = msg.twist
 
     def timer_callback(self):
         # Propagate command
@@ -155,7 +155,7 @@ def test_docking_server(self):
 
         # Subscribe to command velocity
         self.node.create_subscription(
-            Twist,
+            TwistStamped,
             'cmd_vel',
             self.command_velocity_callback,
             10

nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

@@ -74,7 +74,7 @@ def __init__(self):
         self.declare_parameter('scan_frame_id', 'base_scan')
         self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value
 
-        self.declare_parameter('enable_stamped_cmd_vel', False)
+        self.declare_parameter('enable_stamped_cmd_vel', True)
         use_stamped = self.get_parameter('enable_stamped_cmd_vel').get_parameter_value().bool_value
 
         self.declare_parameter('scan_publish_dur', 0.1)

nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp

@@ -54,13 +54,13 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester()
     node_->create_publisher<std_msgs::msg::Empty>("preempt_teleop", 10);
 
   cmd_vel_pub_ =
-    node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel_teleop", 10);
+    node_->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel_teleop", 10);
 
   subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
 
-  filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
+  filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
     "cmd_vel",
     rclcpp::SystemDefaultsQoS(),
     std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1));
@@ -167,9 +167,9 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest(
   counter_ = 0;
   auto start_time = std::chrono::system_clock::now();
   while (rclcpp::ok()) {
-    geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist();
-    cmd_vel.linear.x = lin_vel;
-    cmd_vel.angular.z = ang_vel;
+    geometry_msgs::msg::TwistStamped cmd_vel = geometry_msgs::msg::TwistStamped();
+    cmd_vel.twist.linear.x = lin_vel;
+    cmd_vel.twist.angular.z = ang_vel;
     cmd_vel_pub_->publish(cmd_vel);
 
     if (counter_ > 1) {
@@ -265,9 +265,9 @@ void AssistedTeleopBehaviorTester::amclPoseCallback(
 }
 
 void AssistedTeleopBehaviorTester::filteredVelCallback(
-  geometry_msgs::msg::Twist::SharedPtr msg)
+  geometry_msgs::msg::TwistStamped::SharedPtr msg)
 {
-  if (msg->linear.x == 0.0f) {
+  if (msg->twist.linear.x == 0.0f) {
     counter_++;
   } else {
     counter_ = 0;

nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp

@@ -24,7 +24,7 @@
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
 #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 #include "nav2_msgs/action/assisted_teleop.hpp"
@@ -68,7 +68,7 @@ class AssistedTeleopBehaviorTester
 
   void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
 
-  void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg);
+  void filteredVelCallback(geometry_msgs::msg::TwistStamped::SharedPtr msg);
 
   unsigned int counter_;
   bool is_active_;
@@ -83,11 +83,11 @@ class AssistedTeleopBehaviorTester
   // Publishers
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
   rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr preempt_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_pub_;
 
   // Subscribers
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr filtered_vel_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr filtered_vel_sub_;
 
   // Action client to call AssistedTeleop action
   rclcpp_action::Client<AssistedTeleop>::SharedPtr client_ptr_;

nav2_util/include/nav2_util/twist_publisher.hpp

@@ -61,7 +61,7 @@ class TwistPublisher
     using nav2_util::declare_parameter_if_not_declared;
     declare_parameter_if_not_declared(
       node, "enable_stamped_cmd_vel",
-      rclcpp::ParameterValue{false});
+      rclcpp::ParameterValue{true});
     node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
     if (is_stamped_) {
       twist_stamped_pub_ = node->create_publisher<geometry_msgs::msg::TwistStamped>(
@@ -122,7 +122,7 @@ class TwistPublisher
 
 protected:
   std::string topic_;
-  bool is_stamped_;
+  bool is_stamped_{true};
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::TwistStamped>::SharedPtr
     twist_stamped_pub_;

nav2_util/include/nav2_util/twist_subscriber.hpp

@@ -92,7 +92,7 @@ class TwistSubscriber
   {
     nav2_util::declare_parameter_if_not_declared(
       node, "enable_stamped_cmd_vel",
-      rclcpp::ParameterValue(false));
+      rclcpp::ParameterValue(true));
     node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
     if (is_stamped_) {
       twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
@@ -125,7 +125,7 @@ class TwistSubscriber
   {
     nav2_util::declare_parameter_if_not_declared(
       node, "enable_stamped_cmd_vel",
-      rclcpp::ParameterValue(false));
+      rclcpp::ParameterValue(true));
     node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
     if (is_stamped_) {
       twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
@@ -140,7 +140,7 @@ class TwistSubscriber
 
 protected:
   //! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel
-  bool is_stamped_{false};
+  bool is_stamped_{true};
   //! @brief The subscription when using Twist
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_ {nullptr};
   //! @brief The subscription when using TwistStamped

nav2_util/test/test_twist_publisher.cpp

@@ -29,6 +29,7 @@ TEST(TwistPublisher, Unstamped)
   rclcpp::init(0, nullptr);
   auto pub_node = std::make_shared<nav2_util::LifecycleNode>("pub_node", "");
   pub_node->configure();
+  pub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
   auto vel_publisher = std::make_unique<nav2_util::TwistPublisher>(pub_node, "cmd_vel", 1);
   ASSERT_EQ(vel_publisher->get_subscription_count(), 0);
   EXPECT_FALSE(vel_publisher->is_activated());