Humble Sync 5 (Feb 10)

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -97,7 +97,9 @@ class BtActionNode : public BT::ActionNodeBase
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
-      throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
+      throw std::runtime_error(
+              std::string("Action server ") + action_name +
+              std::string(" not available"));
     }
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp

@@ -93,7 +93,9 @@ class BtCancelActionNode : public BT::ActionNodeBase
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
-      throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
+      throw std::runtime_error(
+              std::string("Action server ") + action_name +
+              std::string(" not available"));
     }
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp

@@ -57,7 +57,6 @@ class SpeedController : public BT::DecoratorNode
       BT::InputPort<double>("max_rate", 1.0, "Maximum rate"),
       BT::InputPort<double>("min_speed", 0.0, "Minimum speed"),
       BT::InputPort<double>("max_speed", 0.5, "Maximum speed"),
-      BT::InputPort<double>("filter_duration", 0.3, "Duration (secs) for velocity smoothing filter")
     };
   }
 

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -11,35 +11,35 @@
       <input_port name="backup_dist">Distance to backup</input_port>
       <input_port name="backup_speed">Speed at which to backup</input_port>
       <input_port name="time_allowance">Allowed time for reversing</input_port>
-      <input_port name="server_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="DriveOnHeading">
       <input_port name="dist_to_travel">Distance to travel</input_port>
       <input_port name="speed">Speed at which to travel</input_port>
       <input_port name="time_allowance">Allowed time for reversing</input_port>
-      <input_port name="server_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="CancelControl">
-      <input_port name="service_name">Service name to cancel the controller server</input_port>
+      <input_port name="server_name">Server name to cancel the controller server</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="CancelBackUp">
-      <input_port name="service_name">Service name to cancel the backup behavior</input_port>
+      <input_port name="server_name">Server name to cancel the backup behavior</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="CancelDriveOnHeading">
-      <input_port name="service_name">Service name to cancel the drive on heading behavior</input_port>
+      <input_port name="server_name">Service name to cancel the drive on heading behavior</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="CancelSpin">
-      <input_port name="service_name">Service name to cancel the spin behavior</input_port>
+      <input_port name="server_name">Server name to cancel the spin behavior</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
@@ -49,7 +49,7 @@
     </Action>
 
     <Action ID="CancelWait">
-      <input_port name="service_name">Service name to cancel the wait behavior</input_port>
+      <input_port name="server_name">Server name to cancel the wait behavior</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
@@ -74,15 +74,15 @@
       <input_port name="goal">Destination to plan to</input_port>
       <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
-      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
     </Action>
 
     <Action ID="ComputePathThroughPoses">
       <input_port name="goals">Destinations to plan through</input_port>
       <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
-      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
@@ -110,20 +110,20 @@
       <input_port name="controller_id" default="FollowPath"/>
       <input_port name="path">Path to follow</input_port>
       <input_port name="goal_checker_id" default="GoalChecker">Goal checker</input_port>
-      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="NavigateToPose">
         <input_port name="goal">Goal</input_port>
-        <input_port name="service_name">Service name</input_port>
+        <input_port name="server_name">Server name</input_port>
         <input_port name="server_timeout">Server timeout</input_port>
         <input_port name="behavior_tree">Behavior tree to run</input_port>
     </Action>
 
     <Action ID="NavigateThroughPoses">
         <input_port name="goals">Goals</input_port>
-        <input_port name="service_name">Service name</input_port>
+        <input_port name="server_name">Server name</input_port>
         <input_port name="server_timeout">Server timeout</input_port>
         <input_port name="behavior_tree">Behavior tree to run</input_port>
     </Action>
@@ -177,13 +177,13 @@
     <Action ID="Spin">
       <input_port name="spin_dist">Spin distance</input_port>
       <input_port name="time_allowance">Allowed time for spinning</input_port>
-      <input_port name="server_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="Wait">
       <input_port name="wait_duration">Wait time</input_port>
-      <input_port name="server_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
@@ -242,6 +242,24 @@
       <input_port name="server_timeout"> Server timeout </input_port>
     </Condition>
 
