From 0f5f60a1b186735776f1b59d75f1edf05b8214e1 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Fri, 20 Jan 2023 21:29:20 +0800 Subject: [PATCH 1/2] Fix SpeedController * Using blackboard get() method that does not throw error * Share odom_smoother on the blackboard Signed-off-by: Borong Yuan --- .../plugins/decorator/speed_controller.hpp | 1 - nav2_behavior_tree/nav2_tree_nodes.xml | 1 - .../plugins/decorator/speed_controller.cpp | 21 +++++++++++++------ .../navigate_w_replanning_speed.xml | 2 +- .../nav2_core/behavior_tree_navigator.hpp | 1 + 5 files changed, 17 insertions(+), 9 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 15322ef85b5..8150eb31098 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -57,7 +57,6 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), - BT::InputPort("filter_duration", 0.3, "Duration (secs) for velocity smoothing filter") }; } diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 6f2337e23d5..1a6791a632b 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -280,7 +280,6 @@ Maximum rate Minimum speed Maximum speed - Duration (secs) for velocity smoothing filter diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 26d96de77a7..2e739569c39 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -50,18 +50,27 @@ 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(node_, duration, odom_topic); + odom_smoother_ = config().blackboard->get>("odom_smoother"); } inline BT::NodeStatus SpeedController::tick() { - auto current_goal = config().blackboard->get("goal"); - auto current_goals = - config().blackboard->get>("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>("goals", goals_); + config().blackboard->get("goal", goal_); + period_ = 1.0 / max_rate_; + start_ = node_->now(); + first_tick_ = true; + } + + std::vector current_goals; + config().blackboard->get>("goals", current_goals); + geometry_msgs::msg::PoseStamped current_goal; + config().blackboard->get("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { // Reset state and set period to max since we have a new goal diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 97b70b7a62d..8134bbdcd13 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -5,7 +5,7 @@ - + diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index 46c1276b8f6..0aa4da0b87e 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -223,6 +223,7 @@ class BehaviorTreeNavigator : public NavigatorBase blackboard->set>("tf_buffer", feedback_utils.tf); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set>("odom_smoother", odom_smoother); // NOLINT return configure(parent_node, odom_smoother) && ok; } From ea5e292b248a8ca8d876d55669af13cc60a4f34c Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 21 Jan 2023 15:03:13 +0800 Subject: [PATCH 2/2] Fix SpeedController linting and test errors Signed-off-by: Borong Yuan --- nav2_behavior_tree/plugins/decorator/speed_controller.cpp | 3 ++- .../test/plugins/decorator/test_speed_controller.cpp | 8 ++++++++ nav2_core/include/nav2_core/behavior_tree_navigator.hpp | 2 +- .../src/behavior_tree/test_behavior_tree_node.cpp | 7 +++++++ 4 files changed, 18 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 2e739569c39..e5c96eba2c2 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -52,7 +52,8 @@ SpeedController::SpeedController( std::string odom_topic; node_->get_parameter_or("odom_topic", odom_topic, std::string("odom")); - odom_smoother_ = config().blackboard->get>("odom_smoother"); + odom_smoother_ = config().blackboard->get>( + "odom_smoother"); } inline BT::NodeStatus SpeedController::tick() diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 494aa13ca1f..44193b62af8 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/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(node_); + config_->blackboard->set>( + "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 odom_smoother_; static std::shared_ptr bt_node_; static std::shared_ptr dummy_node_; }; +std::shared_ptr +SpeedControllerTestFixture::odom_smoother_ = nullptr; std::shared_ptr SpeedControllerTestFixture::bt_node_ = nullptr; std::shared_ptr diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index 0aa4da0b87e..9a101f20ef5 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -223,7 +223,7 @@ class BehaviorTreeNavigator : public NavigatorBase blackboard->set>("tf_buffer", feedback_utils.tf); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT - blackboard->set>("odom_smoother", odom_smoother); // NOLINT + blackboard->set>("odom_smoother", odom_smoother); // NOLINT return configure(parent_node, odom_smoother) && ok; } diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 6be412753f6..48ba35038cd 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/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(*tf_, node_, false); + odom_smoother_ = std::make_shared(node_); + const std::vector 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>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set>("odom_smoother", odom_smoother_); // NOLINT // set dummy goal on blackboard geometry_msgs::msg::PoseStamped goal; @@ -184,6 +189,8 @@ class BehaviorTreeHandler std::shared_ptr tf_; std::shared_ptr tf_listener_; + std::shared_ptr odom_smoother_; + BT::BehaviorTreeFactory factory_; };