diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index f8cd1df408..7526c82698 100644 --- a/doc/parameters/param_list.md +++ b/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 | diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 816fa62d2f..807c803064 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/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) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 6872db8ac7..2917bed763 100644 --- a/nav2_behavior_tree/README.md +++ b/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.| diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/speed_controller.hpp new file mode 100644 index 0000000000..97ff20a0ae --- /dev/null +++ b/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 +#include +#include + +#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("min_rate", 0.1, "Minimum rate"), + 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") + }; + } + +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 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_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 35675d9938..94e3c2591b 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -72,5 +72,13 @@ Distance + + Minimum rate + 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 new file mode 100644 index 0000000000..58551c4c07 --- /dev/null +++ b/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 +#include + +#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("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(node_, duration, odom_topic); +} + +inline BT::NodeStatus SpeedController::tick() +{ + auto current_goal = config().blackboard->get("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("SpeedController"); +} diff --git a/nav2_behavior_tree/test/CMakeLists.txt b/nav2_behavior_tree/test/CMakeLists.txt index 243305dd1e..db804c6621 100644 --- a/nav2_behavior_tree/test/CMakeLists.txt +++ b/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 ) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp index a1b2509d95..9dd329a2d4 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp +++ b/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(); + config_ = std::make_shared(); // 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 transform_handler_; + static std::shared_ptr 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 +DistanceControllerTestFixture::transform_handler_ = nullptr; +std::shared_ptr DistanceControllerTestFixture::config_ = nullptr; nav2_behavior_tree::DistanceController * DistanceControllerTestFixture::node_ = nullptr; nav2_behavior_tree::DummyNode * DistanceControllerTestFixture::dummy_node_ = nullptr; diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp new file mode 100644 index 0000000000..1baf224ec2 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -0,0 +1,172 @@ +// 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 +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" + +#include "../../test_transform_handler.hpp" +#include "../../test_dummy_tree_node.hpp" +#include "nav2_behavior_tree/plugins/speed_controller.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class SpeedControllerTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + transform_handler_ = std::make_shared(); + config_ = std::make_shared(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + rclcpp::Node::SharedPtr(transform_handler_)); + config_->blackboard->set>( + "tf_buffer", + transform_handler_->getBuffer()); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = transform_handler_->now(); + config_->blackboard->set("goal", goal); + + transform_handler_->activate(); + transform_handler_->waitForTransform(); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete dummy_node_; + delete node_; + } + + void SetUp() + { + node_ = new nav2_behavior_tree::SpeedController("speed_controller", *config_); + dummy_node_ = new nav2_behavior_tree::DummyNode(); + node_->setChild(dummy_node_); + } + + void TearDown() + { + dummy_node_ = nullptr; + node_ = nullptr; + } + +protected: + static std::shared_ptr transform_handler_; + static std::shared_ptr config_; + static nav2_behavior_tree::SpeedController * node_; + static nav2_behavior_tree::DummyNode * dummy_node_; +}; + +std::shared_ptr +SpeedControllerTestFixture::transform_handler_ = nullptr; +std::shared_ptr SpeedControllerTestFixture::config_ = nullptr; +nav2_behavior_tree::SpeedController * SpeedControllerTestFixture::node_ = nullptr; +nav2_behavior_tree::DummyNode * SpeedControllerTestFixture::dummy_node_ = nullptr; + +/* + * Test for speed controller behavior + * Speed controller calculates the period after which it should succeed + * based on the current velocity which is scaled to a pre-defined rate range + * Current velocity is set using odom messages + * The period is reset on the basis of current velocity after the last period is exceeded + */ +TEST_F(SpeedControllerTestFixture, test_behavior) +{ + auto odom_pub = transform_handler_->create_publisher("odom", 1); + nav_msgs::msg::Odometry odom_msg; + + auto time = transform_handler_->now(); + odom_msg.header.stamp = time; + odom_msg.twist.twist.linear.x = 0.223; + odom_pub->publish(odom_msg); + + EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE); + + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // after the first tick, period should be a default value of 1s + // first tick should return running since period has not exceeded + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING); + + // set the child node to success so node can return success + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + + // should return success since period has exceeded and new period should be set to ~2s + rclcpp::sleep_for(1s); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + + // send new velocity for update after the next period + odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.5); + odom_msg.twist.twist.linear.x = 0; + odom_msg.twist.twist.linear.y = 0; + odom_pub->publish(odom_msg); + + // Period should be set to ~2s based on the last speed of 0.223 m/s + rclcpp::sleep_for(1s); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING); + + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + rclcpp::sleep_for(1s); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + + // period should be set to ~10s based on the last speed of 0 m/s + // should return running for the first 9 seconds + for (int i = 0; i < 9; ++i) { + rclcpp::sleep_for(1s); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING); + } + + // set the child node to success so node can return success + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + + // should return success since period has exceeded + rclcpp::sleep_for(1s); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 6ce8e32634..cde2aa75ba 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -51,6 +51,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link + odom_topic: /odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -66,6 +67,7 @@ bt_navigator: - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_transform_available_condition_bt_node diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index efa093a6fb..c5c089ef47 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -51,6 +51,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link + odom_topic: /odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -66,6 +67,7 @@ bt_navigator: - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_transform_available_condition_bt_node diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index cb123400a5..9cc62b4a01 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -51,6 +51,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link + odom_topic: /odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -66,6 +67,7 @@ bt_navigator: - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index 23fe3bf8db..c0aa7ea66c 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -64,6 +64,8 @@ returns FAILURE, all nodes are halted and this node returns FAILURE. * DistanceController: A custom control flow node, which controls the tick rate based on the distance traveled. This custom node has only one child. The user can set the distance after which the planner should replan a new path. This node returns RUNNING when it is not ticking its child. Currently, in navigation, the `DistanceController` is used to tick the `ComputePathToPose` and `GoalReached` node after every 0.5 meters. +* SpeedController: A custom control flow node, which controls the tick rate based on the current speed. This decorator offers the most flexibility as the user can set the minimum/maximum tick rate which is adjusted according to the current speed. + #### Condition Nodes * GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked. diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml new file mode 100644 index 0000000000..8bc4b7f05a --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index d69227017f..f0c063c79a 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -50,6 +50,7 @@ BtNavigator::BtNavigator() "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", + "nav2_speed_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", @@ -64,6 +65,7 @@ BtNavigator::BtNavigator() declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter("global_frame", std::string("map")); declare_parameter("robot_base_frame", std::string("base_link")); + declare_parameter("odom_topic", std::string("odom")); } BtNavigator::~BtNavigator() diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp new file mode 100644 index 0000000000..9a1d49d504 --- /dev/null +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -0,0 +1,75 @@ +// 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_UTIL__ODOMETRY_UTILS_HPP_ +#define NAV2_UTIL__ODOMETRY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_util +{ + +/** + * @class OdomSmoother + * Wrapper for getting smooth odometry readings using a simple moving avergae. + * Subscribes to the topic with a mutex. + */ +class OdomSmoother +{ +public: + /** + * @brief Constructor that subscribes to an Odometry topic + * @param nh NodeHandle for creating subscriber + * @param filter_duration Duration for odom history (seconds) + * @param odom_topic Topic on which odometry should be received + */ + explicit OdomSmoother( + rclcpp::Node::SharedPtr nh, + double filter_duration = 0.3, + std::string odom_topic = "odom"); + + inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;} + inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;} + +protected: + void odomCallback(nav_msgs::msg::Odometry::SharedPtr msg); + void updateState(); + + rclcpp::Node::SharedPtr node_; + + rclcpp::Subscription::SharedPtr odom_sub_; + nav_msgs::msg::Odometry odom_cumulate_; + geometry_msgs::msg::TwistStamped vel_smooth_; + std::mutex odom_mutex_; + + rclcpp::Duration odom_history_duration_; + std::deque odom_history_; +}; + +} // namespace nav2_util + +#endif // NAV2_UTIL__ODOMETRY_UTILS_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index bd903ef6d1..bcaf94b5ba 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(${library_name} SHARED lifecycle_node.cpp robot_utils.cpp node_thread.cpp + odometry_utils.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp new file mode 100644 index 0000000000..f866bd9244 --- /dev/null +++ b/nav2_util/src/odometry_utils.cpp @@ -0,0 +1,101 @@ +// 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 + +#include "nav2_util/odometry_utils.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +namespace nav2_util +{ + +OdomSmoother::OdomSmoother( + rclcpp::Node::SharedPtr nh, + double filter_duration, + std::string odom_topic) +: node_(nh), + odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) +{ + odom_sub_ = nh->create_subscription( + odom_topic, + rclcpp::SystemDefaultsQoS(), + std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1)); + + odom_cumulate_.twist.twist.linear.x = 0; + odom_cumulate_.twist.twist.linear.y = 0; + odom_cumulate_.twist.twist.linear.z = 0; + odom_cumulate_.twist.twist.angular.x = 0; + odom_cumulate_.twist.twist.angular.y = 0; + odom_cumulate_.twist.twist.angular.z = 0; +} + +void OdomSmoother::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + std::lock_guard lock(odom_mutex_); + + // update cumulated odom only if history is not empty + if (!odom_history_.empty()) { + // to store current time + auto current_time = rclcpp::Time(msg->header.stamp); + + // to store time of the first odom in history + auto front_time = rclcpp::Time(odom_history_.front().header.stamp); + + // update cumulated odom when duration has exceeded and pop earliest msg + while (current_time - front_time > odom_history_duration_) { + const auto & odom = odom_history_.front(); + odom_cumulate_.twist.twist.linear.x -= odom.twist.twist.linear.x; + odom_cumulate_.twist.twist.linear.y -= odom.twist.twist.linear.y; + odom_cumulate_.twist.twist.linear.z -= odom.twist.twist.linear.z; + odom_cumulate_.twist.twist.angular.x -= odom.twist.twist.angular.x; + odom_cumulate_.twist.twist.angular.y -= odom.twist.twist.angular.y; + odom_cumulate_.twist.twist.angular.z -= odom.twist.twist.angular.z; + odom_history_.pop_front(); + + if (odom_history_.empty()) { + break; + } + + // update with the timestamp of earliest odom message in history + front_time = rclcpp::Time(odom_history_.front().header.stamp); + } + } + + odom_history_.push_back(*msg); + updateState(); +} + +void OdomSmoother::updateState() +{ + const auto & odom = odom_history_.back(); + odom_cumulate_.twist.twist.linear.x += odom.twist.twist.linear.x; + odom_cumulate_.twist.twist.linear.y += odom.twist.twist.linear.y; + odom_cumulate_.twist.twist.linear.z += odom.twist.twist.linear.z; + odom_cumulate_.twist.twist.angular.x += odom.twist.twist.angular.x; + odom_cumulate_.twist.twist.angular.y += odom.twist.twist.angular.y; + odom_cumulate_.twist.twist.angular.z += odom.twist.twist.angular.z; + + vel_smooth_.header = odom.header; + vel_smooth_.twist.linear.x = odom_cumulate_.twist.twist.linear.x / odom_history_.size(); + vel_smooth_.twist.linear.y = odom_cumulate_.twist.twist.linear.y / odom_history_.size(); + vel_smooth_.twist.linear.z = odom_cumulate_.twist.twist.linear.z / odom_history_.size(); + vel_smooth_.twist.angular.x = odom_cumulate_.twist.twist.angular.x / odom_history_.size(); + vel_smooth_.twist.angular.y = odom_cumulate_.twist.twist.angular.y / odom_history_.size(); + vel_smooth_.twist.angular.z = odom_cumulate_.twist.twist.angular.z / odom_history_.size(); +} + +} // namespace nav2_util diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 8840edf675..9755da83ae 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -33,6 +33,10 @@ ament_add_gtest(test_geometry_utils test_geometry_utils.cpp) ament_target_dependencies(test_geometry_utils geometry_msgs) target_link_libraries(test_geometry_utils ${library_name}) +ament_add_gtest(test_odometry_utils test_odometry_utils.cpp) +ament_target_dependencies(test_odometry_utils nav_msgs geometry_msgs) +target_link_libraries(test_odometry_utils ${library_name}) + ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp new file mode 100644 index 0000000000..7d5b8bf9fe --- /dev/null +++ b/nav2_util/test/test_odometry_utils.cpp @@ -0,0 +1,115 @@ +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "gtest/gtest.h" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(OdometryUtils, test_smoothed_velocity) +{ + auto node = std::make_shared("test_node"); + auto odom_pub = node->create_publisher("odom", 1); + + nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom"); + + nav_msgs::msg::Odometry odom_msg; + geometry_msgs::msg::Twist twist_msg; + + auto time = node->now(); + + odom_msg.header.stamp = time; + odom_msg.twist.twist.linear.x = 1.0; + odom_msg.twist.twist.linear.y = 1.0; + odom_msg.twist.twist.angular.z = 1.0; + + odom_pub->publish(odom_msg); + rclcpp::spin_some(node); + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 1.0); + EXPECT_EQ(twist_msg.linear.y, 1.0); + EXPECT_EQ(twist_msg.angular.z, 1.0); + + odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.1); + odom_msg.twist.twist.linear.x = 2.0; + odom_msg.twist.twist.linear.y = 2.0; + odom_msg.twist.twist.angular.z = 2.0; + odom_pub->publish(odom_msg); + + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node); + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 1.5); + EXPECT_EQ(twist_msg.linear.y, 1.5); + EXPECT_EQ(twist_msg.angular.z, 1.5); + + odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.2); + odom_msg.twist.twist.linear.x = 3.0; + odom_msg.twist.twist.linear.y = 3.0; + odom_msg.twist.twist.angular.z = 3.0; + odom_pub->publish(odom_msg); + + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node); + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 2.0); + EXPECT_EQ(twist_msg.linear.y, 2.0); + EXPECT_EQ(twist_msg.angular.z, 2.0); + + odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.45); + odom_msg.twist.twist.linear.x = 4.0; + odom_msg.twist.twist.linear.y = 4.0; + odom_msg.twist.twist.angular.z = 4.0; + odom_pub->publish(odom_msg); + + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node); + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 3.5); + EXPECT_EQ(twist_msg.linear.y, 3.5); + EXPECT_EQ(twist_msg.angular.z, 3.5); + + odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(1.0); + odom_msg.twist.twist.linear.x = 5.0; + odom_msg.twist.twist.linear.y = 5.0; + odom_msg.twist.twist.angular.z = 5.0; + odom_pub->publish(odom_msg); + + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node); + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 5.0); + EXPECT_EQ(twist_msg.linear.y, 5.0); + EXPECT_EQ(twist_msg.angular.z, 5.0); +} diff --git a/tools/bt2img.py b/tools/bt2img.py index 5c2c06057e..ebc2629e45 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -38,6 +38,7 @@ "Timeout", "RateController", "DistanceController", + "SpeedController", "RecoveryNode", "PipelineSequence", "RoundRobin"