+    <Condition ID="WouldAControllerRecoveryHelp">
+      <input_port name="error_code">Error code</input_port>
+    </Condition>
+
+    <Condition ID="WouldAPlannerRecoveryHelp">
+      <input_port name="error_code">Error code</input_port>
+    </Condition>
+
+    <Condition ID="WouldASmootherRecoveryHelp">
+      <input_port name="error_code">Error code</input_port>
+    </Condition>
+
+    <Condition ID="AreErrorCodesPresent">
+      <input_port name="error_code">Error code</input_port>
+      <input_port name="error_codes_to_check">Error codes to check, user defined</input_port>
+    </Condition>
+
+
     <!-- ############################### CONTROL NODES ################################ -->
     <Control ID="PipelineSequence"/>
 
@@ -275,7 +293,6 @@
       <input_port name="max_rate">Maximum rate</input_port>
       <input_port name="min_speed">Minimum speed</input_port>
       <input_port name="max_speed">Maximum speed</input_port>
-      <input_port name="filter_duration">Duration (secs) for velocity smoothing filter</input_port>
     </Decorator>
 
     <Decorator ID="PathLongerOnApproach">

nav2_behavior_tree/plugins/decorator/speed_controller.cpp

@@ -50,18 +50,28 @@ SpeedController::SpeedController(
   d_rate_ = max_rate_ - min_rate_;
   d_speed_ = max_speed_ - min_speed_;
 
-  double duration = 0.3;
-  getInput("filter_duration", duration);
   std::string odom_topic;
   node_->get_parameter_or("odom_topic", odom_topic, std::string("odom"));
-  odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node_, duration, odom_topic);
+  odom_smoother_ = config().blackboard->get<std::shared_ptr<nav2_util::OdomSmoother>>(
+    "odom_smoother");
 }
 
 inline BT::NodeStatus SpeedController::tick()
 {
-  auto current_goal = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
-  auto current_goals =
-    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals");
+  if (status() == BT::NodeStatus::IDLE) {
+    // Reset since we're starting a new iteration of
+    // the speed controller (moving from IDLE to RUNNING)
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    period_ = 1.0 / max_rate_;
+    start_ = node_->now();
+    first_tick_ = true;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
 
   if (goal_ != current_goal || goals_ != current_goals) {
     // Reset state and set period to max since we have a new goal

nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp

@@ -34,6 +34,10 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi
 public:
   void SetUp()
   {
+    odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node_);
+    config_->blackboard->set<std::shared_ptr<nav2_util::OdomSmoother>>(
+      "odom_smoother", odom_smoother_);  // NOLINT
+
     geometry_msgs::msg::PoseStamped goal;
     goal.header.stamp = node_->now();
     config_->blackboard->set("goal", goal);
@@ -50,13 +54,17 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi
   {
     dummy_node_.reset();
     bt_node_.reset();
+    odom_smoother_.reset();
   }
 
 protected:
+  static std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
   static std::shared_ptr<nav2_behavior_tree::SpeedController> bt_node_;
   static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
 };
 
+std::shared_ptr<nav2_util::OdomSmoother>
+SpeedControllerTestFixture::odom_smoother_ = nullptr;
 std::shared_ptr<nav2_behavior_tree::SpeedController>
 SpeedControllerTestFixture::bt_node_ = nullptr;
 std::shared_ptr<nav2_behavior_tree::DummyNode>

nav2_bringup/launch/localization_launch.py

@@ -37,6 +37,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     use_composition = LaunchConfiguration('use_composition')
     container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
     use_respawn = LaunchConfiguration('use_respawn')
     log_level = LaunchConfiguration('log_level')
 
@@ -141,7 +142,7 @@ def generate_launch_description():
 
     load_composable_nodes = LoadComposableNodes(
         condition=IfCondition(use_composition),
-        target_container=container_name,
+        target_container=container_name_full,
         composable_node_descriptions=[
             ComposableNode(
                 package='nav2_map_server',

nav2_bringup/launch/navigation_launch.py

@@ -36,6 +36,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     use_composition = LaunchConfiguration('use_composition')
     container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
     use_respawn = LaunchConfiguration('use_respawn')
     log_level = LaunchConfiguration('log_level')
 
@@ -192,7 +193,7 @@ def generate_launch_description():
 
     load_composable_nodes = LoadComposableNodes(
         condition=IfCondition(use_composition),
-        target_container=container_name,
+        target_container=container_name_full,
         composable_node_descriptions=[
             ComposableNode(
                 package='nav2_controller',