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..4d38d514b39 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -0,0 +1,200 @@ +// 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 "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" + +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: + using ActionServer = nav2_util::SimpleActionServer; + + typedef std::function OnGoalReceivedCallback; + typedef std::function OnLoopCallback; + typedef std::function OnPreemptCallback; + + /** + * @brief A constructor for nav2_behavior_tree::BtActionServer class + */ + 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); + + /** + * @brief A destructor for nav2_behavior_tree::BtActionServer class + */ + ~BtActionServer(); + + /** + * @brief Configures member variables + * + * Initializes action server for, builds behavior tree from xml file, + * and calls user-defined onConfigure. + * @return true on SUCCESS and false on FAILURE + */ + bool on_configure(); + + /** + * @brief Activates action server + * @return true on SUCCESS and false on FAILURE + */ + bool on_activate(); + + /** + * @brief Deactivates action server + * @return true on SUCCESS and false on FAILURE + */ + bool on_deactivate(); + + /** + * @brief Resets member variables + * @return true on SUCCESS and false on FAILURE + */ + bool on_cleanup(); + + /** + * @brief Called when in shutdown state + * @return true on SUCCESS and false on FAILURE + */ + bool on_shutdown(); + + /** + * @brief Replace current BT with another one + * @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 = ""); + + /** + * @brief Getter function for BT Blackboard + * @return shared pointer to current BT blackboard + */ + BT::Blackboard::Ptr getBlackboard() const + { + return blackboard_; + } + + /** + * @brief Getter function for current BT XML filename + * @return string + */ + std::string getCurrentBTFilename() const + { + return current_bt_xml_filename_; + } + + /** + * @brief Wrapper function to accept pending goal if a preempt has been requested + */ + const std::shared_ptr acceptPendingGoal() + { + return action_server_->accept_pending_goal(); + } + + /** + * @brief Wrapper function to get current goal + */ + const std::shared_ptr getCurrentGoal() const + { + return action_server_->get_current_goal(); + } + + /** + * @brief Wrapper function to publish action feedback + */ + void publishFeedback(typename std::shared_ptr feedback) + { + action_server_->publish_feedback(feedback); + } + +protected: + /** + * @brief Action server callback + */ + void executeCallback(); + + // Action name + std::string action_name_; + + // Our action server implements the template action + std::shared_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 file 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_; + + // 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_; + OnPreemptCallback on_preempt_callback_; +}; + +} // 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..58315f3e29e --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -0,0 +1,246 @@ +// 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 + +#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, + 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), + 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)); + + // 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_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 9023f829af8..6807d17dd60 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,14 @@ #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 "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" -#include "tf2_ros/buffer.h" namespace nav2_bt_navigator { @@ -86,66 +85,52 @@ class BtNavigator : public nav2_util::LifecycleNode using Action = nav2_msgs::action::NavigateToPose; - using ActionServer = nav2_util::SimpleActionServer; + /** + * @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 onGoalReceived(Action::Goal::ConstSharedPtr goal); - // Our action server implements the NavigateToPose action - std::unique_ptr action_server_; + /** + * @brief A callback that defines execution that happens on one iteration through the BT + * Can be used to publish action feedback + */ + void onLoop(); /** - * @brief Action server callbacks + * @brief A callback that is called when a preempt is requested */ - void navigateToPose(); + void onPreempt(); /** * @brief Goal pose initialization on the blackboard */ - void initializeGoalPose(); + void initializeGoalPose(Action::Goal::ConstSharedPtr goal); /** * @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_; + // To handle all the BT related execution + std::unique_ptr> bt_action_server_; - // The wrapper class for the BT functionality - std::unique_ptr bt_; - - // 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_; + + // Spinning transform that can be used by the BT nodes + std::shared_ptr tf_; + std::shared_ptr tf_listener_; }; } // namespace nav2_bt_navigator 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 9d224b0d5a5..4dc006156aa 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -14,18 +14,15 @@ #include "nav2_bt_navigator/bt_navigator.hpp" -#include #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 { @@ -61,16 +58,11 @@ BtNavigator::BtNavigator() "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() @@ -82,17 +74,6 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) { 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()); @@ -100,83 +81,39 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_->setUsingDedicatedThread(true); tf_listener_ = std::make_shared(*tf_, this, false); + self_client_ = rclcpp_action::create_client( + shared_from_this(), "navigate_to_pose"); + 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(); + // Libraries to pull plugins (BT Nodes) from + auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array(); - // 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 + 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)); - // 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 (!bt_action_server_->on_configure()) { + return nav2_util::CallbackReturn::FAILURE; } - // if a new tree is created, than the ZMQ Publisher must be destroyed - bt_->resetGrootMonitor(); + 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 - // 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; + return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn @@ -184,13 +121,10 @@ BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) { 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()); + if (!bt_action_server_->on_activate()) { return nav2_util::CallbackReturn::FAILURE; } - action_server_->activate(); - // create bond connection createBond(); @@ -202,7 +136,9 @@ BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - action_server_->deactivate(); + if (!bt_action_server_->on_deactivate()) { + return nav2_util::CallbackReturn::FAILURE; + } // destroy bond connection destroyBond(); @@ -218,20 +154,17 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // 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(); + 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; @@ -241,109 +174,78 @@ nav2_util::CallbackReturn BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Shutting down"); + + if (!bt_action_server_->on_shutdown()) { + return nav2_util::CallbackReturn::FAILURE; + } + return nav2_util::CallbackReturn::SUCCESS; } -void -BtNavigator::navigateToPose() +bool +BtNavigator::onGoalReceived(Action::Goal::ConstSharedPtr goal) { - initializeGoalPose(); + auto bt_xml_filename = goal->behavior_tree; - auto is_canceling = [this]() { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Canceling."); - return true; - } + 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; + } - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Canceling."); - return true; - } + initializeGoalPose(goal); - return action_server_->is_cancel_requested(); - }; + return true; +} - std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree; +void +BtNavigator::onLoop() +{ + // action server feedback (pose, duration of task, + // number of recoveries, and distance remaining to goal) + auto feedback_msg = std::make_shared(); - // Empty id in request is default for backward compatibility - bt_xml_filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; + nav2_util::getCurrentPose( + feedback_msg->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_); - if (!loadBehaviorTree(bt_xml_filename)) { - RCLCPP_ERROR( - get_logger(), "BT file not found: %s. Navigation canceled.", - bt_xml_filename.c_str()); - action_server_->terminate_current(); - return; - } + auto blackboard = bt_action_server_->getBlackboard(); - 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; - } + 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_; + + bt_action_server_->publishFeedback(feedback_msg); } void -BtNavigator::initializeGoalPose() +BtNavigator::onPreempt() { - auto goal = action_server_->get_current_goal(); + RCLCPP_INFO(get_logger(), "Received goal preemption request"); + initializeGoalPose(bt_action_server_->acceptPendingGoal()); +} +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(); - blackboard_->set("number_recoveries", 0); // NOLINT + auto blackboard = bt_action_server_->getBlackboard(); + blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard_->set("goal", goal->pose); + blackboard->set("goal", goal->pose); } 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