WIP: added an ActionServerBT to execute trees

Author: facontidavide

behaviortree_ros2/CMakeLists.txt

@@ -3,32 +3,57 @@ project(behaviortree_ros2)
 
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 set(THIS_PACKAGE_DEPS
     rclcpp
     rclcpp_action
     ament_index_cpp
-    behaviortree_cpp)
+    behaviortree_cpp
+    btcpp_ros2_interfaces
+    generate_parameter_library
+    )
 
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED )
-find_package(rclcpp_action REQUIRED )
-find_package(behaviortree_cpp REQUIRED )
-find_package(ament_index_cpp REQUIRED)
-
-# This is compiled only to check if there are errors in the header file
-# library will not be exported
-include_directories(include)
-add_library(${PROJECT_NAME} src/bt_ros2.cpp)
+
+foreach(PACKAGE ${THIS_PACKAGE_DEPS})
+    find_package(${PACKAGE} REQUIRED )
+endforeach()
+
+
+generate_parameter_library(
+    bt_executor_parameters
+    src/bt_executor_parameters.yaml)
+
+add_library(${PROJECT_NAME}
+    src/bt_ros2.cpp
+    src/bt_utils.cpp
+    src/tree_execution_server.cpp )
+
 ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS})
 
+target_include_directories(${PROJECT_NAME} PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>)
+
+target_link_libraries(${PROJECT_NAME}  bt_executor_parameters)
+
+
 ######################################################
 # INSTALL
 
 install(DIRECTORY include/  DESTINATION include/)
 
-ament_export_include_directories(include)
+ament_export_targets(${PROJECT_NAME}Targets  HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_DEPS})
 
+install(
+  TARGETS ${PROJECT_NAME} bt_executor_parameters
+  EXPORT ${PROJECT_NAME}Targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
 ament_package()

behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp

@@ -0,0 +1,82 @@
+// Copyright 2024 Marq Rasmussen
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
+// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
+// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright
+// notice and this permission notice shall be included in all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+#include <functional>
+#include <memory>
+#include <thread>
+
+// auto-generated header, created by generate_parameter_library
+#include "bt_executor_parameters.hpp"
+
+#include "btcpp_ros2_interfaces/msg/node_status.hpp"
+
+#include "behaviortree_cpp/bt_factory.h"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+namespace action_server_bt
+{
+/**
+ * @brief Convert BT::NodeStatus into Action Server feedback message NodeStatus
+ * @param status Current status of the executing BehaviorTree
+ * @return NodeStatus used to publish feedback to the Action Client
+ */
+btcpp_ros2_interfaces::msg::NodeStatus convert_node_status(BT::NodeStatus& status);
+
+/**
+ * @brief Function the uses ament_index_cpp to get the package path of the parameter specified by the user
+ * @param parameter_value String containing 'package_name/subfolder' for the directory path to look up
+ * @return Full path to the directory specified by the parameter_value
+ */
+std::string get_directory_path(const std::string& parameter_value);
+
+/**
+ * @brief Function to load BehaviorTree xml files from a specific directory
+ * @param factory BehaviorTreeFactory to register the BehaviorTrees into
+ * @param directory_path Full path to the directory to search for BehaviorTree definitions
+ */
+void load_behavior_trees(BT::BehaviorTreeFactory& factory,
+                         const std::string& directory_path);
+
+/**
+ * @brief Function to load BehaviorTree plugins from a specific directory
+ * @param factory BehaviorTreeFactory to register the plugins into
+ * @param directory_path Full path to the directory to search for BehaviorTree plugins
+ */
+void load_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path);
+
+/**
+ * @brief Function to load BehaviorTree ROS plugins from a specific directory
+ * @param factory BehaviorTreeFactory to register the plugins into
+ * @param directory_path Full path to the directory to search for BehaviorTree plugins
+ * @param node node pointer that is shared with the ROS based BehaviorTree plugins
+ */
+void load_ros_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
+                      rclcpp::Node::SharedPtr node);
+
+/**
+ * @brief Function to register all Behaviors and BehaviorTrees from user specified packages
+ * @param params ROS parameters that contain lists of packages to load
+ * plugins, ros_plugins and BehaviorTrees from
+ * @param factory BehaviorTreeFactory to register into
+ * @param node node pointer that is shared with the ROS based Behavior plugins
+ */
+void register_behavior_trees(action_server_bt::Params& params,
+                             BT::BehaviorTreeFactory& factory,
+                             rclcpp::Node::SharedPtr node);
+
+}  // namespace action_server_bt

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

