Fix SpeedController

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

@@ -280,7 +280,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_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml

@@ -5,7 +5,7 @@
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <PipelineSequence name="NavigateWithReplanning">
-      <SpeedController min_rate="0.1" max_rate="1.0" min_speed="0.0" max_speed="0.26" filter_duration="0.3">
+      <SpeedController min_rate="0.1" max_rate="1.0" min_speed="0.0" max_speed="0.26">
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
       </SpeedController>
       <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>

nav2_core/include/nav2_core/behavior_tree_navigator.hpp

@@ -223,6 +223,7 @@ class BehaviorTreeNavigator : public NavigatorBase
     blackboard->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", feedback_utils.tf);  // NOLINT
     blackboard->set<bool>("initial_pose_received", false);  // NOLINT
     blackboard->set<int>("number_recoveries", 0);  // NOLINT
+    blackboard->set<std::shared_ptr<nav2_util::OdomSmoother>>("odom_smoother", odom_smoother);  // NOLINT
 
     return configure(parent_node, odom_smoother) && ok;
   }

nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp

@@ -29,6 +29,8 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
 
+#include "nav2_util/odometry_utils.hpp"
+
 #include "rclcpp/rclcpp.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
@@ -54,6 +56,8 @@ class BehaviorTreeHandler
     tf_->setUsingDedicatedThread(true);
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, node_, false);
 
+    odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node_);
+
     const std::vector<std::string> plugin_libs = {
       "nav2_compute_path_to_pose_action_bt_node",
       "nav2_compute_path_through_poses_action_bt_node",
@@ -137,6 +141,7 @@ class BehaviorTreeHandler
     blackboard->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_);  // NOLINT
     blackboard->set<bool>("initial_pose_received", false);  // NOLINT
     blackboard->set<int>("number_recoveries", 0);  // NOLINT
+    blackboard->set<std::shared_ptr<nav2_util::OdomSmoother>>("odom_smoother", odom_smoother_);  // NOLINT
 
     // set dummy goal on blackboard
     geometry_msgs::msg::PoseStamped goal;
@@ -184,6 +189,8 @@ class BehaviorTreeHandler
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
+  std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
+
   BT::BehaviorTreeFactory factory_;
 };