Add SpeedController nodes to adjust replanning rate according to speed (

#1744)

* Add SpeedController decorator node and update default BTs

* Fix lint errors

* Add OdomSmoother class and fix SpeedController to use smoothed velocity

Signed-off-by: Sarthak Mittal <[email protected]>

* Add duration input port to tree nodes xml for groot

Signed-off-by: Sarthak Mittal <[email protected]>

* Revert BT images

Signed-off-by: Sarthak Mittal <[email protected]>

* Address reviewer's comments

Signed-off-by: Sarthak Mittal <[email protected]>

* Throw BT exception when rate <= 0

Signed-off-by: Sarthak Mittal <[email protected]>

* Add simple behavior tree using speed controller

Signed-off-by: Sarthak Mittal <[email protected]>

* Set default value to maximum rate on IDLE

* Add test for speed controller

Signed-off-by: Sarthak Mittal <[email protected]>

* Use smart pointers in tests

Signed-off-by: Sarthak Mittal <[email protected]>

* Fix test

Signed-off-by: Sarthak Mittal <[email protected]>

* Move speed controller header file

Signed-off-by: Sarthak Mittal <[email protected]>

* Update speed controller to reset only on new goal

Signed-off-by: Sarthak Mittal <[email protected]>

* Fix test

Signed-off-by: Sarthak Mittal <[email protected]>

* Add params to parameter list

Signed-off-by: Sarthak Mittal <[email protected]>

* Fix memory leak

Signed-off-by: Sarthak Mittal <[email protected]>

doc/parameters/param_list.md

@@ -9,6 +9,7 @@
 | transform_tolerance | 0.1 | TF transform tolerance |
 | global_frame | "map" | Reference frame |
 | robot_base_frame | "base_link" | Robot base frame |
+| odom_topic | "odom" | Odometry topic |
 
 # costmaps
 
@@ -582,3 +583,13 @@ Namespace: /parent_ns/local_ns
 | Input Port | Default | Description |
 | ---------- | ------- | ----------- |
 | hz | 10.0 | Rate to throttle |
+
+### BT Node SpeedController
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| min_rate | 0.1 | Minimum rate (hz) |
+| max_rate | 1.0 | Maximum rate (hz) |
+| min_speed | 0.0 | Minimum speed (m/s) |
+| max_speed | 0.5 | Maximum speed (m/s) |
+| filter_duration | 0.3 | Duration (secs) for velocity smoothing filter |

nav2_behavior_tree/CMakeLists.txt

@@ -98,6 +98,9 @@ list(APPEND plugin_libs nav2_rate_controller_bt_node)
 add_library(nav2_distance_controller_bt_node SHARED plugins/decorator/distance_controller.cpp)
 list(APPEND plugin_libs nav2_distance_controller_bt_node)
 
+add_library(nav2_speed_controller_bt_node SHARED plugins/decorator/speed_controller.cpp)
+list(APPEND plugin_libs nav2_speed_controller_bt_node)
+
 add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp)
 list(APPEND plugin_libs nav2_recovery_node_bt_node)
 

nav2_behavior_tree/README.md

@@ -75,6 +75,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |
 | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
 | DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
+| SpeedController | Decorator | A node that controls the tick rate for its child based on the current robot speed. This decorator offers the most flexibility as the user can set the minimum/maximum tick rate which is adjusted according to the current robot speed. |
 | RecoveryNode | Control | The RecoveryNode is a control flow node with two children.  It returns SUCCESS if and only if the first child returns SUCCESS. The second child will be executed only if the first child returns FAILURE. If the second child SUCCEEDS, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning FAILURE. In nav2, the RecoveryNode is included in Behavior Trees to implement recovery actions upon failures.
 | Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is using in nav2 Behavior Trees as a recovery behavior. |
 | PipelineSequence | Control | Ticks the first child till it succeeds, then ticks the first and second children till the second one succeeds. It then ticks the first, second, and third children until the third succeeds, and so on, and so on. If at any time a child returns RUNNING, that doesn't change the behavior. If at any time a child returns FAILURE, that stops all children and returns FAILURE overall.|

nav2_behavior_tree/include/nav2_behavior_tree/plugins/speed_controller.hpp

@@ -0,0 +1,100 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__SPEED_CONTROLLER_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__SPEED_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <deque>
+
+#include "nav_msgs/msg/odometry.hpp"
+#include "nav2_util/odometry_utils.hpp"
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class SpeedController : public BT::DecoratorNode
+{
+public:
+  SpeedController(
+    const std::string & name,
+    const BT::NodeConfiguration & conf);
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("min_rate", 0.1, "Minimum rate"),
+      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")
+    };
+  }
+
+private:
+  BT::NodeStatus tick() override;
+
+  // Scale the rate based speed
+  inline double getScaledRate(const double & speed)
+  {
+    return std::max(
+      std::min(
+        (((speed - min_speed_) / d_speed_) * d_rate_) + min_rate_,
+        max_rate_), min_rate_);
+  }
+
+  // Update period based on current smoothed speed and reset timer
+  inline void updatePeriod()
+  {
+    auto velocity = odom_smoother_->getTwist();
+    double speed = std::hypot(velocity.linear.x, velocity.linear.y);
+    double rate = getScaledRate(speed);
+    period_ = 1.0 / rate;
+  }
+
+  rclcpp::Node::SharedPtr node_;
+
+  // To keep track of time to reset
+  rclcpp::Time start_;
+
+  // To get a smoothed velocity
+  std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
+
+  bool first_tick_;
+
+  // Time period after which child node should be ticked
+  double period_;
+
+  // Rates thresholds to tick child node
+  double min_rate_;
+  double max_rate_;
+  double d_rate_;
+
+  // Speed thresholds
+  double min_speed_;
+  double max_speed_;
+  double d_speed_;
+
+  // current goal
+  geometry_msgs::msg::PoseStamped goal_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__SPEED_CONTROLLER_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -72,5 +72,13 @@
       <input_port name="distance">Distance</input_port>
     </Decorator>
 
