From fb351fc4c18e6821a50a8435aa14c52ba4500235 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 3 Oct 2020 18:04:30 +0530 Subject: [PATCH 01/13] Add nav2_behavior_tree::BtActionServer Signed-off-by: Sarthak Mittal --- .../nav2_behavior_tree/bt_action_server.hpp | 390 ++++++++++++++++++ .../nav2_bt_navigator/bt_navigator.hpp | 96 +---- nav2_bt_navigator/src/bt_navigator.cpp | 315 +++----------- 3 files changed, 463 insertions(+), 338 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp new file mode 100644 index 00000000000..926ff2bb93c --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -0,0 +1,390 @@ +// 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__BT_ACTION_SERVER_HPP_ +#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_ + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_behavior_tree/behavior_tree_engine.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/simple_action_server.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/create_timer_ros.h" + +namespace nav2_behavior_tree +{ +/** + * @class nav2_behavior_tree::BtActionServer + * @brief An action server that uses behavior tree to execute an action + */ +template +class BtActionServer : public nav2_util::LifecycleNode +{ +public: + using ActionServer = nav2_util::SimpleActionServer; + + /** + * @brief A constructor for nav2_behavior_tree::BtActionServer class + */ + explicit BtActionServer( + const std::string & action_name, + const std::string & node_name, + const std::string & namespace_ = "", + bool use_rclcpp_node = false, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : nav2_util::LifecycleNode(node_name, namespace_, use_rclcpp_node, options), + action_name_(action_name) + { + RCLCPP_INFO(get_logger(), "Creating"); + + const std::vector plugin_libs = { + "nav2_compute_path_to_pose_action_bt_node", + "nav2_follow_path_action_bt_node", + "nav2_back_up_action_bt_node", + "nav2_spin_action_bt_node", + "nav2_wait_action_bt_node", + "nav2_clear_costmap_service_bt_node", + "nav2_is_stuck_condition_bt_node", + "nav2_goal_reached_condition_bt_node", + "nav2_initial_pose_received_condition_bt_node", + "nav2_goal_updated_condition_bt_node", + "nav2_reinitialize_global_localization_service_bt_node", + "nav2_rate_controller_bt_node", + "nav2_distance_controller_bt_node", + "nav2_speed_controller_bt_node", + "nav2_truncate_path_action_bt_node", + "nav2_goal_updater_node_bt_node", + "nav2_recovery_node_bt_node", + "nav2_pipeline_sequence_bt_node", + "nav2_round_robin_node_bt_node", + "nav2_transform_available_condition_bt_node", + "nav2_time_expired_condition_bt_node", + "nav2_distance_traveled_condition_bt_node" + }; + + // Declare this node's parameters + declare_parameter("default_bt_xml_filename"); + declare_parameter("plugin_lib_names", plugin_libs); + declare_parameter("enable_groot_monitoring", true); + declare_parameter("groot_zmq_publisher_port", 1666); + declare_parameter("groot_zmq_server_port", 1667); + } + + /** + * @brief A destructor for nav2_behavior_tree::BtActionServer class + */ + ~BtActionServer() {} + + // Derived classes can override any of the following methods to hook into the + // processing for different lifecycle states + + virtual nav2_util::CallbackReturn on_configure() + { + return nav2_util::CallbackReturn::SUCCESS; + } + + virtual nav2_util::CallbackReturn on_activate() + { + return nav2_util::CallbackReturn::SUCCESS; + } + + virtual nav2_util::CallbackReturn on_deactivate() + { + return nav2_util::CallbackReturn::SUCCESS; + } + + virtual nav2_util::CallbackReturn on_cleanup() + { + return nav2_util::CallbackReturn::SUCCESS; + } + + virtual nav2_util::CallbackReturn on_shutdown() + { + return nav2_util::CallbackReturn::SUCCESS; + } + + // Derived classes can override any of the following methods to hook into the + // processing for the action: on_goal_received, on_loop, and is_cancelling + + // Could check if goal is valid and put values + // on the blackboard which depend on the received goal + virtual bool on_goal_received() = 0; + + // Could define execution that happens on one iteration through the BT + // Preempt current goal when new goal is received + // Publish feedback + virtual void on_loop() = 0; + + // Decides if cancel has been requested for the current goal + virtual bool is_canceling() = 0; + +protected: + /** + * @brief Configures member variables + * + * Initializes action server for, builds behavior tree from xml file, + * and calls user-defined onConfigure. + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Configuring"); + + // use suffix '_rclcpp_node' to keep parameter file consistency #1773 + auto options = rclcpp::NodeOptions().arguments( + {"--ros-args", + "-r", std::string("__node:=") + get_name() + "_rclcpp_node", + "--"}); + // Support for handling the topic-based goal pose from rviz + client_node_ = std::make_shared("_", options); + + tf_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), get_node_timers_interface()); + tf_->setCreateTimerInterface(timer_interface); + tf_->setUsingDedicatedThread(true); + tf_listener_ = std::make_shared(*tf_, this, false); + + action_server_ = std::make_unique( + get_node_base_interface(), + get_node_clock_interface(), + get_node_logging_interface(), + get_node_waitables_interface(), + action_name_, std::bind(&BtActionServer::executeCallback, this)); + + // Get the libraries to pull plugins from + plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array(); + + // Create the class that registers our custom nodes and executes the BT + bt_ = std::make_unique(plugin_lib_names_); + + // Create the blackboard that will be shared by all of the nodes in the tree + blackboard_ = BT::Blackboard::create(); + + // Put items on the blackboard + blackboard_->set("node", client_node_); // NOLINT + blackboard_->set>("tf_buffer", tf_); // NOLINT + blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT + blackboard_->set("initial_pose_received", false); // NOLINT + blackboard_->set("number_recoveries", 0); // NOLINT + + // Get the BT filename to use from the node parameter + get_parameter("default_bt_xml_filename", default_bt_xml_filename_); + + return on_configure(); + } + + /** + * @brief Activates action server + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Activating"); + + if (!loadBehaviorTree(default_bt_xml_filename_)) { + RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str()); + return nav2_util::CallbackReturn::FAILURE; + } + + action_server_->activate(); + + // create bond connection + createBond(); + + return on_activate(); + } + + /** + * @brief Deactivates action server + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Deactivating"); + + action_server_->deactivate(); + + // destroy bond connection + destroyBond(); + + return on_deactivate(); + } + + /** + * @brief Resets member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Cleaning up"); + + client_node_.reset(); + + // Reset the listener before the buffer + tf_listener_.reset(); + tf_.reset(); + + action_server_.reset(); + plugin_lib_names_.clear(); + current_bt_xml_filename_.clear(); + blackboard_.reset(); + bt_->haltAllActions(tree_.rootNode()); + bt_->resetGrootMonitor(); + bt_.reset(); + + RCLCPP_INFO(get_logger(), "Completed Cleaning up"); + return on_cleanup(); + } + + /** + * @brief Called when in shutdown state + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Shutting down"); + return on_shutdown(); + } + + /** + * @brief Action server callback + */ + void executeCallback() + { + if (!on_goal_received()) { + action_server_->terminate_current(); + return; + } + + // Execute the BT that was previously created in the configure step + nav2_behavior_tree::BtStatus rc = bt_->run( + &tree_, + std::bind(&BtActionServer::on_loop, this), + std::bind(&BtActionServer::is_canceling, this)); + + // Make sure that the Bt is not in a running state from a previous execution + // note: if all the ControlNodes are implemented correctly, this is not needed. + bt_->haltAllActions(tree_.rootNode()); + + switch (rc) { + case nav2_behavior_tree::BtStatus::SUCCEEDED: + RCLCPP_INFO(get_logger(), "Goal succeeded"); + action_server_->succeeded_current(); + break; + + case nav2_behavior_tree::BtStatus::FAILED: + RCLCPP_ERROR(get_logger(), "Goal failed"); + action_server_->terminate_current(); + break; + + case nav2_behavior_tree::BtStatus::CANCELED: + RCLCPP_INFO(get_logger(), "Goal canceled"); + action_server_->terminate_all(); + break; + } + } + + /** + * @brief Replace current BT with another one + * @param bt_xml_filename The file containing the new BT + * @return true if the resulting BT correspond to the one in bt_xml_filename. false + * if something went wrong, and previous BT is maintained + */ + bool loadBehaviorTree(const std::string & bt_xml_filename) + { + // Use previous BT if it is the existing one + if (current_bt_xml_filename_ == bt_xml_filename) { + RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded"); + return true; + } + + // if a new tree is created, than the ZMQ Publisher must be destroyed + bt_->resetGrootMonitor(); + + // Read the input BT XML from the specified file into a string + std::ifstream xml_file(bt_xml_filename); + + if (!xml_file.good()) { + RCLCPP_ERROR(get_logger(), "Couldn't open input XML file: %s", bt_xml_filename.c_str()); + return false; + } + + auto xml_string = std::string( + std::istreambuf_iterator(xml_file), + std::istreambuf_iterator()); + + // Create the Behavior Tree from the XML input + tree_ = bt_->createTreeFromText(xml_string, blackboard_); + current_bt_xml_filename_ = bt_xml_filename; + + // get parameter for monitoring with Groot via ZMQ Publisher + if (get_parameter("enable_groot_monitoring").as_bool()) { + uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int(); + uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int(); + // optionally add max_msg_per_second = 25 (default) here + try { + bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port); + } catch (const std::logic_error & e) { + RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what()); + } + } + return true; + } + + // Action name + std::string action_name_; + + // Our action server implements the template action + std::unique_ptr action_server_; + + // Behavior Tree to be executed when goal is received + BT::Tree tree_; + + // The blackboard shared by all of the nodes in the tree + BT::Blackboard::Ptr blackboard_; + + // The XML fiñe that cointains the Behavior Tree to create + std::string current_bt_xml_filename_; + std::string default_bt_xml_filename_; + + // The wrapper class for the BT functionality + std::unique_ptr bt_; + + // Libraries to pull plugins (BT Nodes) from + std::vector plugin_lib_names_; + + // A regular, non-spinning ROS node that we can use for calls to the action client + rclcpp::Node::SharedPtr client_node_; + + // Spinning transform that can be used by the BT nodes + std::shared_ptr tf_; + std::shared_ptr tf_listener_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_ diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 9023f829af8..8e24c439c35 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -20,15 +20,11 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_behavior_tree/behavior_tree_engine.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav_msgs/msg/path.hpp" -#include "nav2_util/simple_action_server.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/buffer.h" +#include "nav2_bt_navigator/ros_topic_logger.hpp" +#include "nav2_behavior_tree/bt_action_server.hpp" namespace nav2_bt_navigator { @@ -37,7 +33,7 @@ namespace nav2_bt_navigator * @brief An action server that uses behavior tree for navigating a robot to its * goal position. */ -class BtNavigator : public nav2_util::LifecycleNode +class BtNavigator : public nav2_behavior_tree::BtActionServer { public: /** @@ -50,102 +46,38 @@ class BtNavigator : public nav2_util::LifecycleNode ~BtNavigator(); protected: - /** - * @brief Configures member variables - * - * Initializes action server for "NavigationToPose"; subscription to - * "goal_sub"; and builds behavior tree from xml file. - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; - /** - * @brief Activates action server - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; - /** - * @brief Deactivates action server - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; - /** - * @brief Resets member variables - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in shutdown state - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_configure() override; - using Action = nav2_msgs::action::NavigateToPose; + nav2_util::CallbackReturn on_cleanup() override; - using ActionServer = nav2_util::SimpleActionServer; + bool on_goal_received() override; - // Our action server implements the NavigateToPose action - std::unique_ptr action_server_; + void on_loop() override; - /** - * @brief Action server callbacks - */ - void navigateToPose(); - - /** - * @brief Goal pose initialization on the blackboard - */ - void initializeGoalPose(); + bool is_canceling() override; /** * @brief A subscription and callback to handle the topic-based goal published * from rviz */ void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); - rclcpp::Subscription::SharedPtr goal_sub_; - - /** - * @brief Replace current BT with another one - * @param bt_xml_filename The file containing the new BT - * @return true if the resulting BT correspond to the one in bt_xml_filename. false - * if something went wrong, and previous BT is mantained - */ - bool loadBehaviorTree(const std::string & bt_id); - - BT::Tree tree_; - - // The blackboard shared by all of the nodes in the tree - BT::Blackboard::Ptr blackboard_; - // The XML fiñe that cointains the Behavior Tree to create - std::string current_bt_xml_filename_; - std::string default_bt_xml_filename_; - - // The wrapper class for the BT functionality - std::unique_ptr bt_; + using Action = nav2_msgs::action::NavigateToPose; - // Libraries to pull plugins (BT Nodes) from - std::vector plugin_lib_names_; + rclcpp::Subscription::SharedPtr goal_sub_; // A client that we'll use to send a command message to our own task server rclcpp_action::Client::SharedPtr self_client_; - // A regular, non-spinning ROS node that we can use for calls to the action client - rclcpp::Node::SharedPtr client_node_; - - // Spinning transform that can be used by the BT nodes - std::shared_ptr tf_; - std::shared_ptr tf_listener_; - // Metrics for feedback rclcpp::Time start_time_; std::string robot_frame_; std::string global_frame_; double transform_tolerance_; + + std::unique_ptr topic_logger_; + + std::shared_ptr feedback_msg_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9d224b0d5a5..c8882111846 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -14,63 +14,27 @@ #include "nav2_bt_navigator/bt_navigator.hpp" -#include #include #include #include -#include #include -#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_behavior_tree/bt_conversions.hpp" -#include "nav2_bt_navigator/ros_topic_logger.hpp" namespace nav2_bt_navigator { BtNavigator::BtNavigator() -: nav2_util::LifecycleNode("bt_navigator", "", false), +: nav2_behavior_tree::BtActionServer( + "navigate_to_pose", "bt_navigator", "", false), start_time_(0) { - RCLCPP_INFO(get_logger(), "Creating"); - - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_distance_traveled_condition_bt_node" - }; - - // Declare this node's parameters - declare_parameter("default_bt_xml_filename"); - declare_parameter("plugin_lib_names", plugin_libs); 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")); - declare_parameter("enable_groot_monitoring", true); - declare_parameter("groot_zmq_publisher_port", 1666); - declare_parameter("groot_zmq_server_port", 1667); } BtNavigator::~BtNavigator() @@ -78,192 +42,51 @@ BtNavigator::~BtNavigator() } nav2_util::CallbackReturn -BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_configure() { - RCLCPP_INFO(get_logger(), "Configuring"); - - // use suffix '_rclcpp_node' to keep parameter file consistency #1773 - auto options = rclcpp::NodeOptions().arguments( - {"--ros-args", - "-r", std::string("__node:=") + get_name() + "_rclcpp_node", - "--"}); - // Support for handling the topic-based goal pose from rviz - client_node_ = std::make_shared("_", options); - self_client_ = rclcpp_action::create_client( client_node_, "navigate_to_pose"); - tf_ = std::make_shared(get_clock()); - auto timer_interface = std::make_shared( - get_node_base_interface(), get_node_timers_interface()); - tf_->setCreateTimerInterface(timer_interface); - tf_->setUsingDedicatedThread(true); - tf_listener_ = std::make_shared(*tf_, this, false); - goal_sub_ = create_subscription( "goal_pose", rclcpp::SystemDefaultsQoS(), std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1)); - action_server_ = std::make_unique( - get_node_base_interface(), - get_node_clock_interface(), - get_node_logging_interface(), - get_node_waitables_interface(), - "navigate_to_pose", std::bind(&BtNavigator::navigateToPose, this)); - - // Get the libraries to pull plugins from - plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array(); global_frame_ = get_parameter("global_frame").as_string(); robot_frame_ = get_parameter("robot_base_frame").as_string(); transform_tolerance_ = get_parameter("transform_tolerance").as_double(); - // Create the class that registers our custom nodes and executes the BT - bt_ = std::make_unique(plugin_lib_names_); - - // Create the blackboard that will be shared by all of the nodes in the tree - blackboard_ = BT::Blackboard::create(); - - // Put items on the blackboard - blackboard_->set("node", client_node_); // NOLINT - blackboard_->set>("tf_buffer", tf_); // NOLINT - blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT blackboard_->set("initial_pose_received", false); // NOLINT blackboard_->set("number_recoveries", 0); // NOLINT - // Get the BT filename to use from the node parameter - get_parameter("default_bt_xml_filename", default_bt_xml_filename_); - return nav2_util::CallbackReturn::SUCCESS; } -bool -BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) -{ - // Use previous BT if it is the existing one - if (current_bt_xml_filename_ == bt_xml_filename) { - RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded"); - return true; - } - - // if a new tree is created, than the ZMQ Publisher must be destroyed - bt_->resetGrootMonitor(); - - // Read the input BT XML from the specified file into a string - std::ifstream xml_file(bt_xml_filename); - - if (!xml_file.good()) { - RCLCPP_ERROR(get_logger(), "Couldn't open input XML file: %s", bt_xml_filename.c_str()); - return false; - } - - auto xml_string = std::string( - std::istreambuf_iterator(xml_file), - std::istreambuf_iterator()); - - // Create the Behavior Tree from the XML input - tree_ = bt_->createTreeFromText(xml_string, blackboard_); - current_bt_xml_filename_ = bt_xml_filename; - - // get parameter for monitoring with Groot via ZMQ Publisher - if (get_parameter("enable_groot_monitoring").as_bool()) { - uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int(); - uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int(); - // optionally add max_msg_per_second = 25 (default) here - try { - bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port); - } catch (const std::logic_error & e) { - RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what()); - } - } - return true; -} - nav2_util::CallbackReturn -BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_cleanup() { - RCLCPP_INFO(get_logger(), "Activating"); - - if (!loadBehaviorTree(default_bt_xml_filename_)) { - RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str()); - return nav2_util::CallbackReturn::FAILURE; - } - - action_server_->activate(); - - // create bond connection - createBond(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Deactivating"); - - action_server_->deactivate(); - - // destroy bond connection - destroyBond(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Cleaning up"); - - // TODO(orduno) Fix the race condition between the worker thread ticking the tree - // and the main thread resetting the resources, see #1344 goal_sub_.reset(); - client_node_.reset(); self_client_.reset(); - - // Reset the listener before the buffer - tf_listener_.reset(); - tf_.reset(); - - action_server_.reset(); - plugin_lib_names_.clear(); - current_bt_xml_filename_.clear(); - blackboard_.reset(); - bt_->haltAllActions(tree_.rootNode()); - bt_->resetGrootMonitor(); - bt_.reset(); - - RCLCPP_INFO(get_logger(), "Completed Cleaning up"); return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(get_logger(), "Shutting down"); - return nav2_util::CallbackReturn::SUCCESS; -} - -void -BtNavigator::navigateToPose() +bool +BtNavigator::on_goal_received() { - initializeGoalPose(); + auto goal = action_server_->get_current_goal(); - auto is_canceling = [this]() { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Canceling."); - return true; - } + RCLCPP_INFO( + get_logger(), "Begin navigating from current location to (%.2f, %.2f)", + goal->pose.pose.position.x, goal->pose.pose.position.y); - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Canceling."); - return true; - } + // Reset state for new action feedback + start_time_ = now(); + blackboard_->set("number_recoveries", 0); // NOLINT - return action_server_->is_cancel_requested(); - }; + // Update the goal pose on the blackboard + blackboard_->set("goal", goal->pose); - std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree; + auto bt_xml_filename = goal->behavior_tree; // Empty id in request is default for backward compatibility bt_xml_filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; @@ -272,78 +95,58 @@ BtNavigator::navigateToPose() RCLCPP_ERROR( get_logger(), "BT file not found: %s. Navigation canceled.", bt_xml_filename.c_str()); - action_server_->terminate_current(); - return; + return false; } - RosTopicLogger topic_logger(client_node_, tree_); - std::shared_ptr feedback_msg = std::make_shared(); - - auto on_loop = [&]() { - if (action_server_->is_preempt_requested()) { - RCLCPP_INFO(get_logger(), "Received goal preemption request"); - action_server_->accept_pending_goal(); - initializeGoalPose(); - } - topic_logger.flush(); - - // action server feedback (pose, duration of task, - // number of recoveries, and distance remaining to goal) - nav2_util::getCurrentPose( - feedback_msg->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_); - - geometry_msgs::msg::PoseStamped goal_pose; - blackboard_->get("goal", goal_pose); - - feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance( - feedback_msg->current_pose.pose, goal_pose.pose); - - int recovery_count = 0; - blackboard_->get("number_recoveries", recovery_count); - feedback_msg->number_of_recoveries = recovery_count; - feedback_msg->navigation_time = now() - start_time_; - action_server_->publish_feedback(feedback_msg); - }; - - // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); - // Make sure that the Bt is not in a running state from a previous execution - // note: if all the ControlNodes are implemented correctly, this is not needed. - bt_->haltAllActions(tree_.rootNode()); - - switch (rc) { - case nav2_behavior_tree::BtStatus::SUCCEEDED: - RCLCPP_INFO(get_logger(), "Navigation succeeded"); - action_server_->succeeded_current(); - break; - - case nav2_behavior_tree::BtStatus::FAILED: - RCLCPP_ERROR(get_logger(), "Navigation failed"); - action_server_->terminate_current(); - break; - - case nav2_behavior_tree::BtStatus::CANCELED: - RCLCPP_INFO(get_logger(), "Navigation canceled"); - action_server_->terminate_all(); - break; - } + topic_logger_ = std::make_unique(client_node_, tree_); + feedback_msg_ = std::make_shared(); + + return true; } void -BtNavigator::initializeGoalPose() +BtNavigator::on_loop() { - auto goal = action_server_->get_current_goal(); + if (action_server_->is_preempt_requested()) { + RCLCPP_INFO(get_logger(), "Received goal preemption request"); + action_server_->accept_pending_goal(); + on_goal_received(); + } - RCLCPP_INFO( - get_logger(), "Begin navigating from current location to (%.2f, %.2f)", - goal->pose.pose.position.x, goal->pose.pose.position.y); + topic_logger_->flush(); - // Reset state for new action feedback - start_time_ = now(); - blackboard_->set("number_recoveries", 0); // NOLINT + // action server feedback (pose, duration of task, + // number of recoveries, and distance remaining to goal) + nav2_util::getCurrentPose( + feedback_msg_->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_); - // Update the goal pose on the blackboard - blackboard_->set("goal", goal->pose); + geometry_msgs::msg::PoseStamped goal_pose; + blackboard_->get("goal", goal_pose); + + feedback_msg_->distance_remaining = nav2_util::geometry_utils::euclidean_distance( + feedback_msg_->current_pose.pose, goal_pose.pose); + + int recovery_count = 0; + blackboard_->get("number_recoveries", recovery_count); + feedback_msg_->number_of_recoveries = recovery_count; + feedback_msg_->navigation_time = now() - start_time_; + action_server_->publish_feedback(feedback_msg_); +} + +bool +BtNavigator::is_canceling() +{ + if (action_server_ == nullptr) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable. Canceling."); + return true; + } + + if (!action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server is inactive. Canceling."); + return true; + } + + return action_server_->is_cancel_requested(); } void From 239112964593d91fb40eae157cace5934b4aad02 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 3 Oct 2020 19:52:27 +0530 Subject: [PATCH 02/13] Fix cpplint errors Signed-off-by: Sarthak Mittal --- .../include/nav2_behavior_tree/bt_action_server.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 926ff2bb93c..970de0c8ffe 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "geometry_msgs/msg/pose_stamped.hpp" From bfcd68158ba5ce840ed62a8cbafb8a04fa8e2539 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 8 Oct 2020 21:02:13 +0530 Subject: [PATCH 03/13] Remove unnecessary statements in BtActionServer Signed-off-by: Sarthak Mittal --- .../include/nav2_behavior_tree/bt_action_server.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 970de0c8ffe..200e5b09e9d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -27,6 +27,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/create_timer_ros.h" @@ -185,8 +186,6 @@ class BtActionServer : public nav2_util::LifecycleNode blackboard_->set("node", client_node_); // NOLINT blackboard_->set>("tf_buffer", tf_); // NOLINT blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT - blackboard_->set("initial_pose_received", false); // NOLINT - blackboard_->set("number_recoveries", 0); // NOLINT // Get the BT filename to use from the node parameter get_parameter("default_bt_xml_filename", default_bt_xml_filename_); From 33312ec2bdab9e3913a126b4f9e122dc5eb485c3 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Wed, 16 Dec 2020 07:35:04 +0530 Subject: [PATCH 04/13] Make nav2_behavior_tree::BtActionServer a composable object --- .../nav2_behavior_tree/bt_action_server.hpp | 347 +++++++++--------- .../nav2_behavior_tree/ros_topic_logger.hpp | 89 +++++ nav2_bt_navigator/CMakeLists.txt | 1 - .../nav2_bt_navigator/bt_navigator.hpp | 51 ++- .../nav2_bt_navigator/ros_topic_logger.hpp | 49 --- nav2_bt_navigator/src/bt_navigator.cpp | 141 ++++--- nav2_bt_navigator/src/ros_topic_logger.cpp | 70 ---- 7 files changed, 406 insertions(+), 342 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp delete mode 100644 nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp delete mode 100644 nav2_bt_navigator/src/ros_topic_logger.cpp diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 200e5b09e9d..9ea2bf22804 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -24,6 +24,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_behavior_tree/behavior_tree_engine.hpp" +#include "nav2_behavior_tree/ros_topic_logger.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -38,24 +39,32 @@ namespace nav2_behavior_tree * @brief An action server that uses behavior tree to execute an action */ template -class BtActionServer : public nav2_util::LifecycleNode +class BtActionServer { public: using ActionServer = nav2_util::SimpleActionServer; + typedef std::function OnGoalReceivedCallback; + typedef std::function OnLoopCallback; + /** * @brief A constructor for nav2_behavior_tree::BtActionServer class */ explicit BtActionServer( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & action_name, - const std::string & node_name, - const std::string & namespace_ = "", - bool use_rclcpp_node = false, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : nav2_util::LifecycleNode(node_name, namespace_, use_rclcpp_node, options), - action_name_(action_name) + OnGoalReceivedCallback on_goal_received_callback, + OnLoopCallback on_loop_callback) + : action_name_(action_name), + node_(parent), + on_goal_received_callback_(on_goal_received_callback), + on_loop_callback_(on_loop_callback) { - RCLCPP_INFO(get_logger(), "Creating"); + auto node = node_.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + RCLCPP_INFO(logger_, "Creating"); const std::vector plugin_libs = { "nav2_compute_path_to_pose_action_bt_node", @@ -83,11 +92,11 @@ class BtActionServer : public nav2_util::LifecycleNode }; // Declare this node's parameters - declare_parameter("default_bt_xml_filename"); - declare_parameter("plugin_lib_names", plugin_libs); - declare_parameter("enable_groot_monitoring", true); - declare_parameter("groot_zmq_publisher_port", 1666); - declare_parameter("groot_zmq_server_port", 1667); + node->declare_parameter("default_bt_xml_filename"); + node->declare_parameter("plugin_lib_names", plugin_libs); + node->declare_parameter("enable_groot_monitoring", true); + node->declare_parameter("groot_zmq_publisher_port", 1666); + node->declare_parameter("groot_zmq_server_port", 1667); } /** @@ -95,86 +104,42 @@ class BtActionServer : public nav2_util::LifecycleNode */ ~BtActionServer() {} - // Derived classes can override any of the following methods to hook into the - // processing for different lifecycle states - - virtual nav2_util::CallbackReturn on_configure() - { - return nav2_util::CallbackReturn::SUCCESS; - } - - virtual nav2_util::CallbackReturn on_activate() - { - return nav2_util::CallbackReturn::SUCCESS; - } - - virtual nav2_util::CallbackReturn on_deactivate() - { - return nav2_util::CallbackReturn::SUCCESS; - } - - virtual nav2_util::CallbackReturn on_cleanup() - { - return nav2_util::CallbackReturn::SUCCESS; - } - - virtual nav2_util::CallbackReturn on_shutdown() - { - return nav2_util::CallbackReturn::SUCCESS; - } - - // Derived classes can override any of the following methods to hook into the - // processing for the action: on_goal_received, on_loop, and is_cancelling - - // Could check if goal is valid and put values - // on the blackboard which depend on the received goal - virtual bool on_goal_received() = 0; - - // Could define execution that happens on one iteration through the BT - // Preempt current goal when new goal is received - // Publish feedback - virtual void on_loop() = 0; - - // Decides if cancel has been requested for the current goal - virtual bool is_canceling() = 0; - -protected: /** * @brief Configures member variables * * Initializes action server for, builds behavior tree from xml file, * and calls user-defined onConfigure. - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE + * @return true on SUCCESS and false on FAILURE */ - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override + bool on_configure(const std::shared_ptr & tf) { - RCLCPP_INFO(get_logger(), "Configuring"); + RCLCPP_INFO(logger_, "Configuring"); + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } // use suffix '_rclcpp_node' to keep parameter file consistency #1773 auto options = rclcpp::NodeOptions().arguments( {"--ros-args", - "-r", std::string("__node:=") + get_name() + "_rclcpp_node", + "-r", std::string("__node:=") + node->get_name() + "_rclcpp_node", "--"}); // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); - tf_ = std::make_shared(get_clock()); - auto timer_interface = std::make_shared( - get_node_base_interface(), get_node_timers_interface()); - tf_->setCreateTimerInterface(timer_interface); - tf_->setUsingDedicatedThread(true); - tf_listener_ = std::make_shared(*tf_, this, false); - - action_server_ = std::make_unique( - get_node_base_interface(), - get_node_clock_interface(), - get_node_logging_interface(), - get_node_waitables_interface(), + // TF buffer + tf_ = tf; + + action_server_ = std::make_shared( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), action_name_, std::bind(&BtActionServer::executeCallback, this)); // Get the libraries to pull plugins from - plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array(); + plugin_lib_names_ = node->get_parameter("plugin_lib_names").as_string_array(); // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -188,66 +153,54 @@ class BtActionServer : public nav2_util::LifecycleNode blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT // Get the BT filename to use from the node parameter - get_parameter("default_bt_xml_filename", default_bt_xml_filename_); + node->get_parameter("default_bt_xml_filename", default_bt_xml_filename_); - return on_configure(); + // Get parameter for monitoring with Groot via ZMQ Publisher + node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_); + node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_); + node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_); + + return true; } /** * @brief Activates action server - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE + * @return true on SUCCESS and false on FAILURE */ - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override + bool on_activate() { - RCLCPP_INFO(get_logger(), "Activating"); - + RCLCPP_INFO(logger_, "Activating"); if (!loadBehaviorTree(default_bt_xml_filename_)) { - RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str()); - return nav2_util::CallbackReturn::FAILURE; + RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str()); + return false; } - action_server_->activate(); - - // create bond connection - createBond(); - - return on_activate(); + return true; } /** * @brief Deactivates action server - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE + * @return true on SUCCESS and false on FAILURE */ - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override + bool on_deactivate() { - RCLCPP_INFO(get_logger(), "Deactivating"); - + RCLCPP_INFO(logger_, "Deactivating"); action_server_->deactivate(); - - // destroy bond connection - destroyBond(); - - return on_deactivate(); + return true; } /** * @brief Resets member variables - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE + * @return true on SUCCESS and false on FAILURE */ - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override + bool on_cleanup() { - RCLCPP_INFO(get_logger(), "Cleaning up"); + RCLCPP_INFO(logger_, "Cleaning up"); client_node_.reset(); - // Reset the listener before the buffer - tf_listener_.reset(); - tf_.reset(); - action_server_.reset(); + topic_logger_.reset(); plugin_lib_names_.clear(); current_bt_xml_filename_.clear(); blackboard_.reset(); @@ -255,70 +208,34 @@ class BtActionServer : public nav2_util::LifecycleNode bt_->resetGrootMonitor(); bt_.reset(); - RCLCPP_INFO(get_logger(), "Completed Cleaning up"); - return on_cleanup(); + RCLCPP_INFO(logger_, "Completed Cleaning up"); + return true; } /** * @brief Called when in shutdown state - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override - { - RCLCPP_INFO(get_logger(), "Shutting down"); - return on_shutdown(); - } - - /** - * @brief Action server callback + * @return true on SUCCESS and false on FAILURE */ - void executeCallback() + bool on_shutdown() { - if (!on_goal_received()) { - action_server_->terminate_current(); - return; - } - - // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run( - &tree_, - std::bind(&BtActionServer::on_loop, this), - std::bind(&BtActionServer::is_canceling, this)); - - // Make sure that the Bt is not in a running state from a previous execution - // note: if all the ControlNodes are implemented correctly, this is not needed. - bt_->haltAllActions(tree_.rootNode()); - - switch (rc) { - case nav2_behavior_tree::BtStatus::SUCCEEDED: - RCLCPP_INFO(get_logger(), "Goal succeeded"); - action_server_->succeeded_current(); - break; - - case nav2_behavior_tree::BtStatus::FAILED: - RCLCPP_ERROR(get_logger(), "Goal failed"); - action_server_->terminate_current(); - break; - - case nav2_behavior_tree::BtStatus::CANCELED: - RCLCPP_INFO(get_logger(), "Goal canceled"); - action_server_->terminate_all(); - break; - } + RCLCPP_INFO(logger_, "Shutting down"); + return true; } /** * @brief Replace current BT with another one - * @param bt_xml_filename The file containing the new BT + * @param bt_xml_filename The file containing the new BT, uses default filename if empty * @return true if the resulting BT correspond to the one in bt_xml_filename. false * if something went wrong, and previous BT is maintained */ bool loadBehaviorTree(const std::string & bt_xml_filename) { + // Empty filename is default for backward compatibility + auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; + // Use previous BT if it is the existing one - if (current_bt_xml_filename_ == bt_xml_filename) { - RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded"); + if (current_bt_xml_filename_ == filename) { + RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); return true; } @@ -326,10 +243,10 @@ class BtActionServer : public nav2_util::LifecycleNode bt_->resetGrootMonitor(); // Read the input BT XML from the specified file into a string - std::ifstream xml_file(bt_xml_filename); + std::ifstream xml_file(filename); if (!xml_file.good()) { - RCLCPP_ERROR(get_logger(), "Couldn't open input XML file: %s", bt_xml_filename.c_str()); + RCLCPP_ERROR(logger_, "Couldn't open input XML file: %s", filename.c_str()); return false; } @@ -339,27 +256,103 @@ class BtActionServer : public nav2_util::LifecycleNode // Create the Behavior Tree from the XML input tree_ = bt_->createTreeFromText(xml_string, blackboard_); - current_bt_xml_filename_ = bt_xml_filename; + topic_logger_ = std::make_unique(client_node_, tree_); + + current_bt_xml_filename_ = filename; - // get parameter for monitoring with Groot via ZMQ Publisher - if (get_parameter("enable_groot_monitoring").as_bool()) { - uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int(); - uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int(); + // Enable monitoring with Groot + if (enable_groot_monitoring_) { // optionally add max_msg_per_second = 25 (default) here try { - bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port); + bt_->addGrootMonitoring(&tree_, groot_zmq_publisher_port_, groot_zmq_server_port_); } catch (const std::logic_error & e) { - RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what()); + RCLCPP_ERROR(logger_, "ZMQ already enabled, Error: %s", e.what()); } } return true; } + /** + * @brief Getter function for BT Blackboard + * @return shared pointer to current BT blackboard + */ + BT::Blackboard::Ptr getBlackboard() const + { + return blackboard_; + } + + /** + * @brief Getter function for action server + * @return shared pointer to action server + */ + std::shared_ptr getActionServer() const + { + return action_server_; + } + +protected: + /** + * @brief Action server callback + */ + void executeCallback() + { + if (!on_goal_received_callback_(action_server_->get_current_goal())) { + action_server_->terminate_current(); + return; + } + + auto is_canceling = [this]() { + if (action_server_ == nullptr) { + RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling."); + return true; + } + if (!action_server_->is_server_active()) { + RCLCPP_DEBUG(logger_, "Action server is inactive. Canceling."); + return true; + } + return action_server_->is_cancel_requested(); + }; + + auto on_loop = [&]() { + if (action_server_->is_preempt_requested()) { + RCLCPP_INFO(logger_, "Received goal preemption request"); + action_server_->accept_pending_goal(); + on_goal_received_callback_(action_server_->get_current_goal()); + } + topic_logger_->flush(); + on_loop_callback_(); + }; + + // Execute the BT that was previously created in the configure step + nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); + + // Make sure that the Bt is not in a running state from a previous execution + // note: if all the ControlNodes are implemented correctly, this is not needed. + bt_->haltAllActions(tree_.rootNode()); + + switch (rc) { + case nav2_behavior_tree::BtStatus::SUCCEEDED: + RCLCPP_INFO(logger_, "Goal succeeded"); + action_server_->succeeded_current(); + break; + + case nav2_behavior_tree::BtStatus::FAILED: + RCLCPP_ERROR(logger_, "Goal failed"); + action_server_->terminate_current(); + break; + + case nav2_behavior_tree::BtStatus::CANCELED: + RCLCPP_INFO(logger_, "Goal canceled"); + action_server_->terminate_all(); + break; + } + } + // Action name std::string action_name_; // Our action server implements the template action - std::unique_ptr action_server_; + std::shared_ptr action_server_; // Behavior Tree to be executed when goal is received BT::Tree tree_; @@ -382,7 +375,27 @@ class BtActionServer : public nav2_util::LifecycleNode // Spinning transform that can be used by the BT nodes std::shared_ptr tf_; - std::shared_ptr tf_listener_; + + // Parent node + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + // Clock + rclcpp::Clock::SharedPtr clock_; + + // Logger + rclcpp::Logger logger_{rclcpp::get_logger("BtActionServer")}; + + // To publish BT logs + std::unique_ptr topic_logger_; + + // Parameters for Groot monitoring + bool enable_groot_monitoring_; + int groot_zmq_publisher_port_; + int groot_zmq_server_port_; + + // User-provided callbacks + OnGoalReceivedCallback on_goal_received_callback_; + OnLoopCallback on_loop_callback_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp new file mode 100644 index 00000000000..089cfe67b6e --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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__ROS_TOPIC_LOGGER_HPP_ +#define NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/loggers/abstract_logger.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_msgs/msg/behavior_tree_log.hpp" +#include "nav2_msgs/msg/behavior_tree_status_change.h" +#include "tf2_ros/buffer_interface.h" + +namespace nav2_behavior_tree +{ + +class RosTopicLogger : public BT::StatusChangeLogger +{ +public: + RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree) + : StatusChangeLogger(tree.rootNode()) + { + auto node = ros_node.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + log_pub_ = node->create_publisher( + "behavior_tree_log", + rclcpp::QoS(10)); + } + + void callback( + BT::Duration timestamp, + const BT::TreeNode & node, + BT::NodeStatus prev_status, + BT::NodeStatus status) override + { + nav2_msgs::msg::BehaviorTreeStatusChange event; + + // BT timestamps are a duration since the epoch. Need to convert to a time_point + // before converting to a msg. + event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); + event.node_name = node.name(); + event.previous_status = toStr(prev_status, false); + event.current_status = toStr(status, false); + event_log_.push_back(std::move(event)); + + RCLCPP_DEBUG( + logger_, "[%.3f]: %25s %s -> %s", + std::chrono::duration(timestamp).count(), + node.name().c_str(), + toStr(prev_status, true).c_str(), + toStr(status, true).c_str() ); + } + + void flush() override + { + if (!event_log_.empty()) { + auto log_msg = std::make_unique(); + log_msg->timestamp = clock_->now(); + log_msg->event_log = event_log_; + log_pub_->publish(std::move(log_msg)); + event_log_.clear(); + } + } + +protected: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("bt_navigator")}; + rclcpp::Publisher::SharedPtr log_pub_; + std::vector event_log_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_ diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index da362c91068..091b464a369 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -32,7 +32,6 @@ set(library_name ${executable_name}_core) add_library(${library_name} SHARED src/bt_navigator.cpp - src/ros_topic_logger.cpp ) set(dependencies diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 8e24c439c35..6840211cf8f 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -23,7 +23,6 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav_msgs/msg/path.hpp" -#include "nav2_bt_navigator/ros_topic_logger.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" namespace nav2_bt_navigator @@ -33,7 +32,7 @@ namespace nav2_bt_navigator * @brief An action server that uses behavior tree for navigating a robot to its * goal position. */ -class BtNavigator : public nav2_behavior_tree::BtActionServer +class BtNavigator : public nav2_util::LifecycleNode { public: /** @@ -46,15 +45,45 @@ class BtNavigator : public nav2_behavior_tree::BtActionServer> bt_action_server_; rclcpp::Subscription::SharedPtr goal_sub_; @@ -75,7 +104,9 @@ class BtNavigator : public nav2_behavior_tree::BtActionServer topic_logger_; + // Spinning transform that can be used by the BT nodes + std::shared_ptr tf_; + std::shared_ptr tf_listener_; std::shared_ptr feedback_msg_; }; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp deleted file mode 100644 index 27fe61eebce..00000000000 --- a/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2019 Intel Corporation -// -// 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_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_ -#define NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_ - -#include -#include "behaviortree_cpp_v3/loggers/abstract_logger.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_msgs/msg/behavior_tree_log.hpp" -#include "nav2_msgs/msg/behavior_tree_status_change.h" - -namespace nav2_bt_navigator -{ - -class RosTopicLogger : public BT::StatusChangeLogger -{ -public: - RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree); - - void callback( - BT::Duration timestamp, - const BT::TreeNode & node, - BT::NodeStatus prev_status, - BT::NodeStatus status) override; - - void flush() override; - -protected: - rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_{rclcpp::get_logger("bt_navigator")}; - rclcpp::Publisher::SharedPtr log_pub_; - std::vector event_log_; -}; - -} // namespace nav2_bt_navigator - -#endif // NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_ diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index c8882111846..01a4c80508c 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -27,8 +27,7 @@ namespace nav2_bt_navigator { BtNavigator::BtNavigator() -: nav2_behavior_tree::BtActionServer( - "navigate_to_pose", "bt_navigator", "", false), +: nav2_util::LifecycleNode("bt_navigator", "", false), start_time_(0) { declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); @@ -42,10 +41,19 @@ BtNavigator::~BtNavigator() } nav2_util::CallbackReturn -BtNavigator::on_configure() +BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) { + RCLCPP_INFO(get_logger(), "Configuring"); + + tf_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), get_node_timers_interface()); + tf_->setCreateTimerInterface(timer_interface); + tf_->setUsingDedicatedThread(true); + tf_listener_ = std::make_shared(*tf_, this, false); + self_client_ = rclcpp_action::create_client( - client_node_, "navigate_to_pose"); + shared_from_this(), "navigate_to_pose"); goal_sub_ = create_subscription( "goal_pose", @@ -56,49 +64,113 @@ BtNavigator::on_configure() robot_frame_ = get_parameter("robot_base_frame").as_string(); transform_tolerance_ = get_parameter("transform_tolerance").as_double(); - blackboard_->set("initial_pose_received", false); // NOLINT - blackboard_->set("number_recoveries", 0); // NOLINT + bt_action_server_ = std::make_unique>( + shared_from_this(), + "navigate_to_pose", + std::bind(&BtNavigator::on_goal_received, this, std::placeholders::_1), + std::bind(&BtNavigator::on_loop, this)); + + if (!bt_action_server_->on_configure(tf_)) { + return nav2_util::CallbackReturn::FAILURE; + } + + auto blackboard = bt_action_server_->getBlackboard(); + blackboard->set("initial_pose_received", false); // NOLINT + blackboard->set("number_recoveries", 0); // NOLINT + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + if (!bt_action_server_->on_activate()) { + return nav2_util::CallbackReturn::FAILURE; + } + + // create bond connection + createBond(); return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn -BtNavigator::on_cleanup() +BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { + RCLCPP_INFO(get_logger(), "Deactivating"); + + if (!bt_action_server_->on_deactivate()) { + return nav2_util::CallbackReturn::FAILURE; + } + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + // TODO(orduno) Fix the race condition between the worker thread ticking the tree + // and the main thread resetting the resources, see #1344 goal_sub_.reset(); self_client_.reset(); + + // Reset the listener before the buffer + tf_listener_.reset(); + tf_.reset(); + + if (!bt_action_server_->on_cleanup()) { + return nav2_util::CallbackReturn::FAILURE; + } + + bt_action_server_.reset(); + + RCLCPP_INFO(get_logger(), "Completed Cleaning up"); return nav2_util::CallbackReturn::SUCCESS; } -bool -BtNavigator::on_goal_received() +nav2_util::CallbackReturn +BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { - auto goal = action_server_->get_current_goal(); + RCLCPP_INFO(get_logger(), "Shutting down"); + + if (!bt_action_server_->on_shutdown()) { + return nav2_util::CallbackReturn::FAILURE; + } + + return nav2_util::CallbackReturn::SUCCESS; +} +bool +BtNavigator::on_goal_received(Action::Goal::ConstSharedPtr goal) +{ RCLCPP_INFO( get_logger(), "Begin navigating from current location to (%.2f, %.2f)", goal->pose.pose.position.x, goal->pose.pose.position.y); // Reset state for new action feedback start_time_ = now(); - blackboard_->set("number_recoveries", 0); // NOLINT - - // Update the goal pose on the blackboard - blackboard_->set("goal", goal->pose); auto bt_xml_filename = goal->behavior_tree; - // Empty id in request is default for backward compatibility - bt_xml_filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; - - if (!loadBehaviorTree(bt_xml_filename)) { + if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { RCLCPP_ERROR( get_logger(), "BT file not found: %s. Navigation canceled.", bt_xml_filename.c_str()); return false; } - topic_logger_ = std::make_unique(client_node_, tree_); + auto blackboard = bt_action_server_->getBlackboard(); + blackboard->set("number_recoveries", 0); // NOLINT + // Update the goal pose on the blackboard + blackboard->set("goal", goal->pose); + feedback_msg_ = std::make_shared(); return true; @@ -107,46 +179,25 @@ BtNavigator::on_goal_received() void BtNavigator::on_loop() { - if (action_server_->is_preempt_requested()) { - RCLCPP_INFO(get_logger(), "Received goal preemption request"); - action_server_->accept_pending_goal(); - on_goal_received(); - } - - topic_logger_->flush(); - // action server feedback (pose, duration of task, // number of recoveries, and distance remaining to goal) nav2_util::getCurrentPose( feedback_msg_->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_); + auto blackboard = bt_action_server_->getBlackboard(); + geometry_msgs::msg::PoseStamped goal_pose; - blackboard_->get("goal", goal_pose); + blackboard->get("goal", goal_pose); feedback_msg_->distance_remaining = nav2_util::geometry_utils::euclidean_distance( feedback_msg_->current_pose.pose, goal_pose.pose); int recovery_count = 0; - blackboard_->get("number_recoveries", recovery_count); + blackboard->get("number_recoveries", recovery_count); feedback_msg_->number_of_recoveries = recovery_count; feedback_msg_->navigation_time = now() - start_time_; - action_server_->publish_feedback(feedback_msg_); -} - -bool -BtNavigator::is_canceling() -{ - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Canceling."); - return true; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Canceling."); - return true; - } - return action_server_->is_cancel_requested(); + bt_action_server_->getActionServer()->publish_feedback(feedback_msg_); } void diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp deleted file mode 100644 index e85bae5fa81..00000000000 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2019 Intel Corporation -// -// 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_bt_navigator/ros_topic_logger.hpp" -#include "tf2_ros/buffer_interface.h" - -namespace nav2_bt_navigator -{ - -RosTopicLogger::RosTopicLogger( - const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree) -: StatusChangeLogger(tree.rootNode()) -{ - auto node = ros_node.lock(); - clock_ = node->get_clock(); - logger_ = node->get_logger(); - log_pub_ = node->create_publisher( - "behavior_tree_log", - rclcpp::QoS(10)); -} - -void RosTopicLogger::callback( - BT::Duration timestamp, - const BT::TreeNode & node, - BT::NodeStatus prev_status, - BT::NodeStatus status) -{ - nav2_msgs::msg::BehaviorTreeStatusChange event; - - // BT timestamps are a duration since the epoch. Need to convert to a time_point - // before converting to a msg. - event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); - event.node_name = node.name(); - event.previous_status = toStr(prev_status, false); - event.current_status = toStr(status, false); - event_log_.push_back(std::move(event)); - - RCLCPP_DEBUG( - logger_, "[%.3f]: %25s %s -> %s", - std::chrono::duration(timestamp).count(), - node.name().c_str(), - toStr(prev_status, true).c_str(), - toStr(status, true).c_str() ); -} - -void RosTopicLogger::flush() -{ - if (!event_log_.empty()) { - auto log_msg = std::make_unique(); - log_msg->timestamp = clock_->now(); - log_msg->event_log = event_log_; - log_pub_->publish(std::move(log_msg)); - event_log_.clear(); - } -} - -} // namespace nav2_bt_navigator From 046c78d44d625e1199ec0cdc7a59676fa52caa32 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Wed, 16 Dec 2020 07:50:53 +0530 Subject: [PATCH 05/13] Add comments --- .../include/nav2_behavior_tree/bt_action_server.hpp | 13 +------------ .../include/nav2_bt_navigator/bt_navigator.hpp | 11 +++++++++++ nav2_bt_navigator/src/bt_navigator.cpp | 1 + 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 9ea2bf22804..ef87a1b3c1c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -64,8 +64,6 @@ class BtActionServer logger_ = node->get_logger(); clock_ = node->get_clock(); - RCLCPP_INFO(logger_, "Creating"); - const std::vector plugin_libs = { "nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", @@ -113,8 +111,6 @@ class BtActionServer */ bool on_configure(const std::shared_ptr & tf) { - RCLCPP_INFO(logger_, "Configuring"); - auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; @@ -169,7 +165,6 @@ class BtActionServer */ bool on_activate() { - RCLCPP_INFO(logger_, "Activating"); if (!loadBehaviorTree(default_bt_xml_filename_)) { RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str()); return false; @@ -184,7 +179,6 @@ class BtActionServer */ bool on_deactivate() { - RCLCPP_INFO(logger_, "Deactivating"); action_server_->deactivate(); return true; } @@ -195,10 +189,8 @@ class BtActionServer */ bool on_cleanup() { - RCLCPP_INFO(logger_, "Cleaning up"); - client_node_.reset(); - + tf_.reset(); action_server_.reset(); topic_logger_.reset(); plugin_lib_names_.clear(); @@ -207,8 +199,6 @@ class BtActionServer bt_->haltAllActions(tree_.rootNode()); bt_->resetGrootMonitor(); bt_.reset(); - - RCLCPP_INFO(logger_, "Completed Cleaning up"); return true; } @@ -218,7 +208,6 @@ class BtActionServer */ bool on_shutdown() { - RCLCPP_INFO(logger_, "Shutting down"); return true; } diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 6840211cf8f..604869cf03c 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -81,8 +81,17 @@ class BtNavigator : public nav2_util::LifecycleNode using Action = nav2_msgs::action::NavigateToPose; + /** + * @brief A callback to be called when a new goal is received by the BT action server + * Can be used to check if goal is valid and put values on + * the blackboard which depend on the received goal + */ bool on_goal_received(Action::Goal::ConstSharedPtr goal); + /** + * @brief A callback that defines execution that happens on one iteration through the BT + * Can be used to publish action feedback + */ void on_loop(); /** @@ -91,6 +100,7 @@ class BtNavigator : public nav2_util::LifecycleNode */ void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); + // To handle all the BT related execution std::unique_ptr> bt_action_server_; rclcpp::Subscription::SharedPtr goal_sub_; @@ -108,6 +118,7 @@ class BtNavigator : public nav2_util::LifecycleNode std::shared_ptr tf_; std::shared_ptr tf_listener_; + // Feedback message std::shared_ptr feedback_msg_; }; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 01a4c80508c..d7eedf87847 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -30,6 +30,7 @@ BtNavigator::BtNavigator() : nav2_util::LifecycleNode("bt_navigator", "", false), start_time_(0) { + RCLCPP_INFO(get_logger(), "Creating"); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter("global_frame", std::string("map")); declare_parameter("robot_base_frame", std::string("base_link")); From ef181699df4443802e7a8386ad0d78cbbacde520 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 17 Dec 2020 11:51:48 +0530 Subject: [PATCH 06/13] Add on preempt callback, fix naming issues, and move tf to bt navigator --- .../nav2_behavior_tree/bt_action_server.hpp | 33 ++++++++----------- .../nav2_bt_navigator/bt_navigator.hpp | 13 ++++++-- nav2_bt_navigator/src/bt_navigator.cpp | 21 +++++++++--- 3 files changed, 41 insertions(+), 26 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index ef87a1b3c1c..7cb26662214 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -27,10 +27,6 @@ #include "nav2_behavior_tree/ros_topic_logger.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/simple_action_server.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" namespace nav2_behavior_tree { @@ -46,6 +42,7 @@ class BtActionServer typedef std::function OnGoalReceivedCallback; typedef std::function OnLoopCallback; + typedef std::function OnPreemptCallback; /** * @brief A constructor for nav2_behavior_tree::BtActionServer class @@ -54,11 +51,13 @@ class BtActionServer const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & action_name, OnGoalReceivedCallback on_goal_received_callback, - OnLoopCallback on_loop_callback) + OnLoopCallback on_loop_callback, + OnPreemptCallback on_preempt_callback = nullptr) : action_name_(action_name), node_(parent), on_goal_received_callback_(on_goal_received_callback), - on_loop_callback_(on_loop_callback) + on_loop_callback_(on_loop_callback), + on_preempt_callback_(on_preempt_callback) { auto node = node_.lock(); logger_ = node->get_logger(); @@ -109,7 +108,7 @@ class BtActionServer * and calls user-defined onConfigure. * @return true on SUCCESS and false on FAILURE */ - bool on_configure(const std::shared_ptr & tf) + bool on_configure() { auto node = node_.lock(); if (!node) { @@ -124,9 +123,6 @@ class BtActionServer // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); - // TF buffer - tf_ = tf; - action_server_ = std::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), @@ -145,7 +141,6 @@ class BtActionServer // Put items on the blackboard blackboard_->set("node", client_node_); // NOLINT - blackboard_->set>("tf_buffer", tf_); // NOLINT blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT // Get the BT filename to use from the node parameter @@ -190,7 +185,6 @@ class BtActionServer bool on_cleanup() { client_node_.reset(); - tf_.reset(); action_server_.reset(); topic_logger_.reset(); plugin_lib_names_.clear(); @@ -279,6 +273,11 @@ class BtActionServer return action_server_; } + std::string getCurrentBTFilename() const + { + return current_bt_xml_filename_; + } + protected: /** * @brief Action server callback @@ -303,10 +302,8 @@ class BtActionServer }; auto on_loop = [&]() { - if (action_server_->is_preempt_requested()) { - RCLCPP_INFO(logger_, "Received goal preemption request"); - action_server_->accept_pending_goal(); - on_goal_received_callback_(action_server_->get_current_goal()); + if (action_server_->is_preempt_requested() && on_preempt_callback_) { + on_preempt_callback_(); } topic_logger_->flush(); on_loop_callback_(); @@ -362,9 +359,6 @@ class BtActionServer // A regular, non-spinning ROS node that we can use for calls to the action client rclcpp::Node::SharedPtr client_node_; - // Spinning transform that can be used by the BT nodes - std::shared_ptr tf_; - // Parent node rclcpp_lifecycle::LifecycleNode::WeakPtr node_; @@ -385,6 +379,7 @@ class BtActionServer // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; + OnPreemptCallback on_preempt_callback_; }; } // namespace nav2_behavior_tree diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 604869cf03c..a7edc3ed592 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -24,6 +24,10 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/create_timer_ros.h" namespace nav2_bt_navigator { @@ -86,13 +90,18 @@ class BtNavigator : public nav2_util::LifecycleNode * Can be used to check if goal is valid and put values on * the blackboard which depend on the received goal */ - bool on_goal_received(Action::Goal::ConstSharedPtr goal); + bool onGoalReceived(Action::Goal::ConstSharedPtr goal); /** * @brief A callback that defines execution that happens on one iteration through the BT * Can be used to publish action feedback */ - void on_loop(); + void onLoop(); + + /** + * @brief A callback that is called when a preempt is requested + */ + void onPreempt(); /** * @brief A subscription and callback to handle the topic-based goal published diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index d7eedf87847..0f6d736baa0 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -68,14 +68,16 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) bt_action_server_ = std::make_unique>( shared_from_this(), "navigate_to_pose", - std::bind(&BtNavigator::on_goal_received, this, std::placeholders::_1), - std::bind(&BtNavigator::on_loop, this)); + std::bind(&BtNavigator::onGoalReceived, this, std::placeholders::_1), + std::bind(&BtNavigator::onLoop, this), + std::bind(&BtNavigator::onPreempt, this)); - if (!bt_action_server_->on_configure(tf_)) { + if (!bt_action_server_->on_configure()) { return nav2_util::CallbackReturn::FAILURE; } auto blackboard = bt_action_server_->getBlackboard(); + blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT @@ -149,7 +151,7 @@ BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) } bool -BtNavigator::on_goal_received(Action::Goal::ConstSharedPtr goal) +BtNavigator::onGoalReceived(Action::Goal::ConstSharedPtr goal) { RCLCPP_INFO( get_logger(), "Begin navigating from current location to (%.2f, %.2f)", @@ -178,7 +180,7 @@ BtNavigator::on_goal_received(Action::Goal::ConstSharedPtr goal) } void -BtNavigator::on_loop() +BtNavigator::onLoop() { // action server feedback (pose, duration of task, // number of recoveries, and distance remaining to goal) @@ -201,6 +203,15 @@ BtNavigator::on_loop() bt_action_server_->getActionServer()->publish_feedback(feedback_msg_); } +void +BtNavigator::onPreempt() +{ + RCLCPP_INFO(get_logger(), "Received goal preemption request"); + auto action_server = bt_action_server_->getActionServer(); + action_server->accept_pending_goal(); + onGoalReceived(action_server->get_current_goal()); +} + void BtNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose) { From a478cfac326fe825a077431c06d82302f6a8e648 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 18 Dec 2020 09:00:25 +0530 Subject: [PATCH 07/13] Add separate implementation header for BtActionServer --- .../nav2_behavior_tree/bt_action_server.hpp | 227 +--------------- .../bt_action_server_impl.hpp | 251 ++++++++++++++++++ nav2_bt_navigator/src/bt_navigator.cpp | 27 ++ 3 files changed, 288 insertions(+), 217 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 7cb26662214..e9de5bc9bb0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -18,9 +18,6 @@ #include #include #include -#include -#include -#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_behavior_tree/behavior_tree_engine.hpp" @@ -52,54 +49,12 @@ class BtActionServer const std::string & action_name, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, - OnPreemptCallback on_preempt_callback = nullptr) - : action_name_(action_name), - node_(parent), - on_goal_received_callback_(on_goal_received_callback), - on_loop_callback_(on_loop_callback), - on_preempt_callback_(on_preempt_callback) - { - auto node = node_.lock(); - logger_ = node->get_logger(); - clock_ = node->get_clock(); - - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_distance_traveled_condition_bt_node" - }; - - // Declare this node's parameters - node->declare_parameter("default_bt_xml_filename"); - node->declare_parameter("plugin_lib_names", plugin_libs); - node->declare_parameter("enable_groot_monitoring", true); - node->declare_parameter("groot_zmq_publisher_port", 1666); - node->declare_parameter("groot_zmq_server_port", 1667); - } + OnPreemptCallback on_preempt_callback = nullptr); /** * @brief A destructor for nav2_behavior_tree::BtActionServer class */ - ~BtActionServer() {} + ~BtActionServer(); /** * @brief Configures member variables @@ -108,102 +63,31 @@ class BtActionServer * and calls user-defined onConfigure. * @return true on SUCCESS and false on FAILURE */ - bool on_configure() - { - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - - // use suffix '_rclcpp_node' to keep parameter file consistency #1773 - auto options = rclcpp::NodeOptions().arguments( - {"--ros-args", - "-r", std::string("__node:=") + node->get_name() + "_rclcpp_node", - "--"}); - // Support for handling the topic-based goal pose from rviz - client_node_ = std::make_shared("_", options); - - action_server_ = std::make_shared( - node->get_node_base_interface(), - node->get_node_clock_interface(), - node->get_node_logging_interface(), - node->get_node_waitables_interface(), - action_name_, std::bind(&BtActionServer::executeCallback, this)); - - // Get the libraries to pull plugins from - plugin_lib_names_ = node->get_parameter("plugin_lib_names").as_string_array(); - - // Create the class that registers our custom nodes and executes the BT - bt_ = std::make_unique(plugin_lib_names_); - - // Create the blackboard that will be shared by all of the nodes in the tree - blackboard_ = BT::Blackboard::create(); - - // Put items on the blackboard - blackboard_->set("node", client_node_); // NOLINT - blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT - - // Get the BT filename to use from the node parameter - node->get_parameter("default_bt_xml_filename", default_bt_xml_filename_); - - // Get parameter for monitoring with Groot via ZMQ Publisher - node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_); - node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_); - node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_); - - return true; - } + bool on_configure(); /** * @brief Activates action server * @return true on SUCCESS and false on FAILURE */ - bool on_activate() - { - if (!loadBehaviorTree(default_bt_xml_filename_)) { - RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str()); - return false; - } - action_server_->activate(); - return true; - } + bool on_activate(); /** * @brief Deactivates action server * @return true on SUCCESS and false on FAILURE */ - bool on_deactivate() - { - action_server_->deactivate(); - return true; - } + bool on_deactivate(); /** * @brief Resets member variables * @return true on SUCCESS and false on FAILURE */ - bool on_cleanup() - { - client_node_.reset(); - action_server_.reset(); - topic_logger_.reset(); - plugin_lib_names_.clear(); - current_bt_xml_filename_.clear(); - blackboard_.reset(); - bt_->haltAllActions(tree_.rootNode()); - bt_->resetGrootMonitor(); - bt_.reset(); - return true; - } + bool on_cleanup(); /** * @brief Called when in shutdown state * @return true on SUCCESS and false on FAILURE */ - bool on_shutdown() - { - return true; - } + bool on_shutdown(); /** * @brief Replace current BT with another one @@ -211,49 +95,7 @@ class BtActionServer * @return true if the resulting BT correspond to the one in bt_xml_filename. false * if something went wrong, and previous BT is maintained */ - bool loadBehaviorTree(const std::string & bt_xml_filename) - { - // Empty filename is default for backward compatibility - auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; - - // Use previous BT if it is the existing one - if (current_bt_xml_filename_ == filename) { - RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); - return true; - } - - // if a new tree is created, than the ZMQ Publisher must be destroyed - bt_->resetGrootMonitor(); - - // Read the input BT XML from the specified file into a string - std::ifstream xml_file(filename); - - if (!xml_file.good()) { - RCLCPP_ERROR(logger_, "Couldn't open input XML file: %s", filename.c_str()); - return false; - } - - auto xml_string = std::string( - std::istreambuf_iterator(xml_file), - std::istreambuf_iterator()); - - // Create the Behavior Tree from the XML input - tree_ = bt_->createTreeFromText(xml_string, blackboard_); - topic_logger_ = std::make_unique(client_node_, tree_); - - current_bt_xml_filename_ = filename; - - // Enable monitoring with Groot - if (enable_groot_monitoring_) { - // optionally add max_msg_per_second = 25 (default) here - try { - bt_->addGrootMonitoring(&tree_, groot_zmq_publisher_port_, groot_zmq_server_port_); - } catch (const std::logic_error & e) { - RCLCPP_ERROR(logger_, "ZMQ already enabled, Error: %s", e.what()); - } - } - return true; - } + bool loadBehaviorTree(const std::string & bt_xml_filename); /** * @brief Getter function for BT Blackboard @@ -282,57 +124,7 @@ class BtActionServer /** * @brief Action server callback */ - void executeCallback() - { - if (!on_goal_received_callback_(action_server_->get_current_goal())) { - action_server_->terminate_current(); - return; - } - - auto is_canceling = [this]() { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling."); - return true; - } - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(logger_, "Action server is inactive. Canceling."); - return true; - } - return action_server_->is_cancel_requested(); - }; - - auto on_loop = [&]() { - if (action_server_->is_preempt_requested() && on_preempt_callback_) { - on_preempt_callback_(); - } - topic_logger_->flush(); - on_loop_callback_(); - }; - - // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); - - // Make sure that the Bt is not in a running state from a previous execution - // note: if all the ControlNodes are implemented correctly, this is not needed. - bt_->haltAllActions(tree_.rootNode()); - - switch (rc) { - case nav2_behavior_tree::BtStatus::SUCCEEDED: - RCLCPP_INFO(logger_, "Goal succeeded"); - action_server_->succeeded_current(); - break; - - case nav2_behavior_tree::BtStatus::FAILED: - RCLCPP_ERROR(logger_, "Goal failed"); - action_server_->terminate_current(); - break; - - case nav2_behavior_tree::BtStatus::CANCELED: - RCLCPP_INFO(logger_, "Goal canceled"); - action_server_->terminate_all(); - break; - } - } + void executeCallback(); // Action name std::string action_name_; @@ -384,4 +176,5 @@ class BtActionServer } // namespace nav2_behavior_tree +#include // NOLINT(build/include_order) #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp new file mode 100644 index 00000000000..ca3822c4b20 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -0,0 +1,251 @@ +// 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__BT_ACTION_SERVER_IMPL_HPP_ +#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ + +#include +#include +#include +#include +#include + +#include "nav2_msgs/action/navigate_to_pose.hpp" + +#include "nav2_behavior_tree/bt_action_server.hpp" + +namespace nav2_behavior_tree +{ + +template +BtActionServer::BtActionServer( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & action_name, + OnGoalReceivedCallback on_goal_received_callback, + OnLoopCallback on_loop_callback, + OnPreemptCallback on_preempt_callback) +: action_name_(action_name), + node_(parent), + on_goal_received_callback_(on_goal_received_callback), + on_loop_callback_(on_loop_callback), + on_preempt_callback_(on_preempt_callback) +{ + auto node = node_.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + // Declare this node's parameters + node->declare_parameter("default_bt_xml_filename"); + node->declare_parameter("enable_groot_monitoring", true); + node->declare_parameter("groot_zmq_publisher_port", 1666); + node->declare_parameter("groot_zmq_server_port", 1667); +} + +template +BtActionServer::~BtActionServer() +{} + +template +bool BtActionServer::on_configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // use suffix '_rclcpp_node' to keep parameter file consistency #1773 + auto options = rclcpp::NodeOptions().arguments( + {"--ros-args", + "-r", std::string("__node:=") + node->get_name() + "_rclcpp_node", + "--"}); + // Support for handling the topic-based goal pose from rviz + client_node_ = std::make_shared("_", options); + + action_server_ = std::make_shared( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + action_name_, std::bind(&BtActionServer::executeCallback, this)); + + // Get the libraries to pull plugins from + if (node->has_parameter("plugin_lib_names")) { + plugin_lib_names_ = node->get_parameter("plugin_lib_names").as_string_array(); + } else { + RCLCPP_ERROR(logger_, "'plugin_lib_names' param is not defined"); + return false; + } + + // Create the class that registers our custom nodes and executes the BT + bt_ = std::make_unique(plugin_lib_names_); + + // Create the blackboard that will be shared by all of the nodes in the tree + blackboard_ = BT::Blackboard::create(); + + // Put items on the blackboard + blackboard_->set("node", client_node_); // NOLINT + blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT + + // Get the BT filename to use from the node parameter + node->get_parameter("default_bt_xml_filename", default_bt_xml_filename_); + + // Get parameter for monitoring with Groot via ZMQ Publisher + node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_); + node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_); + node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_); + + return true; +} + +template +bool BtActionServer::on_activate() +{ + if (!loadBehaviorTree(default_bt_xml_filename_)) { + RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str()); + return false; + } + action_server_->activate(); + return true; +} + +template +bool BtActionServer::on_deactivate() +{ + action_server_->deactivate(); + return true; +} + +template +bool BtActionServer::on_cleanup() +{ + client_node_.reset(); + action_server_.reset(); + topic_logger_.reset(); + plugin_lib_names_.clear(); + current_bt_xml_filename_.clear(); + blackboard_.reset(); + bt_->haltAllActions(tree_.rootNode()); + bt_->resetGrootMonitor(); + bt_.reset(); + return true; +} + +template +bool BtActionServer::on_shutdown() +{ + return true; +} + +template +bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) +{ + // Empty filename is default for backward compatibility + auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; + + // Use previous BT if it is the existing one + if (current_bt_xml_filename_ == filename) { + RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); + return true; + } + + // if a new tree is created, than the ZMQ Publisher must be destroyed + bt_->resetGrootMonitor(); + + // Read the input BT XML from the specified file into a string + std::ifstream xml_file(filename); + + if (!xml_file.good()) { + RCLCPP_ERROR(logger_, "Couldn't open input XML file: %s", filename.c_str()); + return false; + } + + auto xml_string = std::string( + std::istreambuf_iterator(xml_file), + std::istreambuf_iterator()); + + // Create the Behavior Tree from the XML input + tree_ = bt_->createTreeFromText(xml_string, blackboard_); + topic_logger_ = std::make_unique(client_node_, tree_); + + current_bt_xml_filename_ = filename; + + // Enable monitoring with Groot + if (enable_groot_monitoring_) { + // optionally add max_msg_per_second = 25 (default) here + try { + bt_->addGrootMonitoring(&tree_, groot_zmq_publisher_port_, groot_zmq_server_port_); + } catch (const std::logic_error & e) { + RCLCPP_ERROR(logger_, "ZMQ already enabled, Error: %s", e.what()); + } + } + + return true; +} + +template +void BtActionServer::executeCallback() +{ + if (!on_goal_received_callback_(action_server_->get_current_goal())) { + action_server_->terminate_current(); + return; + } + + auto is_canceling = [this]() { + if (action_server_ == nullptr) { + RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling."); + return true; + } + if (!action_server_->is_server_active()) { + RCLCPP_DEBUG(logger_, "Action server is inactive. Canceling."); + return true; + } + return action_server_->is_cancel_requested(); + }; + + auto on_loop = [&]() { + if (action_server_->is_preempt_requested() && on_preempt_callback_) { + on_preempt_callback_(); + } + topic_logger_->flush(); + on_loop_callback_(); + }; + + // Execute the BT that was previously created in the configure step + nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); + + // Make sure that the Bt is not in a running state from a previous execution + // note: if all the ControlNodes are implemented correctly, this is not needed. + bt_->haltAllActions(tree_.rootNode()); + + switch (rc) { + case nav2_behavior_tree::BtStatus::SUCCEEDED: + RCLCPP_INFO(logger_, "Goal succeeded"); + action_server_->succeeded_current(); + break; + + case nav2_behavior_tree::BtStatus::FAILED: + RCLCPP_ERROR(logger_, "Goal failed"); + action_server_->terminate_current(); + break; + + case nav2_behavior_tree::BtStatus::CANCELED: + RCLCPP_INFO(logger_, "Goal canceled"); + action_server_->terminate_all(); + break; + } +} + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 0f6d736baa0..8fa08b61bee 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -31,6 +31,33 @@ BtNavigator::BtNavigator() start_time_(0) { RCLCPP_INFO(get_logger(), "Creating"); + + const std::vector plugin_libs = { + "nav2_compute_path_to_pose_action_bt_node", + "nav2_follow_path_action_bt_node", + "nav2_back_up_action_bt_node", + "nav2_spin_action_bt_node", + "nav2_wait_action_bt_node", + "nav2_clear_costmap_service_bt_node", + "nav2_is_stuck_condition_bt_node", + "nav2_goal_reached_condition_bt_node", + "nav2_initial_pose_received_condition_bt_node", + "nav2_goal_updated_condition_bt_node", + "nav2_reinitialize_global_localization_service_bt_node", + "nav2_rate_controller_bt_node", + "nav2_distance_controller_bt_node", + "nav2_speed_controller_bt_node", + "nav2_truncate_path_action_bt_node", + "nav2_goal_updater_node_bt_node", + "nav2_recovery_node_bt_node", + "nav2_pipeline_sequence_bt_node", + "nav2_round_robin_node_bt_node", + "nav2_transform_available_condition_bt_node", + "nav2_time_expired_condition_bt_node", + "nav2_distance_traveled_condition_bt_node" + }; + + declare_parameter("plugin_lib_names", plugin_libs); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter("global_frame", std::string("map")); declare_parameter("robot_base_frame", std::string("base_link")); From e68a45cc9fb6e8859d47289c80c1d3168bc1a8f4 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 18 Dec 2020 09:06:50 +0530 Subject: [PATCH 08/13] Fix cpplint error --- nav2_bt_navigator/src/bt_navigator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 8fa08b61bee..1c024f9ad7a 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" From 3a600c5f6be44a378962ffabde926eda56b63bdb Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 19 Dec 2020 02:44:09 +0530 Subject: [PATCH 09/13] Pass plugin library names as argument to BtActionServer --- .../include/nav2_behavior_tree/bt_action_server.hpp | 1 + .../nav2_behavior_tree/bt_action_server_impl.hpp | 11 +++-------- nav2_bt_navigator/src/bt_navigator.cpp | 4 ++++ 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index e9de5bc9bb0..2bbd94c648e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -47,6 +47,7 @@ class BtActionServer explicit BtActionServer( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & action_name, + const std::vector & plugin_lib_names, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback = nullptr); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index ca3822c4b20..58315f3e29e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "nav2_msgs/action/navigate_to_pose.hpp" @@ -32,10 +33,12 @@ template BtActionServer::BtActionServer( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & action_name, + const std::vector & plugin_lib_names, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback) : action_name_(action_name), + plugin_lib_names_(plugin_lib_names), node_(parent), on_goal_received_callback_(on_goal_received_callback), on_loop_callback_(on_loop_callback), @@ -79,14 +82,6 @@ bool BtActionServer::on_configure() node->get_node_waitables_interface(), action_name_, std::bind(&BtActionServer::executeCallback, this)); - // Get the libraries to pull plugins from - if (node->has_parameter("plugin_lib_names")) { - plugin_lib_names_ = node->get_parameter("plugin_lib_names").as_string_array(); - } else { - RCLCPP_ERROR(logger_, "'plugin_lib_names' param is not defined"); - return false; - } - // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 1c024f9ad7a..4d41d55e4c0 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -93,9 +93,13 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) robot_frame_ = get_parameter("robot_base_frame").as_string(); transform_tolerance_ = get_parameter("transform_tolerance").as_double(); + // Libraries to pull plugins (BT Nodes) from + auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array(); + bt_action_server_ = std::make_unique>( shared_from_this(), "navigate_to_pose", + plugin_lib_names, std::bind(&BtNavigator::onGoalReceived, this, std::placeholders::_1), std::bind(&BtNavigator::onLoop, this), std::bind(&BtNavigator::onPreempt, this)); From 68091763be9fc53396ac8ed3e473bddcfd3fbeae Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 19 Dec 2020 03:54:27 +0530 Subject: [PATCH 10/13] Remove action server getter and update onPreempt to not load BT --- .../nav2_behavior_tree/bt_action_server.hpp | 33 ++++++++++--- .../src/behavior_tree_engine.cpp | 2 +- .../nav2_bt_navigator/bt_navigator.hpp | 8 ++-- nav2_bt_navigator/src/bt_navigator.cpp | 48 ++++++++++--------- 4 files changed, 58 insertions(+), 33 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 2bbd94c648e..755ff2fc371 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -96,7 +96,7 @@ class BtActionServer * @return true if the resulting BT correspond to the one in bt_xml_filename. false * if something went wrong, and previous BT is maintained */ - bool loadBehaviorTree(const std::string & bt_xml_filename); + bool loadBehaviorTree(const std::string & bt_xml_filename = ""); /** * @brief Getter function for BT Blackboard @@ -108,17 +108,36 @@ class BtActionServer } /** - * @brief Getter function for action server - * @return shared pointer to action server + * @brief Getter function for current BT XML filename + * @return string */ - std::shared_ptr getActionServer() const + std::string getCurrentBTFilename() const { - return action_server_; + return current_bt_xml_filename_; } - std::string getCurrentBTFilename() const + /** + * @brief Wrapper function to accept pending goal if a preempt has been requested + */ + const std::shared_ptr accept_pending_goal() { - return current_bt_xml_filename_; + return action_server_->accept_pending_goal(); + } + + /** + * @brief Wrapper function to get current goal + */ + const std::shared_ptr get_current_goal() const + { + return action_server_->get_current_goal(); + } + + /** + * @brief Wrapper function to publish action feedback + */ + void publish_feedback(typename std::shared_ptr feedback) + { + action_server_->publish_feedback(feedback); } protected: diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 93841615d9e..7b89c9f3f0a 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp_v3/utils/shared_library.h" - +#include "nav2_behavior_tree/bt_action_server.hpp" namespace nav2_behavior_tree { diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index a7edc3ed592..6807d17dd60 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -103,6 +103,11 @@ class BtNavigator : public nav2_util::LifecycleNode */ void onPreempt(); + /** + * @brief Goal pose initialization on the blackboard + */ + void initializeGoalPose(Action::Goal::ConstSharedPtr goal); + /** * @brief A subscription and callback to handle the topic-based goal published * from rviz @@ -126,9 +131,6 @@ class BtNavigator : public nav2_util::LifecycleNode // Spinning transform that can be used by the BT nodes std::shared_ptr tf_; std::shared_ptr tf_listener_; - - // Feedback message - std::shared_ptr feedback_msg_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 4d41d55e4c0..976340e9772 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -185,13 +185,6 @@ BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) bool BtNavigator::onGoalReceived(Action::Goal::ConstSharedPtr goal) { - RCLCPP_INFO( - get_logger(), "Begin navigating from current location to (%.2f, %.2f)", - goal->pose.pose.position.x, goal->pose.pose.position.y); - - // Reset state for new action feedback - start_time_ = now(); - auto bt_xml_filename = goal->behavior_tree; if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { @@ -201,12 +194,7 @@ BtNavigator::onGoalReceived(Action::Goal::ConstSharedPtr goal) return false; } - auto blackboard = bt_action_server_->getBlackboard(); - blackboard->set("number_recoveries", 0); // NOLINT - // Update the goal pose on the blackboard - blackboard->set("goal", goal->pose); - - feedback_msg_ = std::make_shared(); + initializeGoalPose(goal); return true; } @@ -216,32 +204,48 @@ BtNavigator::onLoop() { // action server feedback (pose, duration of task, // number of recoveries, and distance remaining to goal) + auto feedback_msg = std::make_shared(); + nav2_util::getCurrentPose( - feedback_msg_->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_); + feedback_msg->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_); auto blackboard = bt_action_server_->getBlackboard(); geometry_msgs::msg::PoseStamped goal_pose; blackboard->get("goal", goal_pose); - feedback_msg_->distance_remaining = nav2_util::geometry_utils::euclidean_distance( - feedback_msg_->current_pose.pose, goal_pose.pose); + feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance( + feedback_msg->current_pose.pose, goal_pose.pose); int recovery_count = 0; blackboard->get("number_recoveries", recovery_count); - feedback_msg_->number_of_recoveries = recovery_count; - feedback_msg_->navigation_time = now() - start_time_; + feedback_msg->number_of_recoveries = recovery_count; + feedback_msg->navigation_time = now() - start_time_; - bt_action_server_->getActionServer()->publish_feedback(feedback_msg_); + bt_action_server_->publish_feedback(feedback_msg); } void BtNavigator::onPreempt() { RCLCPP_INFO(get_logger(), "Received goal preemption request"); - auto action_server = bt_action_server_->getActionServer(); - action_server->accept_pending_goal(); - onGoalReceived(action_server->get_current_goal()); + initializeGoalPose(bt_action_server_->accept_pending_goal()); +} + +void +BtNavigator::initializeGoalPose(Action::Goal::ConstSharedPtr goal) +{ + RCLCPP_INFO( + get_logger(), "Begin navigating from current location to (%.2f, %.2f)", + goal->pose.pose.position.x, goal->pose.pose.position.y); + + // Reset state for new action feedback + start_time_ = now(); + auto blackboard = bt_action_server_->getBlackboard(); + blackboard->set("number_recoveries", 0); // NOLINT + + // Update the goal pose on the blackboard + blackboard->set("goal", goal->pose); } void From 8cab84c2bb44cc232ac1dc030085707b4e70bf4a Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 22 Dec 2020 04:03:32 +0530 Subject: [PATCH 11/13] Fix unnecessary include --- nav2_behavior_tree/src/behavior_tree_engine.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 7b89c9f3f0a..93841615d9e 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp_v3/utils/shared_library.h" -#include "nav2_behavior_tree/bt_action_server.hpp" + namespace nav2_behavior_tree { From e9b684c7b7e3fd04efec213c2638c20fee357a28 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 5 Jan 2021 17:46:55 +0530 Subject: [PATCH 12/13] Fix function names Signed-off-by: Sarthak Mittal --- .../include/nav2_behavior_tree/bt_action_server.hpp | 6 +++--- nav2_bt_navigator/src/bt_navigator.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 755ff2fc371..ec7d9bad1cf 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -119,7 +119,7 @@ class BtActionServer /** * @brief Wrapper function to accept pending goal if a preempt has been requested */ - const std::shared_ptr accept_pending_goal() + const std::shared_ptr acceptPendingGoal() { return action_server_->accept_pending_goal(); } @@ -127,7 +127,7 @@ class BtActionServer /** * @brief Wrapper function to get current goal */ - const std::shared_ptr get_current_goal() const + const std::shared_ptr getCurrentGoal() const { return action_server_->get_current_goal(); } @@ -135,7 +135,7 @@ class BtActionServer /** * @brief Wrapper function to publish action feedback */ - void publish_feedback(typename std::shared_ptr feedback) + void publishFeedback(typename std::shared_ptr feedback) { action_server_->publish_feedback(feedback); } diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 976340e9772..4dc006156aa 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -222,14 +222,14 @@ BtNavigator::onLoop() feedback_msg->number_of_recoveries = recovery_count; feedback_msg->navigation_time = now() - start_time_; - bt_action_server_->publish_feedback(feedback_msg); + bt_action_server_->publishFeedback(feedback_msg); } void BtNavigator::onPreempt() { RCLCPP_INFO(get_logger(), "Received goal preemption request"); - initializeGoalPose(bt_action_server_->accept_pending_goal()); + initializeGoalPose(bt_action_server_->acceptPendingGoal()); } void From 41a201b0a28861eab4e53d3bf3ded53b0711a8e2 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Wed, 13 Jan 2021 18:44:08 +0530 Subject: [PATCH 13/13] Fix typo --- .../include/nav2_behavior_tree/bt_action_server.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index ec7d9bad1cf..4d38d514b39 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -158,7 +158,7 @@ class BtActionServer // The blackboard shared by all of the nodes in the tree BT::Blackboard::Ptr blackboard_; - // The XML fiñe that cointains the Behavior Tree to create + // The XML file that cointains the Behavior Tree to create std::string current_bt_xml_filename_; std::string default_bt_xml_filename_;