diff --git a/plansys2_bt_actions/CMakeLists.txt b/plansys2_bt_actions/CMakeLists.txt index 2691a5bc6..ddd6017a9 100644 --- a/plansys2_bt_actions/CMakeLists.txt +++ b/plansys2_bt_actions/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(plansys2_executor REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(lifecycle_msgs REQUIRED) +find_package(std_msgs REQUIRED) set(BT_ACTIONS_SOURCES src/plansys2_bt_actions/BTAction.cpp @@ -28,6 +29,8 @@ target_link_libraries(${PROJECT_NAME} rclcpp::rclcpp rclcpp_action::rclcpp_action rclcpp_lifecycle::rclcpp_lifecycle + PRIVATE + ${std_msgs_TARGETS} ) add_executable(bt_action_node diff --git a/plansys2_bt_actions/README.md b/plansys2_bt_actions/README.md index 1ab8e63a7..96de16cd0 100644 --- a/plansys2_bt_actions/README.md +++ b/plansys2_bt_actions/README.md @@ -8,14 +8,12 @@ These parameters are 1. `action_name`: The name of the `plansys2` action to implement. Note that this name must match what is in your pddl domain file. 2. `bt_xml_file`: An absolute path to the BT `.xml` file to execute. 3. `plugins`: a list of BehaviorTree.CPP shared libraries to load. Any BT node which is in the `.xml` but is not provided by the BehaviorTree.CPP library itself must be in one of the libraries specified -4. `enable_groot_monitoring`: a boolean which specifies if ZMQ publisher should be created, for use with [Groot](https://github.com/BehaviorTree/Groot) (default is `false`) -5. `publisher_port`: the ZMQ publisher port to use (if `enable_groot_monitoring` is enabled) -6. `server_port`: the ZMQ server port to use (if `enable_groot_monitoring` is enabled) -7. `max_msgs_per_second`: max ZMQ messages per second (if `enable_groot_monitoring` is enabled) -8. `bt_file_logging`: a boolean which [enables logging of BT state changes in `.fbl` files](https://www.behaviortree.dev/tutorial_05_subtrees/), useful for playing back behavior tree execution using `Groot` (default is `false`) -9. `bt_minitrace_logging`: a boolean which enables logging of `.json` files for recording the execution time of each node (default is `false`) +4. `enable_groot_monitoring`: a boolean which specifies if a Groot2 publisher should be created, for use with [Groot](https://www.behaviortree.dev/groot/) (default is `false`) +5. `server_port`: the Groot2 server port to use (if `enable_groot_monitoring` is enabled) +6. `bt_file_logging`: a boolean which [enables logging of BT state changes in `.btlog` files](https://www.behaviortree.dev/docs/tutorial-basics/tutorial_11_groot2), useful for playing back behavior tree execution using `Groot2` (default is `false`) +7. `bt_minitrace_logging`: a boolean which enables logging of `.json` files for recording the execution time of each node (default is `false`) -Files created by the `.fbl` and minitrace loggers are stored in `/tmp//`, with names containing a timestamp. +Files created by the `.btlog` and minitrace loggers are stored in `/tmp//`, with names containing a timestamp. ### BT node for calling ROS2 action servers The `BtActionNode` template class provides a convenient means of calling ROS2 action servers from within a BT. It takes care of the details of setting up and handling a ROS action client, reducing code duplication and providing a simple API. diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp index e7e015edb..6c6b84b77 100644 --- a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp +++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp @@ -24,6 +24,7 @@ #include "behaviortree_cpp/xml_parsing.h" #include "behaviortree_cpp/loggers/bt_file_logger_v2.h" #include "behaviortree_cpp/loggers/bt_minitrace_logger.h" +#include "behaviortree_cpp/loggers/groot2_publisher.h" #include "plansys2_executor/ActionExecutorClient.hpp" #include "rclcpp/rclcpp.hpp" @@ -34,9 +35,7 @@ namespace plansys2 class BTAction : public plansys2::ActionExecutorClient { public: - explicit BTAction( - const std::string & action, - const std::chrono::nanoseconds & rate); + explicit BTAction(const std::string & action, const std::chrono::nanoseconds & rate); const std::string & getActionName() const {return action_;} const std::string & getBTFile() const {return bt_xml_file_;} @@ -56,6 +55,18 @@ class BTAction : public plansys2::ActionExecutorClient void do_work(); + /** + * @brief Add Groot2 monitor to publish BT status changes + * @param tree BT to monitor + * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port) + */ + void addGrootMonitoring(BT::Tree * tree, uint16_t server_port); + + /** + * @brief Reset Groot2 monitor + */ + void resetGrootMonitor(); + BT::BehaviorTreeFactory factory_; private: @@ -67,6 +78,8 @@ class BTAction : public plansys2::ActionExecutorClient bool finished_; std::unique_ptr bt_file_logger_; std::unique_ptr bt_minitrace_logger_; + // Groot2 monitor + std::unique_ptr groot_monitor_; }; } // namespace plansys2 diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/JSONUtils.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/JSONUtils.hpp new file mode 100644 index 000000000..55a70d48e --- /dev/null +++ b/plansys2_bt_actions/include/plansys2_bt_actions/JSONUtils.hpp @@ -0,0 +1,49 @@ +// Copyright (c) 2025 Alberto J. Tudela Roldán +// Copyright (c) 2025 Grupo Avispa, DTE, Universidad de Málaga +// +// 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 PLANSYS2_BT_ACTIONS__JSONUTILS_HPP_ +#define PLANSYS2_BT_ACTIONS__JSONUTILS_HPP_ + +#include + +#include "behaviortree_cpp/json_export.h" +#include "builtin_interfaces/msg/time.hpp" +#include "std_msgs/msg/header.hpp" + +// The follow templates are required when using Groot 2 to visualize the BT. They +// convert the data types into JSON format easy for visualization. + +namespace builtin_interfaces::msg +{ +BT_JSON_CONVERTER(builtin_interfaces::msg::Time, msg) +{ + add_field("sec", &msg.sec); + add_field("nanosec", &msg.nanosec); +} + +} // namespace builtin_interfaces::msg + +namespace std_msgs::msg +{ + +BT_JSON_CONVERTER(std_msgs::msg::Header, msg) +{ + add_field("stamp", &msg.stamp); + add_field("frame_id", &msg.frame_id); +} + +} // namespace std_msgs::msg + +#endif // PLANSYS2_BT_ACTIONS__JSONUTILS_HPP_ diff --git a/plansys2_bt_actions/package.xml b/plansys2_bt_actions/package.xml index 47d6316b2..d6b0fd824 100644 --- a/plansys2_bt_actions/package.xml +++ b/plansys2_bt_actions/package.xml @@ -17,6 +17,8 @@ rclcpp_lifecycle plansys2_executor behaviortree_cpp + lifecycle_msgs + std_msgs ament_lint_common ament_lint_auto diff --git a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp index 84e2531a8..dd9a5471d 100644 --- a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp +++ b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp @@ -22,22 +22,24 @@ #include #include +#include "behaviortree_cpp/json_export.h" #include "behaviortree_cpp/utils/shared_library.h" +#include "std_msgs/msg/header.hpp" #include "plansys2_bt_actions/BTAction.hpp" +#include "plansys2_bt_actions/JSONUtils.hpp" namespace plansys2 { -BTAction::BTAction( - const std::string & action, - const std::chrono::nanoseconds & rate) +BTAction::BTAction(const std::string & action, const std::chrono::nanoseconds & rate) : ActionExecutorClient(action, rate) { declare_parameter("bt_xml_file", ""); - declare_parameter>( - "plugins", std::vector({})); + declare_parameter>("plugins", std::vector({})); declare_parameter("bt_file_logging", false); declare_parameter("bt_minitrace_logging", false); + declare_parameter("enable_groot_monitoring", false); + declare_parameter("server_port", -1); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -75,6 +77,9 @@ BTAction::on_cleanup(const rclcpp_lifecycle::State & previous_state) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) { + // If a new tree is created, than the Groot2 Publisher must be destroyed + resetGrootMonitor(); + try { tree_ = factory_.createTreeFromFile(bt_xml_file_, blackboard_); } catch (const std::exception & ex) { @@ -108,7 +113,7 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) filename << std::put_time(std::localtime(&now_time_t), "%Y_%m_%d__%H_%M_%S"); if (get_parameter("bt_file_logging").as_bool()) { - std::string filename_extension = filename.str() + ".fbl"; + std::string filename_extension = filename.str() + ".btlog"; RCLCPP_INFO_STREAM( get_logger(), "Logging to file: " << filename_extension); @@ -126,6 +131,17 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) } } + bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool(); + int server_port = get_parameter("server_port").as_int(); + if (enable_groot_monitoring) { + if (server_port <= 0) { + RCLCPP_WARN(get_logger(), "Groot2 monitoring port not provided, disabling it"); + } else { + RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", server_port); + addGrootMonitoring(&tree_, server_port); + } + } + finished_ = false; return ActionExecutorClient::on_activate(previous_state); } @@ -136,12 +152,12 @@ BTAction::on_deactivate(const rclcpp_lifecycle::State & previous_state) bt_minitrace_logger_.reset(); bt_file_logger_.reset(); tree_.haltTree(); + resetGrootMonitor(); return ActionExecutorClient::on_deactivate(previous_state); } -void -BTAction::do_work() +void BTAction::do_work() { if (!finished_) { BT::NodeStatus result; @@ -173,4 +189,21 @@ BTAction::do_work() } } +void BTAction::addGrootMonitoring(BT::Tree * tree, uint16_t server_port) +{ + // This logger publish status changes using Groot2 + groot_monitor_ = std::make_unique(*tree, server_port); + + // Register common types JSON definitions + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition(); +} + +void BTAction::resetGrootMonitor() +{ + if (groot_monitor_) { + groot_monitor_.reset(); + } +} + } // namespace plansys2 diff --git a/plansys2_executor/include/plansys2_executor/ComputeBT.hpp b/plansys2_executor/include/plansys2_executor/ComputeBT.hpp index 35ba2ba11..8a2e61f46 100644 --- a/plansys2_executor/include/plansys2_executor/ComputeBT.hpp +++ b/plansys2_executor/include/plansys2_executor/ComputeBT.hpp @@ -18,6 +18,8 @@ #include #include +#include "behaviortree_cpp/loggers/groot2_publisher.h" + #include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/BTBuilder.hpp" @@ -82,6 +84,21 @@ class ComputeBT : public rclcpp_lifecycle::LifecycleNode void savePlan(const plansys2_msgs::msg::Plan & plan, const std::string & filename) const; void saveBT(const std::string & bt_xml, const std::string & filename) const; void saveDotGraph(const std::string & dotgraph, const std::string & filename) const; + + /** + * @brief Add Groot2 monitor to publish BT status changes + * @param tree BT to monitor + * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port) + */ + void addGrootMonitoring(BT::Tree * tree, uint16_t server_port); + + /** + * @brief Reset Groot2 monitor + */ + void resetGrootMonitor(); + + // Groot2 monitor + std::unique_ptr groot_monitor_; }; } // namespace plansys2 diff --git a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp index 9551c459a..489d63f9d 100644 --- a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp +++ b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp @@ -33,6 +33,7 @@ #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/blackboard.h" +#include "behaviortree_cpp/loggers/groot2_publisher.h" #include "plansys2_msgs/action/execute_plan.hpp" #include "plansys2_msgs/msg/action_execution_info.hpp" @@ -175,6 +176,21 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode int executor_state_; PlanRuntineInfo runtime_info_; + + /** + * @brief Add Groot2 monitor to publish BT status changes + * @param tree BT to monitor + * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port) + */ + void addGrootMonitoring(BT::Tree * tree, uint16_t server_port); + + /** + * @brief Reset Groot2 monitor + */ + void resetGrootMonitor(); + + // Groot2 monitor + std::unique_ptr groot_monitor_; }; } // namespace plansys2 diff --git a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp index cd0697971..92345744e 100644 --- a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp +++ b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp @@ -69,6 +69,9 @@ ComputeBT::ComputeBT() 0.0); } + this->declare_parameter("enable_groot_monitoring", false); + this->declare_parameter("server_port", 1800); + compute_bt_srv_ = create_service( "compute_bt", std::bind( @@ -354,6 +357,13 @@ ComputeBT::computeBTCallback( auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool(); + int server_port = get_parameter("server_port").as_int(); + if (enable_groot_monitoring) { + RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", get_name(), server_port); + addGrootMonitoring(&tree, server_port); + } + finish = true; t.join(); @@ -412,4 +422,17 @@ ComputeBT::saveDotGraph(const std::string & dotgraph, const std::string & filena } } +void ComputeBT::addGrootMonitoring(BT::Tree * tree, uint16_t server_port) +{ + // This logger publish status changes using Groot2 + groot_monitor_ = std::make_unique(*tree, server_port); +} + +void ComputeBT::resetGrootMonitor() +{ + if (groot_monitor_) { + groot_monitor_.reset(); + } +} + } // namespace plansys2 diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index ba1e3d5ed..6fc3998eb 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -80,6 +80,9 @@ ExecutorNode::ExecutorNode() 0.0); } + this->declare_parameter("enable_groot_monitoring", false); + this->declare_parameter("server_port", 1800); + execute_plan_action_server_ = rclcpp_action::create_server( this->get_node_base_interface(), this->get_node_clock_interface(), @@ -463,6 +466,13 @@ ExecutorNode::get_tree_from_plan(PlanRuntineInfo & runtime_info) *runtime_info.current_tree = { factory.createTreeFromText(bt_xml_tree, blackboard), blackboard, bt_builder}; + bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool(); + int server_port = get_parameter("server_port").as_int(); + if (enable_groot_monitoring) { + RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", get_name(), server_port); + addGrootMonitoring(&runtime_info.current_tree->tree, server_port); + } + return runtime_info.current_tree != nullptr; } @@ -857,4 +867,17 @@ ExecutorNode::execution_cycle() } } +void ExecutorNode::addGrootMonitoring(BT::Tree * tree, uint16_t server_port) +{ + // This logger publish status changes using Groot2 + groot_monitor_ = std::make_unique(*tree, server_port); +} + +void ExecutorNode::resetGrootMonitor() +{ + if (groot_monitor_) { + groot_monitor_.reset(); + } +} + } // namespace plansys2