+    <Decorator ID="SpeedController">
+      <input_port name="min_rate">Minimum rate</input_port>
+      <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>
+
   </TreeNodesModel>
 </root>

nav2_behavior_tree/plugins/decorator/speed_controller.cpp

@@ -0,0 +1,114 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/speed_controller.hpp"
+
+namespace nav2_behavior_tree
+{
+
+SpeedController::SpeedController(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf),
+  first_tick_(false),
+  period_(1.0),
+  min_rate_(0.1),
+  max_rate_(1.0),
+  min_speed_(0.0),
+  max_speed_(0.5)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  getInput("min_rate", min_rate_);
+  getInput("max_rate", max_rate_);
+  getInput("min_speed", min_speed_);
+  getInput("max_speed", max_speed_);
+
+  if (min_rate_ <= 0.0 || max_rate_ <= 0.0) {
+    std::string err_msg = "SpeedController node cannot have rate <= 0.0";
+    RCLCPP_FATAL(node_->get_logger(), err_msg);
+    throw BT::BehaviorTreeException(err_msg);
+  }
+
+  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);
+}
+
+inline BT::NodeStatus SpeedController::tick()
+{
+  auto current_goal = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
+
+  if (goal_ != current_goal) {
+    // Reset state and set period to max since we have a new goal
+    period_ = 1.0 / max_rate_;
+    start_ = node_->now();
+    first_tick_ = true;
+    goal_ = current_goal;
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  auto elapsed = node_->now() - start_;
+
+  // The child gets ticked the first time through and any time the period has
+  // expired. In addition, once the child begins to run, it is ticked each time
+  // 'til completion
+  if (first_tick_ || (child_node_->status() == BT::NodeStatus::RUNNING) ||
+    elapsed.seconds() >= period_)
+  {
+    first_tick_ = false;
+
+    // update period if the last period is exceeded
+    if (elapsed.seconds() >= period_) {
+      updatePeriod();
+      start_ = node_->now();
+    }
+
+    const BT::NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state) {
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::SUCCESS:
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::FAILURE:
+      default:
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  return status();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::SpeedController>("SpeedController");
+}

nav2_behavior_tree/test/CMakeLists.txt

@@ -8,6 +8,16 @@ ament_target_dependencies(test_decorator_distance_controller
   ${dependencies}
 )
 
+ament_add_gtest(test_decorator_speed_controller
+  plugins/decorator/test_speed_controller.cpp
+)
+target_link_libraries(test_decorator_speed_controller
+  nav2_speed_controller_bt_node
+)
+ament_target_dependencies(test_decorator_speed_controller
+  ${dependencies}
+)
+
 ament_add_gtest(test_condition_distance_traveled
   plugins/condition/test_distance_traveled.cpp
 )

nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp

@@ -31,9 +31,8 @@ class DistanceControllerTestFixture : public ::testing::Test
 public:
   static void SetUpTestCase()
   {
-    transform_handler_ = new nav2_behavior_tree::TransformHandler();
-    config_ = new BT::NodeConfiguration();
-    dummy_node_ = new nav2_behavior_tree::DummyNode();
+    transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>();
+    config_ = std::make_shared<BT::NodeConfiguration>();
 
     // Create the blackboard that will be shared by all of the nodes in the tree
     config_->blackboard = BT::Blackboard::create();
@@ -57,34 +56,33 @@ class DistanceControllerTestFixture : public ::testing::Test
   static void TearDownTestCase()
   {
     transform_handler_->deactivate();
-    delete transform_handler_;
-    delete config_;
     delete dummy_node_;
-    transform_handler_ = nullptr;
-    config_ = nullptr;
-    dummy_node_ = nullptr;
+    delete node_;
   }
 
   void SetUp()
   {
     node_ = new nav2_behavior_tree::DistanceController("distance_controller", *config_);
+    dummy_node_ = new nav2_behavior_tree::DummyNode();
     node_->setChild(dummy_node_);
   }
 
   void TearDown()
   {
+    dummy_node_ = nullptr;
     node_ = nullptr;
   }
 
 protected:
-  static nav2_behavior_tree::TransformHandler * transform_handler_;
-  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
+  static std::shared_ptr<BT::NodeConfiguration> config_;
   static nav2_behavior_tree::DistanceController * node_;
   static nav2_behavior_tree::DummyNode * dummy_node_;
 };
 
-nav2_behavior_tree::TransformHandler * DistanceControllerTestFixture::transform_handler_ = nullptr;
-BT::NodeConfiguration * DistanceControllerTestFixture::config_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::TransformHandler>
+DistanceControllerTestFixture::transform_handler_ = nullptr;
+std::shared_ptr<BT::NodeConfiguration> DistanceControllerTestFixture::config_ = nullptr;
 nav2_behavior_tree::DistanceController * DistanceControllerTestFixture::node_ = nullptr;
 nav2_behavior_tree::DummyNode * DistanceControllerTestFixture::dummy_node_ = nullptr;