@@ -0,0 +1,132 @@
+// Copyright 2024 Marq Rasmussen
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
+// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
+// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright
+// notice and this permission notice shall be included in all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+#include <memory>
+#include <optional>
+
+#include "btcpp_ros2_interfaces/action/execute_tree.hpp"
+
+#include "behaviortree_cpp/bt_factory.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+namespace action_server_bt
+{
+
+/**
+ * @brief ActionServerBT class hosts a ROS Action Server that is able
+ * to load Behavior plugins, BehaviorTree.xml files and execute them.
+ */
+class ActionServerBT
+{
+public:
+  using ExecuteTree = btcpp_ros2_interfaces::action::ExecuteTree;
+  using GoalHandleExecuteTree = rclcpp_action::ServerGoalHandle<ExecuteTree>;
+
+  /**
+   * @brief Constructor for ActionServerBT.
+   * @details This initializes a ParameterListener to read configurable options from the user and
+   * starts an Action Server that takes requests to execute BehaviorTrees.
+   *
+   * @param options rclcpp::NodeOptions to pass to node_ when initializing it.
+   * after the tree is created, while its running and after it finishes.
+   */
+  explicit ActionServerBT(const rclcpp::NodeOptions& options);
+
+  virtual ~ActionServerBT();
+
+  /**
+   * @brief Gets the NodeBaseInterface of node_.
+   * @details This function exists to allow running ActionServerBT as a component in a composable node container.
+   *
+   * @return A shared_ptr to the NodeBaseInterface of node_.
+   */
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nodeBaseInterface();
+
+  // name of the tree being executed
+  const std::string& currentTreeName() const;
+
+  // tree being executed, nullptr if it doesn't exist yet.
+  BT::Tree* currentTree();
+
+  // pointer to the global blackboard
+  BT::Blackboard::Ptr globalBlackboard();
+
+protected:
+  // To be overridden by the user.
+  // Callback invoked when the tree is created and before it is executed,
+  // Can be used to update the blackboard or to attach loggers.
+  virtual void onTreeCreated(BT::Tree& tree)
+  {}
+
+  // To be overridden by the user.
+  // In addition to the built in mechanism to register nodes from plugins,
+  // you can use this method to register custom nodes into the factory.
+  virtual void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory)
+  {}
+
+  // To be overridden by the user.
+  // Callback invoked after the tickOnce.
+  // If it returns something different than std::nullopt, the tree execution will
+  // be halted and the returned value will be the optional NodeStatus.
+  virtual std::optional<BT::NodeStatus> onLoopAfterTick(BT::NodeStatus status)
+  {
+    return std::nullopt;
+  }
+
+  // To be overridden by the user.
+  // Callback invoked when the tree execution is completed
+  virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled)
+  {}
+
+  virtual std::optional<std::string> onLoopFeedback()
+  {
+    return std::nullopt;
+  }
+
+private:
+  struct Pimpl;
+  std::unique_ptr<Pimpl> p_;
+
+  /**
+   * @brief handle the goal requested: accept or reject. This implementation always accepts.
+   * @param uuid Goal ID
+   * @param goal A shared pointer to the specific goal
+   * @return GoalResponse response of the goal processed
+   */
+  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid,
+                                          std::shared_ptr<const ExecuteTree::Goal> goal);
+
+  /**
+   * @brief Accepts cancellation requests of action server.
+   * @param goal A server goal handle to cancel
+   * @return CancelResponse response of the goal cancelled
+   */
+  rclcpp_action::CancelResponse
+  handle_cancel(const std::shared_ptr<GoalHandleExecuteTree> goal_handle);
+
+  /**
+   * @brief Handles accepted goals and starts a thread to process them
+   * @param goal_handle Server goal handle to process feedback and set the response when finished
+   */
+  void handle_accepted(const std::shared_ptr<GoalHandleExecuteTree> goal_handle);
+
+  /**
+   * @brief Background processes to execute the BehaviorTree and handle stop requests
+   * @param goal_handle Server goal handle to process feedback and set the response when finished
+   */
+  void execute(const std::shared_ptr<GoalHandleExecuteTree> goal_handle);
+};
+
+}  // namespace action_server_bt

behaviortree_ros2/package.xml

@@ -11,10 +11,14 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>fmt</depend>
   <depend>libboost-dev</depend>
+
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>behaviortree_cpp</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>btcpp_ros2_interfaces</depend>
 
 <export>
   <build_type>ament_cmake</build_type>

behaviortree_ros2/src/bt_executor_parameters.yaml

@@ -0,0 +1,49 @@
+action_server_bt:
+  action_name: {
+    type: string,
+    default_value: "action_server_bt",
+    read_only: true,
+    description: "The name the Action Server takes requests from",
+  }
+  behavior_tick_frequency: {
+    type: int,
+    default_value: 100,
+    read_only: true,
+    description: "Frequency in Hz to tick() the Behavior tree at",
+    validation: {
+      bounds<>: [1, 1000]
+    }
+  }
+  groot2_port: {
+    type: int,
+    default_value: 1667,
+    read_only: true,
+    description: "Server port value to publish Groot2 messages on",
+    validation: {
+      bounds<>: [1, 49151]
+    }
+  }
+  plugins: {
+    type: string_array,
+    default_value: [],
+    description: "List of 'package_name/subfolder' containing BehaviorTree plugins to load into the factory",
+    validation: {
+      unique<>: null,
+    }
+  }
+  ros_plugins: {
+    type: string_array,
+    default_value: [],
+    description: "List of 'package_name/subfolder' containing BehaviorTree ROS plugins to load into the factory",
+    validation: {
+      unique<>: null,
+    }
+  }
+  behavior_trees: {
+    type: string_array,
+    default_value: [],
+    description: "List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTree factory",
+    validation: {
+      unique<>: null,
+    }
+  }