diff --git a/auto_apms_behavior_tree/CMakeLists.txt b/auto_apms_behavior_tree/CMakeLists.txt index df14c2de..07c3f8b7 100644 --- a/auto_apms_behavior_tree/CMakeLists.txt +++ b/auto_apms_behavior_tree/CMakeLists.txt @@ -90,8 +90,10 @@ set(CMAKE_CURRENT_SOURCE_DIR ${_temp}) add_library(${PROJECT_NAME} SHARED "src/build_handler/build_handler.cpp" "src/build_handler/build_handler_loader.cpp" + "src/executor/options.cpp" "src/executor/state_observer.cpp" "src/executor/executor_base.cpp" + "src/executor/generic_executor_node.cpp" "src/executor/executor_node.cpp" "src/util/parameter.cpp" "src/util/node.cpp" diff --git a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/action_based_executor_node.hpp b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/action_based_executor_node.hpp new file mode 100644 index 00000000..48785d48 --- /dev/null +++ b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/action_based_executor_node.hpp @@ -0,0 +1,276 @@ +// Copyright 2026 Robin Müller +// +// 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. + +#pragma once + +#include + +#include "auto_apms_behavior_tree/executor/generic_executor_node.hpp" +#include "auto_apms_util/action_context.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace auto_apms_behavior_tree +{ + +/** + * @ingroup auto_apms_behavior_tree + * @brief ROS 2 node template for executing behavior trees triggered by a custom ROS 2 action goal. + * + * This class extends GenericTreeExecutorNode to trigger behavior tree execution when an action goal of the + * specified type @p ActionT is received. It creates an action server and manages the lifecycle of the action goal + * in coordination with the behavior tree execution. + * + * Derived classes must implement: + * - `getTreeConstructorFromGoal()`: Create a TreeConstructor from the received action goal. + * + * Derived classes may optionally override: + * - `shouldAcceptGoal()`: Custom goal acceptance logic. + * - `onAcceptedGoal()`: Hook called after a goal is accepted. + * - `onExecutionStarted()`: Hook called after execution has started successfully. + * - `onGoalExecutionTermination()`: Handle the result of the execution for the action client. + * + * @tparam ActionT The ROS 2 action type that triggers the behavior tree execution. + */ +template +class ActionBasedTreeExecutorNode : public GenericTreeExecutorNode +{ +public: + using TriggerActionContext = auto_apms_util::ActionContext; + using TriggerGoal = typename ActionT::Goal; + using TriggerFeedback = typename ActionT::Feedback; + using TriggerResult = typename ActionT::Result; + using TriggerGoalHandle = rclcpp_action::ServerGoalHandle; + + /** + * @brief Constructor. + * @param name Name of the `rclcpp::Node`. + * @param action_name Name for the trigger action server. + * @param options Executor options. + */ + ActionBasedTreeExecutorNode(const std::string & name, const std::string & action_name, Options options); + + /** + * @brief Constructor with default options. + * @param name Name of the `rclcpp::Node`. + * @param action_name Name for the trigger action server. + * @param ros_options ROS 2 node options. + */ + ActionBasedTreeExecutorNode( + const std::string & name, const std::string & action_name, rclcpp::NodeOptions ros_options = rclcpp::NodeOptions()); + + virtual ~ActionBasedTreeExecutorNode() override = default; + +protected: + /* Virtual methods to implement/override */ + + /** + * @brief Create a TreeConstructor from the received action goal. + * + * Derived classes must implement this to define how the action goal maps to a behavior tree execution. + * This is where the build request, entry point, and node manifest should be determined from the goal. + * @param goal_ptr Shared pointer to the action goal. + * @return Callback for creating the behavior tree. + * @throw std::exception if the goal cannot be processed (will cause goal rejection). + */ + virtual TreeConstructor getTreeConstructorFromGoal(std::shared_ptr goal_ptr) = 0; + + /** + * @brief Determine whether an incoming trigger action goal should be accepted. + * + * The default implementation rejects the goal if the executor is currently busy. + * @param uuid The unique identifier of the incoming goal. + * @param goal_ptr Shared pointer to the incoming goal. + * @return `true` if the goal should be accepted, `false` if it should be rejected. + */ + virtual bool shouldAcceptGoal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr); + + /** + * @brief Hook called after a trigger action goal has been accepted and before execution begins. + * @param goal_handle_ptr Shared pointer to the accepted goal handle. + */ + virtual void onAcceptedGoal(std::shared_ptr goal_handle_ptr); + + /** + * @brief Hook called after execution has been started successfully. + * + * The default implementation sets up the action context for tracking the execution result (attached mode). + * Derived classes may override this to implement detached behavior by succeeding the goal handle immediately. + * @param goal_handle_ptr Shared pointer to the accepted goal handle. + */ + virtual void onExecutionStarted(std::shared_ptr goal_handle_ptr); + + /** + * @brief Handle the execution result for the action client. + * + * The default implementation succeeds for TREE_SUCCEEDED, aborts for TREE_FAILED, handles cancellation + * for TERMINATED_PREMATURELY, and aborts for ERROR. + * @param result The execution result. + * @param context The action context for sending the result back. + */ + virtual void onGoalExecutionTermination(const ExecutionResult & result, TriggerActionContext & context); + + TriggerActionContext trigger_action_context_; + std::map pending_tree_constructors_; + +private: + void onTermination(const ExecutionResult & result) override final; + + /* Internal action callbacks */ + + rclcpp_action::GoalResponse handle_trigger_goal_( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr); + rclcpp_action::CancelResponse handle_trigger_cancel_(std::shared_ptr goal_handle_ptr); + void handle_trigger_accept_(std::shared_ptr goal_handle_ptr); + + typename rclcpp_action::Server::SharedPtr trigger_action_ptr_; +}; + +// ##################################################################################################################### +// ################################ DEFINITIONS ############################################## +// ##################################################################################################################### + +template +ActionBasedTreeExecutorNode::ActionBasedTreeExecutorNode( + const std::string & name, const std::string & action_name, Options options) +: GenericTreeExecutorNode(name, options), trigger_action_context_(logger_) +{ + using namespace std::placeholders; + trigger_action_ptr_ = rclcpp_action::create_server( + node_ptr_, action_name, std::bind(&ActionBasedTreeExecutorNode::handle_trigger_goal_, this, _1, _2), + std::bind(&ActionBasedTreeExecutorNode::handle_trigger_cancel_, this, _1), + std::bind(&ActionBasedTreeExecutorNode::handle_trigger_accept_, this, _1)); +} + +template +ActionBasedTreeExecutorNode::ActionBasedTreeExecutorNode( + const std::string & name, const std::string & action_name, rclcpp::NodeOptions ros_options) +: ActionBasedTreeExecutorNode(name, action_name, Options(ros_options)) +{ +} + +template +bool ActionBasedTreeExecutorNode::shouldAcceptGoal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr /*goal_ptr*/) +{ + if (isBusy()) { + RCLCPP_WARN( + logger_, "Goal %s was REJECTED: Tree '%s' is currently executing.", rclcpp_action::to_string(uuid).c_str(), + getTreeName().c_str()); + return false; + } + return true; +} + +template +void ActionBasedTreeExecutorNode::onAcceptedGoal(std::shared_ptr /*goal_handle_ptr*/) +{ +} + +template +void ActionBasedTreeExecutorNode::onExecutionStarted(std::shared_ptr goal_handle_ptr) +{ + trigger_action_context_.setUp(goal_handle_ptr); + RCLCPP_INFO(logger_, "Successfully started execution of tree '%s' via action trigger.", getTreeName().c_str()); +} + +template +void ActionBasedTreeExecutorNode::onGoalExecutionTermination( + const ExecutionResult & result, TriggerActionContext & context) +{ + switch (result) { + case ExecutionResult::TREE_SUCCEEDED: + context.succeed(); + break; + case ExecutionResult::TREE_FAILED: + context.abort(); + break; + case ExecutionResult::TERMINATED_PREMATURELY: + if (context.getGoalHandlePtr()->is_canceling()) { + context.cancel(); + } else { + context.abort(); + } + break; + case ExecutionResult::ERROR: + default: + context.abort(); + break; + } +} + +template +void ActionBasedTreeExecutorNode::onTermination(const ExecutionResult & result) +{ + if (trigger_action_context_.isValid()) { + onGoalExecutionTermination(result, trigger_action_context_); + trigger_action_context_.invalidate(); + } +} + +template +rclcpp_action::GoalResponse ActionBasedTreeExecutorNode::handle_trigger_goal_( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr) +{ + if (!shouldAcceptGoal(uuid, goal_ptr)) { + return rclcpp_action::GoalResponse::REJECT; + } + + try { + pending_tree_constructors_[uuid] = getTreeConstructorFromGoal(goal_ptr); + } catch (const std::exception & e) { + RCLCPP_WARN( + logger_, "Goal %s was REJECTED: Exception in getTreeConstructorFromGoal(): %s", + rclcpp_action::to_string(uuid).c_str(), e.what()); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +template +rclcpp_action::CancelResponse ActionBasedTreeExecutorNode::handle_trigger_cancel_( + std::shared_ptr /*goal_handle_ptr*/) +{ + setControlCommand(ControlCommand::TERMINATE); + return rclcpp_action::CancelResponse::ACCEPT; +} + +template +void ActionBasedTreeExecutorNode::handle_trigger_accept_(std::shared_ptr goal_handle_ptr) +{ + onAcceptedGoal(goal_handle_ptr); + + const rclcpp_action::GoalUUID uuid = goal_handle_ptr->get_goal_id(); + auto node = pending_tree_constructors_.extract(uuid); + if (node.empty()) { + RCLCPP_ERROR(logger_, "No pending tree constructor found for goal %s.", rclcpp_action::to_string(uuid).c_str()); + auto result_ptr = std::make_shared(); + goal_handle_ptr->abort(result_ptr); + return; + } + TreeConstructor tree_constructor = std::move(node.mapped()); + + const ExecutorParameters params = executor_param_listener_.get_params(); + try { + startExecution(tree_constructor, params.tick_rate, params.groot2_port); + } catch (const std::exception & e) { + auto result_ptr = std::make_shared(); + goal_handle_ptr->abort(result_ptr); + RCLCPP_ERROR(logger_, "An error occurred trying to start execution: %s", e.what()); + return; + } + + onExecutionStarted(goal_handle_ptr); +} + +} // namespace auto_apms_behavior_tree \ No newline at end of file diff --git a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/executor_node.hpp b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/executor_node.hpp index 171b283c..769822c1 100644 --- a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/executor_node.hpp +++ b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/executor_node.hpp @@ -14,119 +14,30 @@ #pragma once -#include "auto_apms_behavior_tree/build_handler/build_handler_loader.hpp" -#include "auto_apms_behavior_tree/executor/executor_base.hpp" -#include "auto_apms_behavior_tree/executor_params.hpp" -#include "auto_apms_behavior_tree_core/builder.hpp" -#include "auto_apms_behavior_tree_core/node/node_registration_loader.hpp" -#include "auto_apms_interfaces/action/command_tree_executor.hpp" +#include "auto_apms_behavior_tree/executor/action_based_executor_node.hpp" #include "auto_apms_interfaces/action/start_tree_executor.hpp" -#include "auto_apms_util/action_context.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_srvs/srv/trigger.hpp" namespace auto_apms_behavior_tree { -/** - * @brief Configuration options for TreeExecutorNode. - * - * This allows to hardcode certain configurations. During initialization, a TreeExecutorNode parses the provided options - * and activates/deactivates the corresponding features. - */ -class TreeExecutorNodeOptions -{ -public: - /** - * @brief Constructor. - * - * Executor options must be created by passing an existing `rclcpp::NodeOptions` object. - * @param ros_node_options ROS 2 node options. - */ - TreeExecutorNodeOptions(const rclcpp::NodeOptions & ros_node_options); - - /** - * @brief Configure whether the executor node accepts scripting enum parameters. - * @param from_overrides `true` allows to set scripting enums from parameter overrides, `false` forbids that. - * @param dynamic `true` allows to dynamically set scripting enums at runtime, `false` forbids that. - * @return Modified options object. - */ - TreeExecutorNodeOptions & enableScriptingEnumParameters(bool from_overrides, bool dynamic); - - /** - * @brief Configure whether the executor node accepts global blackboard parameters. - * @param from_overrides `true` allows to set global blackboard entries from parameter overrides, `false` forbids - * that. - * @param dynamic `true` allows to dynamically set global blackboard entries at runtime, `false` forbids that. - * @return Modified options object. - */ - TreeExecutorNodeOptions & enableGlobalBlackboardParameters(bool from_overrides, bool dynamic); - - /** - * @brief Specify a default behavior tree build handler that will be used initially. - * @param name Fully qualified class name of the behavior tree build handler plugin. - * @return Modified options object. - */ - TreeExecutorNodeOptions & setDefaultBuildHandler(const std::string & name); - - /** - * @brief Set a custom name for the start tree executor action server. - * - * If not set, the default name `/start` is used. - * @param name Full action name. - * @return Modified options object. - */ - TreeExecutorNodeOptions & setStartActionName(const std::string & name); - - /** - * @brief Set a custom name for the command tree executor action server. - * - * If not set, the default name `/cmd` is used. - * @param name Full action name. - * @return Modified options object. - */ - TreeExecutorNodeOptions & setCommandActionName(const std::string & name); - - /** - * @brief Set a custom name for the clear blackboard service. - * - * If not set, the default name `/clear_blackboard` is used. - * @param name Full service name. - * @return Modified options object. - */ - TreeExecutorNodeOptions & setClearBlackboardServiceName(const std::string & name); - - /** - * @brief Get the ROS 2 node options that comply with the given options. - * @return Corresponding `rclcpp::NodeOptions` object. - */ - rclcpp::NodeOptions getROSNodeOptions() const; - -private: - friend class TreeExecutorNode; - - rclcpp::NodeOptions ros_node_options_; - bool scripting_enum_parameters_from_overrides_ = true; - bool scripting_enum_parameters_dynamic_ = true; - bool blackboard_parameters_from_overrides_ = true; - bool blackboard_parameters_dynamic_ = true; - std::map custom_default_parameters_; - std::string start_action_name_; - std::string command_action_name_; - std::string clear_blackboard_service_name_; -}; - /** * @ingroup auto_apms_behavior_tree * @brief Flexible ROS 2 node implementing a standardized interface for dynamically executing behavior trees. * - * The executor is configured using ROS 2 parameters. Parameters are typically set using parameter overrides when - * launching a node or during runtime using the `ros2 param set` command line tool. + * This class uses the ActionBasedTreeExecutorNode template with the builtin StartTreeExecutor action type that allows + * external clients to trigger behavior tree execution via a flexible and standardized interface. The executor is + * configured using ROS 2 parameters. * * A behavior tree can be executed via command line: * * ```sh - * ros2 run auto_apms_behavior_tree run_behavior + * ros2 run auto_apms_behavior_tree run_behavior ... + * ``` + * + * or using the ROS 2 CLI integration offered by `auto_apms_ros2behavior`: + * + * ```sh + * ros2 behavior run ... * ``` * * Alternatively, an executor can also be included as part of a ROS 2 components container. The following executor @@ -144,193 +55,50 @@ class TreeExecutorNodeOptions * * - `auto_apms_behavior_tree::OnlyInitialBlackboardParamsExecutorNode` */ -class TreeExecutorNode : public TreeExecutorBase +class TreeExecutorNode : public ActionBasedTreeExecutorNode { public: - using Options = TreeExecutorNodeOptions; - using ExecutorParameters = executor_params::Params; - using ExecutorParameterListener = executor_params::ParamListener; - using StartActionContext = auto_apms_util::ActionContext; - using CommandActionContext = auto_apms_util::ActionContext; - - inline static const std::string PARAM_VALUE_NO_BUILD_HANDLER = "none"; - inline static const std::string SCRIPTING_ENUM_PARAM_PREFIX = "enum"; - inline static const std::string BLACKBOARD_PARAM_PREFIX = "bb"; + /** + * @brief Constructor allowing to specify a custom node name and executor options. + * @param name Default name of the `rclcpp::Node`. + * @param start_action_name Name for the StartTreeExecutor action server. If empty, defaults to `/start`. + * @param options Executor specific options. Simply pass a `rclcpp::NodeOptions` object to use the default + * options. + */ + TreeExecutorNode(const std::string & name, const std::string & start_action_name, Options options); /** * @brief Constructor allowing to specify a custom node name and executor options. * @param name Default name of the `rclcpp::Node`. - * @param executor_options Executor specific options. Simply pass a `rclcpp::NodeOptions` object to use the default + * @param options Executor specific options. Simply pass a `rclcpp::NodeOptions` object to use the default * options. */ - TreeExecutorNode(const std::string & name, TreeExecutorNodeOptions executor_options); + TreeExecutorNode(const std::string & name, Options options); /** * @brief Constructor populating both the node's name and the executor options with the default. * @param options Options forwarded to rclcpp::Node constructor. */ - explicit TreeExecutorNode(rclcpp::NodeOptions options); + explicit TreeExecutorNode(rclcpp::NodeOptions ros_options); virtual ~TreeExecutorNode() override = default; - using TreeExecutorBase::startExecution; - - /** - * @brief Start the behavior tree that is specified by a particular build request. - * - * Executing the behavior tree is achieved by regularly invoking the internal routine that ticks the behavior tree - * created using @p make_tree. This requires to register a timer with the associated ROS 2 node. Consequently, the - * behavior tree is executed asynchronously. The user is provided a shared future object that allows to check whether - * the execution finished. Once this future completes, the execution result can be evaluated. - * @param build_request Behavior build request for creating the behavior. - * @param entry_point Single point of entry for behavior execution. - * @param node_manifest Behavior tree node manifest to be loaded for behavior execution. - * @return Shared future that completes once executing the tree is finished or an error occurs. In that case, it is - * assigned an execution result code. - */ - std::shared_future startExecution( - const std::string & build_request, const std::string & entry_point = "", - const core::NodeManifest & node_manifest = {}); - -private: - /* Virtual methods */ - - /** - * @brief Callback invoked before building the behavior tree using the loaded build handler. - * - * This is invoked first thing inside the tree constructor callback returned by TreeExecutorNode::makeTreeConstructor - * just after the tree builder object has been instantiated. Therefore, the builder object is "empty" when passed to - * this method. - * - * There are two common use cases where this method is useful: - * - * - The user wants to define an executor-specific initial configuration of the builder object, before it is - * passed to the currently configured build handler. - * - * - The user wants to bypass the build handler concept and directly create the behavior tree using this method. - * @param builder Tree builder to be configured. This is used for creating the behavior tree later. - * @param build_request Behavior build request for creating the behavior. - * @param entry_point Single point of entry for behavior execution. - * @param node_manifest Behavior tree node manifest to be loaded for behavior execution. - * @param bb Local blackboard of the tree that is being created. This blackboard is derived from the global blackboard - * and is passed when instantiating the tree. Changes to this blackboard are not reflected on the global blackboard. - */ - virtual void preBuild( - core::TreeBuilder & builder, const std::string & build_request, const std::string & entry_point, - const core::NodeManifest & node_manifest, TreeBlackboard & bb); - - /** - * @brief Callback invoked after the behavior tree has been instantiated. - * - * This is invoked last thing inside the tree constructor callback returned by - * TreeExecutorNode::makeTreeConstructor just after the tree has been instantiated but before the constructor callback - * returns. Therefore, the tree is not yet being executed when this method is invoked. - * - * @param tree Behavior tree that has been created and is about to be executed. - */ - virtual void postBuild(Tree & tree); - protected: - /* Utility methods */ - - /** - * @brief Get a copy of the current executor parameters. - * @return Current executor parameters. - */ - ExecutorParameters getExecutorParameters() const; + /* ActionBasedTreeExecutorNode overrides */ /** - * @brief Assemble all parameters of this node that have a specific prefix. + * @brief Create a TreeConstructor from a StartTreeExecutor action goal. * - * This function searches with depth = 2, so given that `prefix = "foo"` it will only consider parameters with names - * that match `foo.bar` but not `foo.bar1.bar2` and deeper patterns. The user must specify @p prefix accordingly. - * @param prefix Only consider parameters that have this prefix in their names. - * @return Map of parameter names and their respective values. + * Loads the build handler (if specified), parses the node manifest, and creates a tree constructor using the + * build request from the goal. + * @param goal_ptr Shared pointer to the StartTreeExecutor action goal. + * @return Callback for creating the behavior tree. + * @throw std::exception if the goal cannot be processed. */ - std::map getParameterValuesWithPrefix(const std::string & prefix); + TreeConstructor getTreeConstructorFromGoal(std::shared_ptr goal_ptr) override; /** - * @brief Get the name of a parameter without its prefix. - * @param prefix Prefix to remove from @p param_name. - * @param param_name Name of the parameter with its prefix. - * @return Name of the parameter without its prefix. If @p param_name doesn't contain @p prefix, an empty string is - * returned. - */ - static std::string stripPrefixFromParameterName(const std::string & prefix, const std::string & param_name); - - /** - * @brief Update the internal buffer of scripting enums used when a behavior tree is created. - * - * This function automatically deduces the underlying type of each parameter value in @p value_map. Scripting enums - * are stored as integers, so if a parameter value is not `int` or `bool` (`true == 1` and `false == 0`) an error is - * raised. If @p simulate is `true`, the buffer won't be modified and instead of raising an error this function simply - * returns `false`. - * @param value_map Map of parameter names and their respective values. - * @param simulate Set this to `true` to only validate that the underlying types of the provided parameter values - * comply with the above mentioned requirements. - * @return `true` if updating the scripting enums using @p value_map is possible, `false` otherwise. Only if @p - * simulate is `false`, they are actually updated. - * @throw auto_apms_behavior_tree::exceptions::ParameterConversionError if a parameter value cannot be converted to a - * valid scripting enum. - */ - bool updateScriptingEnumsWithParameterValues( - const std::map & value_map, bool simulate = false); - - /** - * @brief Update the global blackboard using parameter values. - * - * This function automatically deduces the underlying type of each parameter value in @p value_map and sets the global - * blackboard entries determined by the map's keys accordingly. If a blackboard entry already exists, it is only - * allowed to modify it with a value that has the same type. If the types mismatch, an error is raised. If @p simulate - * is `true`, the blackboard is not modified and instead of raising an error this function simply returns `false`. - * @param value_map Map of parameter names and their respective values. - * @param simulate Set this to `true` to only validate that the underlying types of the provided parameter values - * comply with the above mentioned requirements. - * @return `true` if updating the global blackboard using @p value_map is possible, `false` otherwise. Only if @p - * simulate is `false`, it is actually updated. - * @throw auto_apms_behavior_tree::exceptions::ParameterConversionError if a parameter value cannot be converted to a - * valid blackboard entry. - * @throw BT::LogicError if the user tries to change the type of an existing blackboard entry. - */ - bool updateGlobalBlackboardWithParameterValues( - const std::map & value_map, bool simulate = false); - - /** - * @brief Load a particular behavior tree build handler plugin. - * - * This enables this executor to dynamically change the build handler which accepts incoming build requests. - * @param name Fully qualified name of the respective behavior tree build handler class. Set to `none` to simply - * unload the current build handler. - */ - void loadBuildHandler(const std::string & name); - - /** - * @brief Create a callback that builds a behavior tree according to a specific request. - * - * The created callback makes all defined scripting enums available for the behavior tree and invokes the currently - * configured build handler to build it. It returns a corresponding instance of `BT::Tree` that may be ticked to - * execute the tree. - * @param build_request Request that specifies how to build the behavior tree encoded in a string. - * @param entry_point Single point of entry for behavior execution. - * @param node_manifest Behavior tree node manifest that specifies which nodes to use and how to load them. - * @return Callback for creating the behavior tree according to the build request. - * @throw auto_apms_behavior_tree::exceptions::TreeBuildError if the build handler rejects the request. - */ - TreeConstructor makeTreeConstructor( - const std::string & build_request, const std::string & entry_point = "", - const core::NodeManifest & node_manifest = {}); - - /** - * @brief Reset the global blackboard and clear all entries. This also unsets the corresponding parameters. - * @return `true` if blackboard was cleared, `false` if executor is not idle meaning that the blackboard cannot be - * cleared. - */ - virtual bool clearGlobalBlackboard() override; - - /* Virtual methods */ - - /** - * @brief Hook called to determine whether an incoming start action goal should be accepted. + * @brief Determine whether an incoming start action goal should be accepted. * * The default implementation rejects the goal if the executor is currently busy executing a behavior tree. Derived * classes may override this to add additional validation logic. @@ -338,63 +106,35 @@ class TreeExecutorNode : public TreeExecutorBase * @param goal_ptr Shared pointer to the incoming goal. * @return `true` if the goal should be accepted, `false` if it should be rejected. */ - virtual bool shouldAcceptStartGoal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr); + bool shouldAcceptGoal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr) override; /** * @brief Hook called after a start action goal has been accepted and before execution begins. * - * The default implementation does nothing. Derived classes may override this to perform setup actions - * when a start goal is accepted. + * Clears the global blackboard if the goal's `clear_blackboard` flag is set. * @param goal_handle_ptr Shared pointer to the accepted goal handle. */ - virtual void onAcceptedStartGoal(std::shared_ptr goal_handle_ptr); - - bool onTick() override; - - bool afterTick() override; + void onAcceptedGoal(std::shared_ptr goal_handle_ptr) override; - void onTermination(const ExecutionResult & result) override; - -private: - /* Internal callbacks */ - - rcl_interfaces::msg::SetParametersResult on_set_parameters_callback_( - const std::vector & parameters); - - void parameter_event_callback_(const rcl_interfaces::msg::ParameterEvent & event); - - rclcpp_action::GoalResponse handle_start_goal_( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr); - rclcpp_action::CancelResponse handle_start_cancel_(std::shared_ptr goal_handle_ptr); - void handle_start_accept_(std::shared_ptr goal_handle_ptr); - - rclcpp_action::GoalResponse handle_command_goal_( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr); - rclcpp_action::CancelResponse handle_command_cancel_( - std::shared_ptr goal_handle_ptr); - void handle_command_accept_(std::shared_ptr goal_handle_ptr); + /** + * @brief Hook called after execution has been started successfully. + * + * Handles attached vs detached mode: in attached mode, sets up the action context to track execution; + * in detached mode, immediately succeeds the goal. + * @param goal_handle_ptr Shared pointer to the accepted goal handle. + */ + void onExecutionStarted(std::shared_ptr goal_handle_ptr) override; - const TreeExecutorNodeOptions executor_options_; - ExecutorParameterListener executor_param_listener_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_ptr_; - std::shared_ptr parameter_event_handler_ptr_; - rclcpp::ParameterEventCallbackHandle::SharedPtr parameter_event_callback_handle_ptr_; - core::NodeRegistrationLoader::SharedPtr tree_node_loader_ptr_; - TreeBuildHandlerLoader::UniquePtr build_handler_loader_ptr_; - core::TreeBuilder::UniquePtr builder_ptr_; - TreeBuildHandler::UniquePtr build_handler_ptr_; - std::string current_build_handler_name_; - std::map scripting_enums_; - std::map translated_global_blackboard_entries_; - TreeConstructor tree_constructor_; + /** + * @brief Handle the execution result for the StartTreeExecutor action client. + * + * Populates the result with tree status information and the terminated tree identity. + * @param result The execution result. + * @param context The action context for sending the result back. + */ + void onGoalExecutionTermination(const ExecutionResult & result, TriggerActionContext & context) override; - // Interface objects - rclcpp_action::Server::SharedPtr start_action_ptr_; - StartActionContext start_action_context_; - rclcpp_action::Server::SharedPtr command_action_ptr_; - rclcpp::TimerBase::SharedPtr command_timer_ptr_; - rclcpp::Service::SharedPtr clear_blackboard_service_ptr_; + bool afterTick() override; }; } // namespace auto_apms_behavior_tree \ No newline at end of file diff --git a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/generic_executor_node.hpp b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/generic_executor_node.hpp new file mode 100644 index 00000000..72e1001c --- /dev/null +++ b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/generic_executor_node.hpp @@ -0,0 +1,246 @@ +// Copyright 2026 Robin Müller +// +// 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. + +#pragma once + +#include "auto_apms_behavior_tree/build_handler/build_handler_loader.hpp" +#include "auto_apms_behavior_tree/executor/executor_base.hpp" +#include "auto_apms_behavior_tree/executor/options.hpp" +#include "auto_apms_behavior_tree/executor_params.hpp" +#include "auto_apms_behavior_tree_core/builder.hpp" +#include "auto_apms_behavior_tree_core/node/node_registration_loader.hpp" +#include "auto_apms_interfaces/action/command_tree_executor.hpp" +#include "auto_apms_util/action_context.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace auto_apms_behavior_tree +{ + +static const std::vector TREE_EXECUTOR_EXPLICITLY_ALLOWED_PARAMETERS{ + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_ALLOW_OTHER_BUILD_HANDLERS, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_ALLOW_DYNAMIC_BLACKBOARD, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_ALLOW_DYNAMIC_SCRIPTING_ENUMS, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_EXCLUDE_PACKAGES_NODE, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_EXCLUDE_PACKAGES_BUILD_HANDLER, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_BUILD_HANDLER, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_TICK_RATE, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_GROOT2_PORT, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_STATE_CHANGE_LOGGER}; + +static const std::vector TREE_EXECUTOR_EXPLICITLY_ALLOWED_PARAMETERS_WHILE_BUSY{ + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_ALLOW_DYNAMIC_BLACKBOARD, + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_STATE_CHANGE_LOGGER}; + +/** + * @ingroup auto_apms_behavior_tree + * @brief Flexible and configurable ROS 2 behavior tree executor node. + * + * This executor extends TreeExecutorBase with configurable support for: + * - Build handler management (loading and switching build handlers) + * - CommandTreeExecutor action interface (pause, resume, halt, terminate) + * - Parameter/blackboard synchronization + * - Scripting enum parameters + * + * Derived classes can trigger the behavior tree execution by calling GenericTreeExecutorNode::startExecution with a + * build request or a TreeConstructor directly. + */ +class GenericTreeExecutorNode : public TreeExecutorBase +{ +public: + using Options = TreeExecutorNodeOptions; + using ExecutorParameters = executor_params::Params; + using ExecutorParameterListener = executor_params::ParamListener; + using CommandActionContext = auto_apms_util::ActionContext; + + inline static const std::string SCRIPTING_ENUM_PARAM_PREFIX = "enum"; + inline static const std::string BLACKBOARD_PARAM_PREFIX = "bb"; + + /// Value indicating that no build handler is loaded. + inline static const std::string PARAM_VALUE_NO_BUILD_HANDLER = "none"; + + /** + * @brief Constructor. + * @param name Name of the `rclcpp::Node`. + * @param options Executor options. + */ + GenericTreeExecutorNode(const std::string & name, Options options); + + /** + * @brief Constructor with default options. + * @param options ROS 2 node options. + */ + explicit GenericTreeExecutorNode(rclcpp::NodeOptions options); + + virtual ~GenericTreeExecutorNode() override = default; + + using TreeExecutorBase::startExecution; + + /** + * @brief Start the behavior tree specified by a particular build request. + * @param build_request Behavior build request for creating the behavior. + * @param entry_point Single point of entry for behavior execution. + * @param node_manifest Behavior tree node manifest to be loaded for behavior execution. + * @return Shared future that completes once executing the tree is finished or an error occurs. + */ + std::shared_future startExecution( + const std::string & build_request, const std::string & entry_point = "", + const core::NodeManifest & node_manifest = {}); + +private: + /* Virtual methods to be overridden by derived classes */ + + /** + * @brief Callback invoked before building the behavior tree. + * + * @note This hook is only used in the TreeConstructor returned by GenericTreeExecutorNode::makeTreeConstructor. + * Remember that if you pass a custom TreeConstructor directly to TreeExecutorBase::startExecution, you bypass this + * hook if you don't explicitly include it. + * @param builder Tree builder to be configured. + * @param build_request Behavior build request. + * @param entry_point Single point of entry for behavior execution. + * @param node_manifest Behavior tree node manifest. + * @param bb Local blackboard of the tree being created. + */ + virtual void preBuild( + core::TreeBuilder & builder, const std::string & build_request, const std::string & entry_point, + const core::NodeManifest & node_manifest, TreeBlackboard & bb); + + /** + * @brief Callback invoked after the behavior tree has been instantiated. + * + * @note This hook is only used in the TreeConstructor returned by GenericTreeExecutorNode::makeTreeConstructor. + * Remember that if you pass a custom TreeConstructor directly to TreeExecutorBase::startExecution, you bypass this + * hook if you don't explicitly include it. + * @param tree Behavior tree that has been created and is about to be executed. + */ + virtual void postBuild(Tree & tree); + +protected: + /* Utility methods */ + + /** + * @brief Get a copy of the current executor parameters. + * @return Current executor parameters. + */ + ExecutorParameters getExecutorParameters() const; + + /** + * @brief Assemble all parameters of this node that have a specific prefix. + * @param prefix Only consider parameters that have this prefix in their names. + * @return Map of parameter names and their respective values. + */ + std::map getParameterValuesWithPrefix(const std::string & prefix); + + /** + * @brief Get the name of a parameter without its prefix. + * @param prefix Prefix to remove from @p param_name. + * @param param_name Name of the parameter with its prefix. + * @return Name of the parameter without its prefix. + */ + static std::string stripPrefixFromParameterName(const std::string & prefix, const std::string & param_name); + + /** + * @brief Update the internal buffer of scripting enums. + * @param value_map Map of parameter names and their respective values. + * @param simulate Set to `true` to only validate. + * @return `true` if updating is possible. + */ + bool updateScriptingEnumsWithParameterValues( + const std::map & value_map, bool simulate = false); + + /** + * @brief Update the global blackboard using parameter values. + * @param value_map Map of parameter names and their respective values. + * @param simulate Set to `true` to only validate. + * @return `true` if updating is possible. + */ + bool updateGlobalBlackboardWithParameterValues( + const std::map & value_map, bool simulate = false); + + /** + * @brief Load a particular behavior tree build handler plugin. + * @param name Fully qualified name of the build handler class. Set to `"none"` to unload. + */ + void loadBuildHandler(const std::string & name); + + /** + * @brief Create a callback that builds a behavior tree according to a specific request. + * + * + * @param build_request Request that specifies how to build the behavior tree. + * @param entry_point Single point of entry for behavior execution. + * @param node_manifest Behavior tree node manifest. + * @return Callback for creating the behavior tree. + */ + TreeConstructor makeTreeConstructor( + const std::string & build_request, const std::string & entry_point = "", + const core::NodeManifest & node_manifest = {}); + + /** + * @brief Create a tree builder for building the behavior tree. + * @return Shared pointer to the created tree builder. + */ + core::TreeBuilder::SharedPtr createTreeBuilder(); + + /** + * @brief Reset the global blackboard and clear all entries. + * @return `true` if blackboard was cleared, `false` if executor is not idle. + */ + virtual bool clearGlobalBlackboard() override; + + bool onTick() override; + + bool afterTick() override; + +private: + /* Internal callbacks */ + + rcl_interfaces::msg::SetParametersResult on_set_parameters_callback_( + const std::vector & parameters); + + void parameter_event_callback_(const rcl_interfaces::msg::ParameterEvent & event); + + rclcpp_action::GoalResponse handle_command_goal_( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr); + rclcpp_action::CancelResponse handle_command_cancel_( + std::shared_ptr goal_handle_ptr); + void handle_command_accept_(std::shared_ptr goal_handle_ptr); + +protected: + const Options executor_options_; + ExecutorParameterListener executor_param_listener_; + + core::NodeRegistrationLoader::SharedPtr tree_node_loader_ptr_; + TreeBuildHandlerLoader::UniquePtr build_handler_loader_ptr_; + core::TreeBuilder::UniquePtr builder_ptr_; + TreeBuildHandler::UniquePtr build_handler_ptr_; + std::string current_build_handler_name_; + std::map scripting_enums_; + std::map translated_global_blackboard_entries_; + +private: + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_ptr_; + std::shared_ptr parameter_event_handler_ptr_; + rclcpp::ParameterEventCallbackHandle::SharedPtr parameter_event_callback_handle_ptr_; + + // Command action interface (optional) + rclcpp_action::Server::SharedPtr command_action_ptr_; + rclcpp::TimerBase::SharedPtr command_timer_ptr_; + + // Clear blackboard service (optional) + rclcpp::Service::SharedPtr clear_blackboard_service_ptr_; +}; + +} // namespace auto_apms_behavior_tree \ No newline at end of file diff --git a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/options.hpp b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/options.hpp new file mode 100644 index 00000000..661d29b1 --- /dev/null +++ b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/executor/options.hpp @@ -0,0 +1,116 @@ +// Copyright 2026 Robin Müller +// +// 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. + +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace auto_apms_behavior_tree +{ + +/** + * @brief Configuration options for GenericTreeExecutorNode. + * + * This allows configuring which features of the behavior tree executor are enabled, such as the command action + * interface, parameter/blackboard synchronization, and scripting enum support. + */ +class TreeExecutorNodeOptions +{ +public: + /** + * @brief Constructor. + * @param ros_node_options ROS 2 node options. + */ + TreeExecutorNodeOptions(const rclcpp::NodeOptions & ros_node_options = rclcpp::NodeOptions()); + + /** + * @brief Enable or disable the CommandTreeExecutor action interface. + * @param enable `true` to enable, `false` to disable. + * @return Modified options object. + */ + TreeExecutorNodeOptions & enableCommandAction(bool enable); + + /** + * @brief Enable or disable the clear blackboard service. + * @param enable `true` to enable, `false` to disable. + * @return Modified options object. + */ + TreeExecutorNodeOptions & enableClearBlackboardService(bool enable); + + /** + * @brief Configure whether the executor accepts scripting enum parameters. + * @param from_overrides `true` allows to set scripting enums from parameter overrides. + * @param dynamic `true` allows to dynamically set scripting enums at runtime. + * @return Modified options object. + */ + TreeExecutorNodeOptions & enableScriptingEnumParameters(bool from_overrides, bool dynamic); + + /** + * @brief Configure whether the executor accepts global blackboard parameters. + * @param from_overrides `true` allows to set global blackboard entries from parameter overrides. + * @param dynamic `true` allows to dynamically set global blackboard entries at runtime. + * @return Modified options object. + */ + TreeExecutorNodeOptions & enableGlobalBlackboardParameters(bool from_overrides, bool dynamic); + + /** + * @brief Specify a default behavior tree build handler. + * @param name Fully qualified class name of the behavior tree build handler plugin. + * @return Modified options object. + */ + TreeExecutorNodeOptions & setDefaultBuildHandler(const std::string & name); + + /** + * @brief Set a custom name for the command tree executor action server. + * + * If not set, the default name `/cmd` is used. + * @param name Full action name. + * @return Modified options object. + */ + TreeExecutorNodeOptions & setCommandActionName(const std::string & name); + + /** + * @brief Set a custom name for the clear blackboard service. + * + * If not set, the default name `/clear_blackboard` is used. + * @param name Full service name. + * @return Modified options object. + */ + TreeExecutorNodeOptions & setClearBlackboardServiceName(const std::string & name); + + /** + * @brief Get the ROS 2 node options that comply with the given options. + * @return Corresponding `rclcpp::NodeOptions` object. + */ + rclcpp::NodeOptions getROSNodeOptions() const; + +private: + friend class GenericTreeExecutorNode; + + rclcpp::NodeOptions ros_node_options_; + bool enable_command_action_ = true; + bool enable_clear_blackboard_service_ = true; + bool scripting_enum_parameters_from_overrides_ = true; + bool scripting_enum_parameters_dynamic_ = true; + bool blackboard_parameters_from_overrides_ = true; + bool blackboard_parameters_dynamic_ = true; + std::map custom_default_parameters_; + std::string command_action_name_; + std::string clear_blackboard_service_name_; +}; + +} // namespace auto_apms_behavior_tree \ No newline at end of file diff --git a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/util/node.hpp b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/util/node.hpp index fd8573d7..5de41844 100644 --- a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/util/node.hpp +++ b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/util/node.hpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#pragma once + #include "auto_apms_behavior_tree/behavior_tree_nodes.hpp" #include "auto_apms_behavior_tree_core/tree/tree_document.hpp" #include "auto_apms_behavior_tree_core/tree/tree_resource.hpp" diff --git a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/util/parameter.hpp b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/util/parameter.hpp index e647037e..0d5051d5 100644 --- a/auto_apms_behavior_tree/include/auto_apms_behavior_tree/util/parameter.hpp +++ b/auto_apms_behavior_tree/include/auto_apms_behavior_tree/util/parameter.hpp @@ -37,7 +37,7 @@ BT::Expected createAnyFromParameterValue(const rclcpp::ParameterValue & * You may optionally specify the type of the parameter value to convert to using @p type. If you want to deduce the * type from @p any, set this to `rclcpp::PARAMETER_NOT_SET`. * @param any `BT::Any` object. - * @param type Desired type of the parameter value. Set to `rclcpp::PARAMETER_NOT_SET` if it is unkown (e.g. if the + * @param type Desired type of the parameter value. Set to `rclcpp::PARAMETER_NOT_SET` if it is unknown (e.g. if the * parameter hasn't been set before). * @return Expected object for `rclcpp::ParameterValue`. Holds an error message if @p any couldn't be converted to @p * type. diff --git a/auto_apms_behavior_tree/src/executor/executor_node.cpp b/auto_apms_behavior_tree/src/executor/executor_node.cpp index b9c9447d..5eb46630 100644 --- a/auto_apms_behavior_tree/src/executor/executor_node.cpp +++ b/auto_apms_behavior_tree/src/executor/executor_node.cpp @@ -14,241 +14,23 @@ #include "auto_apms_behavior_tree/executor/executor_node.hpp" -#include #include -#include #include "auto_apms_behavior_tree/exceptions.hpp" -#include "auto_apms_behavior_tree/util/parameter.hpp" #include "auto_apms_behavior_tree_core/definitions.hpp" #include "auto_apms_util/container.hpp" #include "auto_apms_util/string.hpp" -#include "pluginlib/exceptions.hpp" #include "rclcpp/utilities.hpp" namespace auto_apms_behavior_tree { -const std::vector EXPLICITLY_ALLOWED_PARAMETERS{ - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_ALLOW_OTHER_BUILD_HANDLERS, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_ALLOW_DYNAMIC_BLACKBOARD, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_ALLOW_DYNAMIC_SCRIPTING_ENUMS, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_EXCLUDE_PACKAGES_NODE, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_EXCLUDE_PACKAGES_BUILD_HANDLER, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_BUILD_HANDLER, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_TICK_RATE, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_GROOT2_PORT, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_STATE_CHANGE_LOGGER}; - -const std::vector EXPLICITLY_ALLOWED_PARAMETERS_WHILE_BUSY{ - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_ALLOW_DYNAMIC_BLACKBOARD, - _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_STATE_CHANGE_LOGGER}; - -TreeExecutorNodeOptions::TreeExecutorNodeOptions(const rclcpp::NodeOptions & ros_node_options) -: ros_node_options_(ros_node_options) -{ -} - -TreeExecutorNodeOptions & TreeExecutorNodeOptions::enableScriptingEnumParameters(bool from_overrides, bool dynamic) -{ - scripting_enum_parameters_from_overrides_ = from_overrides; - scripting_enum_parameters_dynamic_ = dynamic; - return *this; -} - -TreeExecutorNodeOptions & TreeExecutorNodeOptions::enableGlobalBlackboardParameters(bool from_overrides, bool dynamic) +TreeExecutorNode::TreeExecutorNode(const std::string & name, const std::string & start_action_name, Options options) +: ActionBasedTreeExecutorNode( + name, + start_action_name.empty() ? name + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_START_ACTION_NAME_SUFFIX : start_action_name, + options) { - blackboard_parameters_from_overrides_ = from_overrides; - blackboard_parameters_dynamic_ = dynamic; - return *this; -} - -TreeExecutorNodeOptions & TreeExecutorNodeOptions::setDefaultBuildHandler(const std::string & name) -{ - custom_default_parameters_[_AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_BUILD_HANDLER] = rclcpp::ParameterValue(name); - return *this; -} - -TreeExecutorNodeOptions & TreeExecutorNodeOptions::setStartActionName(const std::string & name) -{ - start_action_name_ = name; - return *this; -} - -TreeExecutorNodeOptions & TreeExecutorNodeOptions::setCommandActionName(const std::string & name) -{ - command_action_name_ = name; - return *this; -} - -TreeExecutorNodeOptions & TreeExecutorNodeOptions::setClearBlackboardServiceName(const std::string & name) -{ - clear_blackboard_service_name_ = name; - return *this; -} - -rclcpp::NodeOptions TreeExecutorNodeOptions::getROSNodeOptions() const -{ - rclcpp::NodeOptions opt(ros_node_options_); - opt.automatically_declare_parameters_from_overrides( - scripting_enum_parameters_from_overrides_ || blackboard_parameters_from_overrides_); - opt.allow_undeclared_parameters(scripting_enum_parameters_dynamic_ || blackboard_parameters_dynamic_); - - // Default configuration - opt.enable_logger_service(true); - - return opt; -} - -TreeExecutorNode::TreeExecutorNode(const std::string & name, TreeExecutorNodeOptions executor_options) -: TreeExecutorBase(std::make_shared(name, executor_options.getROSNodeOptions())), - executor_options_(executor_options), - executor_param_listener_(node_ptr_), - start_action_context_(logger_) -{ - // Set custom parameter default values. - // NOTE: We cannot do this before the node is created, because we also need the global parameter overrides here, not - // just rclcpp::NodeOptions::parameter_overrides (This only contains parameter overrides explicitly provided to the - // options object. So generally speaking, this variable doesn't represent all parameter overrides). - std::vector new_default_parameters; - std::map effective_param_overrides = - node_ptr_->get_node_parameters_interface()->get_parameter_overrides(); - for (const auto & [name, value] : executor_options_.custom_default_parameters_) { - // Only set custom default parameters if not present in overrides - if (effective_param_overrides.find(name) == effective_param_overrides.end()) { - new_default_parameters.push_back(rclcpp::Parameter(name, value)); - } - } - if (!new_default_parameters.empty()) node_ptr_->set_parameters_atomically(new_default_parameters); - - const ExecutorParameters initial_params = executor_param_listener_.get_params(); - - // Remove all parameters from overrides that are not supported - rcl_interfaces::msg::ListParametersResult res = node_ptr_->list_parameters({}, 0); - std::vector unkown_param_names; - for (const std::string & param_name : res.names) { - if (!stripPrefixFromParameterName(SCRIPTING_ENUM_PARAM_PREFIX, param_name).empty()) continue; - if (!stripPrefixFromParameterName(BLACKBOARD_PARAM_PREFIX, param_name).empty()) continue; - if (auto_apms_util::contains(EXPLICITLY_ALLOWED_PARAMETERS, param_name)) continue; - try { - node_ptr_->undeclare_parameter(param_name); - } catch (const rclcpp::exceptions::ParameterImmutableException & e) { - // Allow all builtin read only parameters - continue; - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - // Allow all builtin statically typed parameters - continue; - } - unkown_param_names.push_back(param_name); - } - if (!unkown_param_names.empty()) { - RCLCPP_WARN( - logger_, "The following initial parameters are not supported and have been removed: [ %s ].", - auto_apms_util::join(unkown_param_names, ", ").c_str()); - } - - // Create behavior tree node loader - tree_node_loader_ptr_ = core::NodeRegistrationLoader::make_shared( - std::set(initial_params.node_exclude_packages.begin(), initial_params.node_exclude_packages.end())); - - // Create behavior tree build handler loader - build_handler_loader_ptr_ = TreeBuildHandlerLoader::make_unique( - std::set( - initial_params.build_handler_exclude_packages.begin(), initial_params.build_handler_exclude_packages.end())); - - // Instantiate behavior tree build handler - if ( - initial_params.build_handler != PARAM_VALUE_NO_BUILD_HANDLER && - !build_handler_loader_ptr_->isClassAvailable(initial_params.build_handler)) { - throw exceptions::TreeExecutorError( - "Cannot load build handler '" + initial_params.build_handler + - "' because no corresponding ament_index resource was found. Make sure that you spelled the build handler's " - "name correctly " - "and registered it by calling auto_apms_behavior_tree_register_build_handlers() in the CMakeLists.txt of the " - "corresponding package."); - } - loadBuildHandler(initial_params.build_handler); - - // Collect scripting enum and blackboard parameters from initial parameters - const auto initial_scripting_enums = getParameterValuesWithPrefix(SCRIPTING_ENUM_PARAM_PREFIX); - if (!initial_scripting_enums.empty()) { - if (executor_options_.scripting_enum_parameters_from_overrides_) { - updateScriptingEnumsWithParameterValues(initial_scripting_enums); - } else { - RCLCPP_WARN( - logger_, - "Initial scripting enums have been provided, but the 'Scripting enums from overrides' option is disabled. " - "Ignoring."); - } - } - const auto initial_blackboard = getParameterValuesWithPrefix(BLACKBOARD_PARAM_PREFIX); - if (!initial_blackboard.empty()) { - if (executor_options_.blackboard_parameters_from_overrides_) { - updateGlobalBlackboardWithParameterValues(initial_blackboard); - } else { - RCLCPP_WARN( - logger_, - "Initial blackboard entries have been provided, but the 'Blackboard from overrides' option is disabled. " - "Ignoring."); - } - } - - using namespace std::placeholders; - - // Determine action/service names: use custom names from options or fall back to defaults - const std::string start_action_name = - executor_options_.start_action_name_.empty() - ? std::string(node_ptr_->get_name()) + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_START_ACTION_NAME_SUFFIX - : executor_options_.start_action_name_; - const std::string command_action_name = - executor_options_.command_action_name_.empty() - ? std::string(node_ptr_->get_name()) + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_COMMAND_ACTION_NAME_SUFFIX - : executor_options_.command_action_name_; - const std::string clear_bb_service_name = - executor_options_.clear_blackboard_service_name_.empty() - ? std::string(node_ptr_->get_name()) + _AUTO_APMS_BEHAVIOR_TREE__CLEAR_BLACKBOARD_SERVICE_NAME_SUFFIX - : executor_options_.clear_blackboard_service_name_; - - start_action_ptr_ = rclcpp_action::create_server( - node_ptr_, start_action_name, std::bind(&TreeExecutorNode::handle_start_goal_, this, _1, _2), - std::bind(&TreeExecutorNode::handle_start_cancel_, this, _1), - std::bind(&TreeExecutorNode::handle_start_accept_, this, _1)); - - command_action_ptr_ = rclcpp_action::create_server( - node_ptr_, command_action_name, std::bind(&TreeExecutorNode::handle_command_goal_, this, _1, _2), - std::bind(&TreeExecutorNode::handle_command_cancel_, this, _1), - std::bind(&TreeExecutorNode::handle_command_accept_, this, _1)); - - clear_blackboard_service_ptr_ = node_ptr_->create_service( - clear_bb_service_name, [this]( - const std::shared_ptr /*request*/, - std::shared_ptr response) { - response->success = this->clearGlobalBlackboard(); - if (response->success) { - response->message = "Blackboard was cleared successfully"; - } else { - response->message = "Blackboard cannot be cleared, because executor is in state " + - toStr(this->getExecutionState()) + " but must be idling"; - } - RCLCPP_DEBUG_STREAM(this->logger_, response->message); - }); - - // Adding the local on_set_parameters_callback after the parameter listeners from generate_parameters_library - // are created makes sure that this callback will be evaluated before the listener callbacks. - // This is desired to keep the internal parameter struct in sync, because the callbacks of the listeners implicitly - // set them if the change is accepted. Otherwise, they would be set even if the local callback rejects the change. - // We DO NOT set any variables in this callback, but only check if the request to change certain parameters is - // valid. The actual change is performed in the callback registered with rclcpp::ParameterEventListener - on_set_parameters_callback_handle_ptr_ = - node_ptr_->add_on_set_parameters_callback([this](const std::vector & parameters) { - return this->on_set_parameters_callback_(parameters); - }); - - // Add an event handler that applies the actual parameter change - parameter_event_handler_ptr_ = std::make_shared(node_ptr_); - parameter_event_callback_handle_ptr_ = parameter_event_handler_ptr_->add_parameter_event_callback( - [this](const rcl_interfaces::msg::ParameterEvent & event) { this->parameter_event_callback_(event); }); - // Make sure ROS arguments are removed. When using rclcpp_components, this is typically not the case. std::vector args_with_ros_arguments = node_ptr_->get_node_options().arguments(); int argc = args_with_ros_arguments.size(); @@ -267,338 +49,21 @@ TreeExecutorNode::TreeExecutorNode(const std::string & name, TreeExecutorNodeOpt logger_, "Additional cli arguments in rclcpp::NodeOptions: [ %s ]", auto_apms_util::join(relevant_args, ", ").c_str()); + const ExecutorParameters initial_params = executor_param_listener_.get_params(); // Start tree execution with the build handler request being the first relevant argument startExecution(makeTreeConstructor(relevant_args[0]), initial_params.tick_rate, initial_params.groot2_port); } } -TreeExecutorNode::TreeExecutorNode(rclcpp::NodeOptions options) -: TreeExecutorNode(_AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_DEFAULT_NAME, TreeExecutorNodeOptions(options)) -{ -} +TreeExecutorNode::TreeExecutorNode(const std::string & name, Options options) : TreeExecutorNode(name, "", options) {} -void TreeExecutorNode::preBuild( - core::TreeBuilder & /*builder*/, const std::string & /*build_request*/, const std::string & /*entry_point*/, - const core::NodeManifest & /*node_manifest*/, TreeBlackboard & /*bb*/) +TreeExecutorNode::TreeExecutorNode(rclcpp::NodeOptions ros_options) +: TreeExecutorNode(_AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_DEFAULT_NAME, Options(ros_options)) { } -void TreeExecutorNode::postBuild(Tree & /*tree*/) {} - -std::shared_future TreeExecutorNode::startExecution( - const std::string & build_request, const std::string & entry_point, const core::NodeManifest & node_manifest) -{ - const ExecutorParameters params = executor_param_listener_.get_params(); - return startExecution( - makeTreeConstructor(build_request, entry_point, node_manifest), params.tick_rate, params.groot2_port); -} - -TreeExecutorNode::ExecutorParameters TreeExecutorNode::getExecutorParameters() const -{ - return executor_param_listener_.get_params(); -} - -std::map TreeExecutorNode::getParameterValuesWithPrefix(const std::string & prefix) -{ - const auto res = node_ptr_->list_parameters({prefix}, 2); - std::map value_map; - for (const std::string & name_with_prefix : res.names) { - if (const std::string suffix = stripPrefixFromParameterName(prefix, name_with_prefix); !suffix.empty()) { - value_map[suffix] = node_ptr_->get_parameter(name_with_prefix).get_parameter_value(); - } - } - return value_map; -} - -std::string TreeExecutorNode::stripPrefixFromParameterName(const std::string & prefix, const std::string & param_name) -{ - const std::regex reg("^" + prefix + "\\.(\\S+)"); - if (std::smatch match; std::regex_match(param_name, match, reg)) return match[1].str(); - return ""; -} - -bool TreeExecutorNode::updateScriptingEnumsWithParameterValues( - const std::map & value_map, bool simulate) -{ - std::map set_successfully_map; - for (const auto & [enum_key, pval] : value_map) { - try { - switch (pval.get_type()) { - case rclcpp::ParameterType::PARAMETER_BOOL: - if (simulate) continue; - scripting_enums_[enum_key] = static_cast(pval.get()); - break; - case rclcpp::ParameterType::PARAMETER_INTEGER: - if (simulate) continue; - scripting_enums_[enum_key] = static_cast(pval.get()); - break; - default: - if (simulate) return false; - throw exceptions::ParameterConversionError("Parameter to scripting enum conversion is not allowed."); - } - set_successfully_map[enum_key] = rclcpp::to_string(pval); - } catch (const std::exception & e) { - RCLCPP_ERROR( - logger_, "Error setting scripting enum from parameter %s=%s (Type: %s): %s", enum_key.c_str(), - rclcpp::to_string(pval).c_str(), rclcpp::to_string(pval.get_type()).c_str(), e.what()); - return false; - } - } - if (!set_successfully_map.empty()) { - RCLCPP_DEBUG( - logger_, "Updated scripting enums from parameters: { %s }", - auto_apms_util::printMap(set_successfully_map).c_str()); - } - return true; -} - -bool TreeExecutorNode::updateGlobalBlackboardWithParameterValues( - const std::map & value_map, bool simulate) -{ - TreeBlackboard & bb = *getGlobalBlackboardPtr(); - std::map set_successfully_map; - for (const auto & [entry_key, pval] : value_map) { - try { - if (const BT::Expected expected = createAnyFromParameterValue(pval)) { - BT::Any any(expected.value()); - if (simulate) { - // Verify that any has the same type as the entry (if it exists) - if (const BT::TypeInfo * entry_info = bb.entryInfo(entry_key)) { - if (entry_info->isStronglyTyped() && entry_info->type() != any.type()) return false; - } - continue; - } else { - bb.set(entry_key, any); - } - } else { - throw exceptions::ParameterConversionError(expected.error()); - } - translated_global_blackboard_entries_[entry_key] = pval; - set_successfully_map[entry_key] = rclcpp::to_string(pval); - } catch (const std::exception & e) { - RCLCPP_ERROR( - logger_, "Error updating blackboard from parameter %s=%s (Type: %s): %s", entry_key.c_str(), - rclcpp::to_string(pval).c_str(), rclcpp::to_string(pval.get_type()).c_str(), e.what()); - return false; - } - } - if (!set_successfully_map.empty()) { - RCLCPP_DEBUG( - logger_, "Updated blackboard from parameters: { %s }", auto_apms_util::printMap(set_successfully_map).c_str()); - } - return true; -} - -void TreeExecutorNode::loadBuildHandler(const std::string & name) -{ - if (build_handler_ptr_ && !executor_param_listener_.get_params().allow_other_build_handlers) { - throw std::logic_error( - "Executor option 'Allow other build handlers' is disabled, but loadBuildHandler() was called again after " - "instantiating '" + - current_build_handler_name_ + "'."); - } - if (current_build_handler_name_ == name) return; - if (name == PARAM_VALUE_NO_BUILD_HANDLER) { - build_handler_ptr_.reset(); - } else { - try { - build_handler_ptr_ = - build_handler_loader_ptr_->createUniqueInstance(name)->makeUnique(node_ptr_, tree_node_loader_ptr_); - } catch (const pluginlib::CreateClassException & e) { - throw exceptions::TreeExecutorError( - "An error occurred when trying to create an instance of tree build handler class '" + name + - "'. This might be because you forgot to call the AUTO_APMS_BEHAVIOR_TREE_REGISTER_BUILD_HANDLER macro " - "in the source file: " + - e.what()); - } catch (const std::exception & e) { - throw exceptions::TreeExecutorError( - "An error occurred when trying to create an instance of tree build handler class '" + name + "': " + e.what()); - } - } - current_build_handler_name_ = name; -} - -TreeConstructor TreeExecutorNode::makeTreeConstructor( - const std::string & build_request, const std::string & entry_point, const core::NodeManifest & node_manifest) -{ - // Request the tree identity - if (build_handler_ptr_ && !build_handler_ptr_->setBuildRequest(build_request, entry_point, node_manifest)) { - throw exceptions::TreeBuildError( - "Build request '" + build_request + "' was denied by '" + executor_param_listener_.get_params().build_handler + - "' (setBuildRequest() returned false)."); - } - - // By passing the the local variables to the callback's captures by value they live on and can be used for creating - // the tree later. Otherwise a segmentation fault might occur since memory allocated for the arguments might be - // released at the time the method returns. - return [this, build_request, entry_point, node_manifest](TreeBlackboardSharedPtr bb_ptr) { - // Currently, BehaviorTree.CPP requires the memory allocated by the factory to persist even after the tree has - // been created, so we make the builder a unique pointer that is only reset when a new tree is to be created. See - // https://github.com/BehaviorTree/BehaviorTree.CPP/issues/890 - builder_ptr_.reset(new core::TreeBuilder( - node_ptr_, getTreeNodeWaitablesCallbackGroupPtr(), getTreeNodeWaitablesExecutorPtr(), tree_node_loader_ptr_)); - - // Allow executor to make modifications prior to building the tree - preBuild(*builder_ptr_, build_request, entry_point, node_manifest, *bb_ptr); - - // Make scripting enums available to tree instance - for (const auto & [enum_key, val] : scripting_enums_) builder_ptr_->setScriptingEnum(enum_key, val); - - // If a build handler is specified, let it configure the builder and determine which tree is to be instantiated - std::string instantiate_name = ""; - if (build_handler_ptr_) { - instantiate_name = build_handler_ptr_->buildTree(*builder_ptr_, *bb_ptr).getName(); - } - - // Finally, instantiate the tree - Tree tree = instantiate_name.empty() ? builder_ptr_->instantiate(bb_ptr) - : builder_ptr_->instantiate(instantiate_name, bb_ptr); - - // Allow executor to make modifications after building the tree, but before execution starts - postBuild(tree); - return tree; - }; -} - -bool TreeExecutorNode::clearGlobalBlackboard() -{ - if (TreeExecutorBase::clearGlobalBlackboard()) { - const auto res = node_ptr_->list_parameters({BLACKBOARD_PARAM_PREFIX}, 2); - for (const std::string & name : res.names) { - node_ptr_->undeclare_parameter(name); - }; - return true; - } - return false; -} - -rcl_interfaces::msg::SetParametersResult TreeExecutorNode::on_set_parameters_callback_( - const std::vector & parameters) -{ - const ExecutorParameters params = executor_param_listener_.get_params(); - - // Iterate through parameters and individually decide wether to reject the change - for (const rclcpp::Parameter & p : parameters) { - auto create_rejected = [&p](const std::string msg) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = false; - result.reason = "Rejected to set " + p.get_name() + " = " + p.value_to_string() + " (Type: " + p.get_type_name() + - "): " + msg + "."; - return result; - }; - const std::string param_name = p.get_name(); - - // Check if parameter is a scripting enum - if (const std::string enum_key = stripPrefixFromParameterName(SCRIPTING_ENUM_PARAM_PREFIX, param_name); - !enum_key.empty()) { - if (isBusy()) { - return create_rejected("Scripting enums cannot change while tree executor is running"); - } - if (!executor_options_.scripting_enum_parameters_dynamic_ || !params.allow_dynamic_scripting_enums) { - return create_rejected( - "Cannot set scripting enum '" + enum_key + "', because the 'Dynamic scripting enums' option is disabled"); - } - // Validate type of scripting enum parameters - if (!updateScriptingEnumsWithParameterValues({{enum_key, p.get_parameter_value()}}, true)) { - return create_rejected( - "Type of scripting enum must be bool or int. Tried to set enum '" + enum_key + "' with value '" + - p.value_to_string() + "' (Type: " + p.get_type_name() + ")"); - }; - // If scripting enum is allowed to change, continue with next parameter. - continue; - } - - // Check if parameter is a blackboard parameter - if (const std::string entry_key = stripPrefixFromParameterName(BLACKBOARD_PARAM_PREFIX, param_name); - !entry_key.empty()) { - if (!executor_options_.blackboard_parameters_dynamic_ || !params.allow_dynamic_blackboard) { - return create_rejected( - "Cannot set blackboard entry '" + entry_key + "', because the 'Dynamic blackboard' option is disabled"); - } - // Validate type of blackboard parameters won't change - if (!updateGlobalBlackboardWithParameterValues({{entry_key, p.get_parameter_value()}}, true)) { - return create_rejected( - "Type of blackboard entries must not change. Tried to set entry '" + entry_key + - "' (Type: " + getGlobalBlackboardPtr()->getEntry(entry_key)->info.typeName() + ") with value '" + - p.value_to_string() + "' (Type: " + p.get_type_name() + ")"); - }; - // If blackboard entry is allowed to change, continue with next parameter. - continue; - } - - // Check if parameter is known - if (!auto_apms_util::contains(EXPLICITLY_ALLOWED_PARAMETERS, param_name)) { - return create_rejected("Parameter is unkown"); - } - - // Check if the parameter is allowed to change during execution - if (isBusy() && !auto_apms_util::contains(EXPLICITLY_ALLOWED_PARAMETERS_WHILE_BUSY, param_name)) { - return create_rejected("Parameter is not allowed to change while tree executor is running"); - } - - // Check if build handler is allowed to change and valid - if (param_name == _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_BUILD_HANDLER) { - if (!params.allow_other_build_handlers) { - return create_rejected( - "This executor operates with tree build handler '" + executor_param_listener_.get_params().build_handler + - "' and doesn't allow other build handlers to be loaded since the 'Allow other build handlers' option is " - "disabled"); - } - const std::string class_name = p.as_string(); - if (class_name != PARAM_VALUE_NO_BUILD_HANDLER && !build_handler_loader_ptr_->isClassAvailable(class_name)) { - return create_rejected( - "Cannot load build handler '" + class_name + - "' because no corresponding ament_index resource was found. Make sure that you spelled the build handler's " - "name correctly " - "and registered it by calling auto_apms_behavior_tree_register_build_handlers() in the CMakeLists.txt of " - "the " - "corresponding package"); - } - } - - // At this point, if the parameter hasn't been declared, we do not support it. - if (!node_ptr_->has_parameter(param_name)) { - return create_rejected("Parameter '" + param_name + "' is not supported"); - } - } - - // If not returned yet, accept to set the parameter - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - return result; -} - -void TreeExecutorNode::parameter_event_callback_(const rcl_interfaces::msg::ParameterEvent & event) -{ - // Look for any updates to parameters of this node - std::regex re(node_ptr_->get_fully_qualified_name()); - if (std::regex_match(event.node, re)) { - // Enumerate all changes that came in on this event - for (const rclcpp::Parameter & p : rclcpp::ParameterEventHandler::get_parameters_from_event(event)) { - const std::string param_name = p.get_name(); - - // Change scripting enums - if (const std::string enum_key = stripPrefixFromParameterName(SCRIPTING_ENUM_PARAM_PREFIX, param_name); - !enum_key.empty()) { - updateScriptingEnumsWithParameterValues({{enum_key, p.get_parameter_value()}}); - } - - // Change blackboard parameters - if (const std::string entry_key = stripPrefixFromParameterName(BLACKBOARD_PARAM_PREFIX, param_name); - !entry_key.empty()) { - updateGlobalBlackboardWithParameterValues({{entry_key, p.get_parameter_value()}}, false); - } - - // Change tree build handler instance - if (param_name == _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_BUILD_HANDLER) { - loadBuildHandler(p.as_string()); - } - } - } -} - -bool TreeExecutorNode::shouldAcceptStartGoal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr /*goal_ptr*/) +bool TreeExecutorNode::shouldAcceptGoal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr /*goal_ptr*/) { if (isBusy()) { RCLCPP_WARN( @@ -609,303 +74,58 @@ bool TreeExecutorNode::shouldAcceptStartGoal( return true; } -void TreeExecutorNode::onAcceptedStartGoal(std::shared_ptr /*goal_handle_ptr*/) {} - -rclcpp_action::GoalResponse TreeExecutorNode::handle_start_goal_( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal_ptr) +TreeConstructor TreeExecutorNode::getTreeConstructorFromGoal(std::shared_ptr goal_ptr) { - // Validate goal acceptance via virtual hook - if (!shouldAcceptStartGoal(uuid, goal_ptr)) { - return rclcpp_action::GoalResponse::REJECT; - } - if (!goal_ptr->build_handler.empty()) { if (executor_param_listener_.get_params().allow_other_build_handlers) { - try { - loadBuildHandler(goal_ptr->build_handler); - } catch (const std::exception & e) { - RCLCPP_WARN( - logger_, "Goal %s was REJECTED: Loading tree build handler '%s' failed: %s", - rclcpp_action::to_string(uuid).c_str(), goal_ptr->build_handler.c_str(), e.what()); - return rclcpp_action::GoalResponse::REJECT; - } + loadBuildHandler(goal_ptr->build_handler); } else if (goal_ptr->build_handler != current_build_handler_name_) { - RCLCPP_WARN( - logger_, - "Goal %s was REJECTED: Current tree build handler '%s' must not change since the 'Allow other build " - "handlers' option is disabled.", - rclcpp_action::to_string(uuid).c_str(), current_build_handler_name_.c_str()); - return rclcpp_action::GoalResponse::REJECT; + throw exceptions::TreeExecutorError( + "Current tree build handler '" + current_build_handler_name_ + + "' must not change since the 'Allow other build handlers' option is disabled."); } } - core::NodeManifest node_manifest; - try { - node_manifest = core::NodeManifest::decode(goal_ptr->node_manifest); - } catch (const std::exception & e) { - RCLCPP_WARN( - logger_, "Goal %s was REJECTED: Parsing the initial node manifest failed: %s", - rclcpp_action::to_string(uuid).c_str(), e.what()); - return rclcpp_action::GoalResponse::REJECT; - } - - try { - tree_constructor_ = makeTreeConstructor(goal_ptr->build_request, goal_ptr->entry_point, node_manifest); - } catch (const std::exception & e) { - RCLCPP_WARN( - logger_, "Goal %s was REJECTED: Exception in makeTreeConstructor(): %s", rclcpp_action::to_string(uuid).c_str(), - e.what()); - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse TreeExecutorNode::handle_start_cancel_( - std::shared_ptr /*goal_handle_ptr*/) -{ - setControlCommand(ControlCommand::TERMINATE); - return rclcpp_action::CancelResponse::ACCEPT; + core::NodeManifest node_manifest = core::NodeManifest::decode(goal_ptr->node_manifest); + return makeTreeConstructor(goal_ptr->build_request, goal_ptr->entry_point, node_manifest); } -void TreeExecutorNode::handle_start_accept_(std::shared_ptr goal_handle_ptr) +void TreeExecutorNode::onAcceptedGoal(std::shared_ptr goal_handle_ptr) { - // Notify derived classes that the goal has been accepted - onAcceptedStartGoal(goal_handle_ptr); - // Clear blackboard parameters if desired if (goal_handle_ptr->get_goal()->clear_blackboard) { clearGlobalBlackboard(); } +} - const ExecutorParameters params = executor_param_listener_.get_params(); - try { - startExecution(tree_constructor_, params.tick_rate, params.groot2_port); - } catch (const std::exception & e) { - auto result_ptr = std::make_shared(); - result_ptr->message = "An error occurred trying to start execution: " + std::string(e.what()); - result_ptr->tree_result = StartActionContext::Result::TREE_RESULT_NOT_SET; - goal_handle_ptr->abort(result_ptr); - RCLCPP_ERROR_STREAM(logger_, result_ptr->message); - return; - } +void TreeExecutorNode::onExecutionStarted(std::shared_ptr goal_handle_ptr) +{ const std::string started_tree_name = getTreeName(); // If attach is true, the goal's life time is synchronized with the execution. Otherwise we succeed immediately and // leave the executor running (Detached mode). if (goal_handle_ptr->get_goal()->attach) { - start_action_context_.setUp(goal_handle_ptr); + trigger_action_context_.setUp(goal_handle_ptr); RCLCPP_INFO(logger_, "Successfully started execution of tree '%s' (Mode: Attached).", started_tree_name.c_str()); } else { - auto result_ptr = std::make_shared(); + auto result_ptr = std::make_shared(); result_ptr->message = "Successfully started execution of tree '" + started_tree_name + "' (Mode: Detached)."; - result_ptr->tree_result = StartActionContext::Result::TREE_RESULT_NOT_SET; + result_ptr->tree_result = TriggerResult::TREE_RESULT_NOT_SET; result_ptr->terminated_tree_identity = started_tree_name; goal_handle_ptr->succeed(result_ptr); RCLCPP_INFO_STREAM(logger_, result_ptr->message); } } -rclcpp_action::GoalResponse TreeExecutorNode::handle_command_goal_( - const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr goal_ptr) -{ - if (command_timer_ptr_ && !command_timer_ptr_->is_canceled()) { - RCLCPP_WARN(logger_, "Request for setting tree executor command rejected, because previous one is still busy."); - return rclcpp_action::GoalResponse::REJECT; - } - - if (isBusy() && start_action_context_.isValid() && start_action_context_.getGoalHandlePtr()->is_canceling()) { - RCLCPP_WARN(logger_, "Request for setting tree executor command rejected, because tree executor is canceling."); - return rclcpp_action::GoalResponse::REJECT; - } - - const auto execution_state = getExecutionState(); - switch (goal_ptr->command) { - case CommandActionContext::Goal::COMMAND_RESUME: - if (execution_state == ExecutionState::PAUSED || execution_state == ExecutionState::HALTED) { - RCLCPP_INFO(logger_, "Tree with ID '%s' will RESUME.", getTreeName().c_str()); - } else { - RCLCPP_WARN( - logger_, "Requested to RESUME with executor being in state %s. Rejecting request.", - toStr(execution_state).c_str()); - return rclcpp_action::GoalResponse::REJECT; - } - break; - case CommandActionContext::Goal::COMMAND_PAUSE: - if (execution_state == ExecutionState::STARTING || execution_state == ExecutionState::RUNNING) { - RCLCPP_INFO(logger_, "Tree with ID '%s' will PAUSE", getTreeName().c_str()); - } else { - RCLCPP_INFO( - logger_, "Requested to PAUSE with executor already being inactive (State: %s).", - toStr(execution_state).c_str()); - } - break; - case CommandActionContext::Goal::COMMAND_HALT: - if ( - execution_state == ExecutionState::STARTING || execution_state == ExecutionState::RUNNING || - execution_state == ExecutionState::PAUSED) { - RCLCPP_INFO(logger_, "Tree with ID '%s' will HALT.", getTreeName().c_str()); - } else { - RCLCPP_INFO( - logger_, "Requested to HALT with executor already being inactive (State: %s).", - toStr(execution_state).c_str()); - } - break; - case CommandActionContext::Goal::COMMAND_TERMINATE: - if (isBusy()) { - RCLCPP_INFO(logger_, "Executor will TERMINATE tree '%s'.", getTreeName().c_str()); - } else { - RCLCPP_INFO( - logger_, "Requested to TERMINATE with executor already being inactive (State: %s).", - toStr(execution_state).c_str()); - } - break; - default: - RCLCPP_WARN(logger_, "Executor command %i is undefined. Rejecting request.", goal_ptr->command); - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse TreeExecutorNode::handle_command_cancel_( - std::shared_ptr /*goal_handle_ptr*/) -{ - return rclcpp_action::CancelResponse::ACCEPT; -} - -void TreeExecutorNode::handle_command_accept_(std::shared_ptr goal_handle_ptr) -{ - const auto command_request = goal_handle_ptr->get_goal()->command; - ExecutionState requested_state; - switch (command_request) { - case CommandActionContext::Goal::COMMAND_RESUME: - setControlCommand(ControlCommand::RUN); - requested_state = ExecutionState::RUNNING; - break; - case CommandActionContext::Goal::COMMAND_PAUSE: - setControlCommand(ControlCommand::PAUSE); - requested_state = ExecutionState::PAUSED; - break; - case CommandActionContext::Goal::COMMAND_HALT: - setControlCommand(ControlCommand::HALT); - requested_state = ExecutionState::HALTED; - break; - case CommandActionContext::Goal::COMMAND_TERMINATE: - setControlCommand(ControlCommand::TERMINATE); - requested_state = ExecutionState::IDLE; - break; - default: - throw std::logic_error("command_request is unkown"); - } - - command_timer_ptr_ = node_ptr_->create_wall_timer( - std::chrono::duration(executor_param_listener_.get_params().tick_rate), - [this, requested_state, goal_handle_ptr, action_result_ptr = std::make_shared()]() { - // Check if canceling - if (goal_handle_ptr->is_canceling()) { - // Will abandon any progress - goal_handle_ptr->canceled(action_result_ptr); - command_timer_ptr_->cancel(); - return; - } - - const auto current_state = getExecutionState(); - - // If the execution state has become IDLE in the mean time, request failed if termination was not desired - if (requested_state != ExecutionState::IDLE && current_state == ExecutionState::IDLE) { - RCLCPP_ERROR( - logger_, "Failed to reach requested state %s due to cancellation of execution timer. Aborting.", - toStr(requested_state).c_str()); - goal_handle_ptr->abort(action_result_ptr); - command_timer_ptr_->cancel(); - return; - } - - // Wait for the requested state to be reached - if (current_state != requested_state) return; - - goal_handle_ptr->succeed(action_result_ptr); - command_timer_ptr_->cancel(); - }); -} - -bool TreeExecutorNode::onTick() -{ - const ExecutorParameters params = executor_param_listener_.get_params(); - - // Set state change logging flag - getStateObserver().setLogging(params.state_change_logger); - return true; -} - bool TreeExecutorNode::afterTick() { - const ExecutorParameters params = executor_param_listener_.get_params(); + // Call parent's afterTick for blackboard synchronization etc. + if (!GenericTreeExecutorNode::afterTick()) return false; - /** - * Synchronize parameters with new blackboard entries if enabled - */ - - if (executor_options_.blackboard_parameters_dynamic_ && params.allow_dynamic_blackboard) { - TreeBlackboardSharedPtr bb_ptr = getGlobalBlackboardPtr(); - std::vector new_parameters; - for (const BT::StringView & str : bb_ptr->getKeys()) { - const std::string key = std::string(str); - const BT::TypeInfo * type_info = bb_ptr->entryInfo(key); - const BT::Any * any = bb_ptr->getAnyLocked(key).get(); - - // BehaviorTree.CPP does some type validation logic for all data ports when instantiating the tree. It parses - // the XML file, finds the ports of all nodes and initializes blackboard entries wherever used with the - // corresponding type. This is done to to detect type mismatches at construction time. This means, that the - // blackboard will be initialized with empty instances of BT::Any by the time the tree is created. Therefore, we - // must additionally check whether the blackboard entry is empty or not using BT::Any::empty(). - if (any->empty()) continue; - - // If the entry has actually been set with any value during runtime, we update this node's parameters - - if (translated_global_blackboard_entries_.find(key) == translated_global_blackboard_entries_.end()) { - // The key is new, so we must try to infer the parameter's type from BT::Any - const BT::Expected expected = - createParameterValueFromAny(*any, rclcpp::PARAMETER_NOT_SET); - if (expected) { - new_parameters.push_back(rclcpp::Parameter(BLACKBOARD_PARAM_PREFIX + "." + key, expected.value())); - translated_global_blackboard_entries_[key] = expected.value(); - } else { - RCLCPP_WARN( - logger_, "Failed to translate new blackboard entry '%s' (Type: %s) to parameters: %s", key.c_str(), - type_info->typeName().c_str(), expected.error().c_str()); - } - } else { - // The key is not new, so we can look up the parameter's type and update its value (if it changed) - const BT::Expected expected = - createParameterValueFromAny(*any, translated_global_blackboard_entries_[key].get_type()); - if (expected) { - if (expected.value() != translated_global_blackboard_entries_[key]) { - new_parameters.push_back(rclcpp::Parameter(BLACKBOARD_PARAM_PREFIX + "." + key, expected.value())); - } - } else { - RCLCPP_WARN( - logger_, "Failed to translate blackboard entry '%s' (Type: %s) to parameters: %s", key.c_str(), - type_info->typeName().c_str(), expected.error().c_str()); - } - } - } - if (!new_parameters.empty()) { - const rcl_interfaces::msg::SetParametersResult result = node_ptr_->set_parameters_atomically(new_parameters); - if (!result.successful) { - throw exceptions::TreeExecutorError( - "Unexpectedly failed to set parameters inferred from global blackboard. Reason: " + result.reason); - } - } - } - - /** - * Send feedback - */ - - // Only send feedback if started in attached mode - if (start_action_context_.isValid()) { + // Send feedback (only in attached mode) + if (trigger_action_context_.isValid()) { TreeStateObserver & state_observer = getStateObserver(); - auto feedback_ptr = start_action_context_.getFeedbackPtr(); // feedback from previous tick persists + auto feedback_ptr = trigger_action_context_.getFeedbackPtr(); feedback_ptr->execution_state_str = toStr(getExecutionState()); feedback_ptr->running_tree_identity = getTreeName(); auto running_action_history = state_observer.getRunningActionHistory(); @@ -918,54 +138,48 @@ bool TreeExecutorNode::afterTick() // Reset the history cache state_observer.flush(); } - start_action_context_.publishFeedback(); + trigger_action_context_.publishFeedback(); } return true; } -void TreeExecutorNode::onTermination(const ExecutionResult & result) +void TreeExecutorNode::onGoalExecutionTermination(const ExecutionResult & result, TriggerActionContext & context) { - if (!start_action_context_.isValid()) // Do nothing if started in detached mode - return; - - auto result_ptr = start_action_context_.getResultPtr(); + auto result_ptr = context.getResultPtr(); result_ptr->terminated_tree_identity = getTreeName(); switch (result) { case ExecutionResult::TREE_SUCCEEDED: - result_ptr->tree_result = StartActionContext::Result::TREE_RESULT_SUCCESS; + result_ptr->tree_result = TriggerResult::TREE_RESULT_SUCCESS; result_ptr->message = "Tree execution finished with status SUCCESS"; - start_action_context_.succeed(); + context.succeed(); break; case ExecutionResult::TREE_FAILED: - result_ptr->tree_result = StartActionContext::Result::TREE_RESULT_FAILURE; + result_ptr->tree_result = TriggerResult::TREE_RESULT_FAILURE; result_ptr->message = "Tree execution finished with status FAILURE"; - start_action_context_.abort(); + context.abort(); break; case ExecutionResult::TERMINATED_PREMATURELY: - result_ptr->tree_result = StartActionContext::Result::TREE_RESULT_NOT_SET; - if (start_action_context_.getGoalHandlePtr()->is_canceling()) { + result_ptr->tree_result = TriggerResult::TREE_RESULT_NOT_SET; + if (context.getGoalHandlePtr()->is_canceling()) { result_ptr->message = "Tree execution canceled successfully"; - start_action_context_.cancel(); + context.cancel(); } else { result_ptr->message = "Tree execution terminated prematurely"; - start_action_context_.abort(); + context.abort(); } break; case ExecutionResult::ERROR: - result_ptr->tree_result = StartActionContext::Result::TREE_RESULT_NOT_SET; + result_ptr->tree_result = TriggerResult::TREE_RESULT_NOT_SET; result_ptr->message = "An unexpected error occurred during tree execution"; - start_action_context_.abort(); + context.abort(); break; default: - result_ptr->tree_result = StartActionContext::Result::TREE_RESULT_NOT_SET; - result_ptr->message = "Execution result unkown"; - start_action_context_.abort(); + result_ptr->tree_result = TriggerResult::TREE_RESULT_NOT_SET; + result_ptr->message = "Execution result unknown"; + context.abort(); break; } - - // Reset action context - start_action_context_.invalidate(); } } // namespace auto_apms_behavior_tree \ No newline at end of file diff --git a/auto_apms_behavior_tree/src/executor/generic_executor_node.cpp b/auto_apms_behavior_tree/src/executor/generic_executor_node.cpp new file mode 100644 index 00000000..3446e7cf --- /dev/null +++ b/auto_apms_behavior_tree/src/executor/generic_executor_node.cpp @@ -0,0 +1,667 @@ +// Copyright 2026 Robin Müller +// +// 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 "auto_apms_behavior_tree/executor/generic_executor_node.hpp" + +#include +#include +#include + +#include "auto_apms_behavior_tree/exceptions.hpp" +#include "auto_apms_behavior_tree/util/parameter.hpp" +#include "auto_apms_behavior_tree_core/definitions.hpp" +#include "auto_apms_util/container.hpp" +#include "auto_apms_util/string.hpp" +#include "pluginlib/exceptions.hpp" + +namespace auto_apms_behavior_tree +{ + +GenericTreeExecutorNode::GenericTreeExecutorNode(const std::string & name, Options options) +: TreeExecutorBase(std::make_shared(name, options.getROSNodeOptions())), + executor_options_(options), + executor_param_listener_(node_ptr_) +{ + // Remove all parameters from overrides that are not supported + rcl_interfaces::msg::ListParametersResult res = node_ptr_->list_parameters({}, 0); + std::vector unknown_param_names; + for (const std::string & param_name : res.names) { + if (!stripPrefixFromParameterName(SCRIPTING_ENUM_PARAM_PREFIX, param_name).empty()) continue; + if (!stripPrefixFromParameterName(BLACKBOARD_PARAM_PREFIX, param_name).empty()) continue; + if (auto_apms_util::contains(TREE_EXECUTOR_EXPLICITLY_ALLOWED_PARAMETERS, param_name)) continue; + try { + node_ptr_->undeclare_parameter(param_name); + } catch (const rclcpp::exceptions::ParameterImmutableException & e) { + // Allow all builtin read only parameters + continue; + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + // Allow all builtin statically typed parameters + continue; + } + unknown_param_names.push_back(param_name); + } + if (!unknown_param_names.empty()) { + RCLCPP_WARN( + logger_, "The following initial parameters are not supported and have been removed: [ %s ].", + auto_apms_util::join(unknown_param_names, ", ").c_str()); + } + + // Set custom parameter default values. + std::vector new_default_parameters; + std::map effective_param_overrides = + node_ptr_->get_node_parameters_interface()->get_parameter_overrides(); + for (const auto & [name, value] : executor_options_.custom_default_parameters_) { + if (effective_param_overrides.find(name) == effective_param_overrides.end()) { + new_default_parameters.push_back(rclcpp::Parameter(name, value)); + } + } + if (!new_default_parameters.empty()) node_ptr_->set_parameters_atomically(new_default_parameters); + + const ExecutorParameters initial_params = executor_param_listener_.get_params(); + + // Create behavior tree node loader + tree_node_loader_ptr_ = core::NodeRegistrationLoader::make_shared( + std::set(initial_params.node_exclude_packages.begin(), initial_params.node_exclude_packages.end())); + + // Create behavior tree build handler loader + build_handler_loader_ptr_ = TreeBuildHandlerLoader::make_unique( + std::set( + initial_params.build_handler_exclude_packages.begin(), initial_params.build_handler_exclude_packages.end())); + + // Instantiate behavior tree build handler + if ( + initial_params.build_handler != PARAM_VALUE_NO_BUILD_HANDLER && + !build_handler_loader_ptr_->isClassAvailable(initial_params.build_handler)) { + throw exceptions::TreeExecutorError( + "Cannot load build handler '" + initial_params.build_handler + + "' because no corresponding ament_index resource was found. Make sure that you spelled the build handler's " + "name correctly " + "and registered it by calling auto_apms_behavior_tree_register_build_handlers() in the CMakeLists.txt of the " + "corresponding package."); + } + loadBuildHandler(initial_params.build_handler); + + // Collect scripting enum and blackboard parameters from initial parameters + const auto initial_scripting_enums = getParameterValuesWithPrefix(SCRIPTING_ENUM_PARAM_PREFIX); + if (!initial_scripting_enums.empty()) { + if (executor_options_.scripting_enum_parameters_from_overrides_) { + updateScriptingEnumsWithParameterValues(initial_scripting_enums); + } else { + RCLCPP_WARN( + logger_, + "Initial scripting enums have been provided, but the 'Scripting enums from overrides' option is disabled. " + "Ignoring."); + } + } + const auto initial_blackboard = getParameterValuesWithPrefix(BLACKBOARD_PARAM_PREFIX); + if (!initial_blackboard.empty()) { + if (executor_options_.blackboard_parameters_from_overrides_) { + updateGlobalBlackboardWithParameterValues(initial_blackboard); + } else { + RCLCPP_WARN( + logger_, + "Initial blackboard entries have been provided, but the 'Blackboard from overrides' option is disabled. " + "Ignoring."); + } + } + + using namespace std::placeholders; + + // Determine action/service names + const std::string command_action_name = + executor_options_.command_action_name_.empty() + ? std::string(node_ptr_->get_name()) + _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_COMMAND_ACTION_NAME_SUFFIX + : executor_options_.command_action_name_; + const std::string clear_bb_service_name = + executor_options_.clear_blackboard_service_name_.empty() + ? std::string(node_ptr_->get_name()) + _AUTO_APMS_BEHAVIOR_TREE__CLEAR_BLACKBOARD_SERVICE_NAME_SUFFIX + : executor_options_.clear_blackboard_service_name_; + + // Command action server (optional) + if (executor_options_.enable_command_action_) { + command_action_ptr_ = rclcpp_action::create_server( + node_ptr_, command_action_name, std::bind(&GenericTreeExecutorNode::handle_command_goal_, this, _1, _2), + std::bind(&GenericTreeExecutorNode::handle_command_cancel_, this, _1), + std::bind(&GenericTreeExecutorNode::handle_command_accept_, this, _1)); + } + + // Clear blackboard service (optional) + if (executor_options_.enable_clear_blackboard_service_) { + clear_blackboard_service_ptr_ = node_ptr_->create_service( + clear_bb_service_name, [this]( + const std::shared_ptr /*request*/, + std::shared_ptr response) { + response->success = this->clearGlobalBlackboard(); + if (response->success) { + response->message = "Blackboard was cleared successfully"; + } else { + response->message = "Blackboard cannot be cleared, because executor is in state " + + toStr(this->getExecutionState()) + " but must be idling"; + } + RCLCPP_DEBUG_STREAM(this->logger_, response->message); + }); + } + + // Parameter callbacks (only if parameter sync is enabled) + if ( + executor_options_.scripting_enum_parameters_from_overrides_ || + executor_options_.scripting_enum_parameters_dynamic_ || executor_options_.blackboard_parameters_from_overrides_ || + executor_options_.blackboard_parameters_dynamic_) { + on_set_parameters_callback_handle_ptr_ = + node_ptr_->add_on_set_parameters_callback([this](const std::vector & parameters) { + return this->on_set_parameters_callback_(parameters); + }); + + parameter_event_handler_ptr_ = std::make_shared(node_ptr_); + parameter_event_callback_handle_ptr_ = parameter_event_handler_ptr_->add_parameter_event_callback( + [this](const rcl_interfaces::msg::ParameterEvent & event) { this->parameter_event_callback_(event); }); + } +} + +GenericTreeExecutorNode::GenericTreeExecutorNode(rclcpp::NodeOptions options) +: GenericTreeExecutorNode("event_based_tree_executor", Options(options)) +{ +} + +void GenericTreeExecutorNode::preBuild( + core::TreeBuilder & /*builder*/, const std::string & /*build_request*/, const std::string & /*entry_point*/, + const core::NodeManifest & /*node_manifest*/, TreeBlackboard & /*bb*/) +{ +} + +void GenericTreeExecutorNode::postBuild(Tree & /*tree*/) {} + +std::shared_future GenericTreeExecutorNode::startExecution( + const std::string & build_request, const std::string & entry_point, const core::NodeManifest & node_manifest) +{ + const ExecutorParameters params = executor_param_listener_.get_params(); + return startExecution( + makeTreeConstructor(build_request, entry_point, node_manifest), params.tick_rate, params.groot2_port); +} + +GenericTreeExecutorNode::ExecutorParameters GenericTreeExecutorNode::getExecutorParameters() const +{ + return executor_param_listener_.get_params(); +} + +std::map GenericTreeExecutorNode::getParameterValuesWithPrefix( + const std::string & prefix) +{ + const auto res = node_ptr_->list_parameters({prefix}, 2); + std::map value_map; + for (const std::string & name_with_prefix : res.names) { + if (const std::string suffix = stripPrefixFromParameterName(prefix, name_with_prefix); !suffix.empty()) { + value_map[suffix] = node_ptr_->get_parameter(name_with_prefix).get_parameter_value(); + } + } + return value_map; +} + +std::string GenericTreeExecutorNode::stripPrefixFromParameterName( + const std::string & prefix, const std::string & param_name) +{ + const std::regex reg("^" + prefix + "\\.(\\S+)"); + if (std::smatch match; std::regex_match(param_name, match, reg)) return match[1].str(); + return ""; +} + +bool GenericTreeExecutorNode::updateScriptingEnumsWithParameterValues( + const std::map & value_map, bool simulate) +{ + std::map set_successfully_map; + for (const auto & [enum_key, pval] : value_map) { + try { + switch (pval.get_type()) { + case rclcpp::ParameterType::PARAMETER_BOOL: + if (simulate) continue; + scripting_enums_[enum_key] = static_cast(pval.get()); + break; + case rclcpp::ParameterType::PARAMETER_INTEGER: + if (simulate) continue; + scripting_enums_[enum_key] = static_cast(pval.get()); + break; + default: + if (simulate) return false; + throw exceptions::ParameterConversionError("Parameter to scripting enum conversion is not allowed."); + } + set_successfully_map[enum_key] = rclcpp::to_string(pval); + } catch (const std::exception & e) { + RCLCPP_ERROR( + logger_, "Error setting scripting enum from parameter %s=%s (Type: %s): %s", enum_key.c_str(), + rclcpp::to_string(pval).c_str(), rclcpp::to_string(pval.get_type()).c_str(), e.what()); + return false; + } + } + if (!set_successfully_map.empty()) { + RCLCPP_DEBUG( + logger_, "Updated scripting enums from parameters: { %s }", + auto_apms_util::printMap(set_successfully_map).c_str()); + } + return true; +} + +bool GenericTreeExecutorNode::updateGlobalBlackboardWithParameterValues( + const std::map & value_map, bool simulate) +{ + TreeBlackboard & bb = *getGlobalBlackboardPtr(); + std::map set_successfully_map; + for (const auto & [entry_key, pval] : value_map) { + try { + if (const BT::Expected expected = createAnyFromParameterValue(pval)) { + BT::Any any(expected.value()); + if (simulate) { + if (const BT::TypeInfo * entry_info = bb.entryInfo(entry_key)) { + if (entry_info->isStronglyTyped() && entry_info->type() != any.type()) return false; + } + continue; + } else { + bb.set(entry_key, any); + } + } else { + throw exceptions::ParameterConversionError(expected.error()); + } + translated_global_blackboard_entries_[entry_key] = pval; + set_successfully_map[entry_key] = rclcpp::to_string(pval); + } catch (const std::exception & e) { + RCLCPP_ERROR( + logger_, "Error updating blackboard from parameter %s=%s (Type: %s): %s", entry_key.c_str(), + rclcpp::to_string(pval).c_str(), rclcpp::to_string(pval.get_type()).c_str(), e.what()); + return false; + } + } + if (!set_successfully_map.empty()) { + RCLCPP_DEBUG( + logger_, "Updated blackboard from parameters: { %s }", auto_apms_util::printMap(set_successfully_map).c_str()); + } + return true; +} + +void GenericTreeExecutorNode::loadBuildHandler(const std::string & name) +{ + if (build_handler_ptr_ && !executor_param_listener_.get_params().allow_other_build_handlers) { + throw std::logic_error( + "Executor option 'Allow other build handlers' is disabled, but loadBuildHandler() was called again after " + "instantiating '" + + current_build_handler_name_ + "'."); + } + if (current_build_handler_name_ == name) return; + if (name == PARAM_VALUE_NO_BUILD_HANDLER) { + build_handler_ptr_.reset(); + } else { + try { + build_handler_ptr_ = + build_handler_loader_ptr_->createUniqueInstance(name)->makeUnique(node_ptr_, tree_node_loader_ptr_); + } catch (const pluginlib::CreateClassException & e) { + throw exceptions::TreeExecutorError( + "An error occurred when trying to create an instance of tree build handler class '" + name + + "'. This might be because you forgot to call the AUTO_APMS_BEHAVIOR_TREE_REGISTER_BUILD_HANDLER macro " + "in the source file: " + + e.what()); + } catch (const std::exception & e) { + throw exceptions::TreeExecutorError( + "An error occurred when trying to create an instance of tree build handler class '" + name + "': " + e.what()); + } + } + current_build_handler_name_ = name; +} + +TreeConstructor GenericTreeExecutorNode::makeTreeConstructor( + const std::string & build_request, const std::string & entry_point, const core::NodeManifest & node_manifest) +{ + // Request the tree identity + if (build_handler_ptr_ && !build_handler_ptr_->setBuildRequest(build_request, entry_point, node_manifest)) { + throw exceptions::TreeBuildError( + "Build request '" + build_request + "' was denied by '" + current_build_handler_name_ + + "' (setBuildRequest() returned false)."); + } + + return [this, build_request, entry_point, node_manifest](TreeBlackboardSharedPtr bb_ptr) { + // Currently, BehaviorTree.CPP requires the memory allocated by the factory to persist even after the tree has + // been created, so we make the builder a unique pointer that is only reset when a new tree is to be created. See + // https://github.com/BehaviorTree/BehaviorTree.CPP/issues/890 + this->builder_ptr_.reset(new core::TreeBuilder( + this->node_ptr_, this->getTreeNodeWaitablesCallbackGroupPtr(), this->getTreeNodeWaitablesExecutorPtr(), + this->tree_node_loader_ptr_)); + + // Allow executor to make modifications prior to building the tree + this->preBuild(*this->builder_ptr_, build_request, entry_point, node_manifest, *bb_ptr); + + // Make scripting enums available to tree instance + for (const auto & [enum_key, val] : this->scripting_enums_) this->builder_ptr_->setScriptingEnum(enum_key, val); + + // If a build handler is specified, let it configure the builder and determine which tree is to be instantiated + std::string instantiate_name = ""; + if (this->build_handler_ptr_) { + instantiate_name = this->build_handler_ptr_->buildTree(*this->builder_ptr_, *bb_ptr).getName(); + } + + // Finally, instantiate the tree + Tree tree = instantiate_name.empty() ? this->builder_ptr_->instantiate(bb_ptr) + : this->builder_ptr_->instantiate(instantiate_name, bb_ptr); + + // Allow executor to make modifications after building the tree, but before execution starts + this->postBuild(tree); + return tree; + }; +} + +core::TreeBuilder::SharedPtr GenericTreeExecutorNode::createTreeBuilder() +{ + return core::TreeBuilder::make_shared( + this->node_ptr_, this->getTreeNodeWaitablesCallbackGroupPtr(), this->getTreeNodeWaitablesExecutorPtr(), + this->tree_node_loader_ptr_); +} + +bool GenericTreeExecutorNode::clearGlobalBlackboard() +{ + if (TreeExecutorBase::clearGlobalBlackboard()) { + if (executor_options_.blackboard_parameters_from_overrides_ || executor_options_.blackboard_parameters_dynamic_) { + const auto res = node_ptr_->list_parameters({BLACKBOARD_PARAM_PREFIX}, 2); + for (const std::string & name : res.names) { + node_ptr_->undeclare_parameter(name); + } + } + return true; + } + return false; +} + +rcl_interfaces::msg::SetParametersResult GenericTreeExecutorNode::on_set_parameters_callback_( + const std::vector & parameters) +{ + const ExecutorParameters params = executor_param_listener_.get_params(); + + for (const rclcpp::Parameter & p : parameters) { + auto create_rejected = [&p](const std::string msg) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = "Rejected to set " + p.get_name() + " = " + p.value_to_string() + " (Type: " + p.get_type_name() + + "): " + msg + "."; + return result; + }; + const std::string param_name = p.get_name(); + + // Check if parameter is a scripting enum + if (const std::string enum_key = stripPrefixFromParameterName(SCRIPTING_ENUM_PARAM_PREFIX, param_name); + !enum_key.empty()) { + if (isBusy()) { + return create_rejected("Scripting enums cannot change while tree executor is running"); + } + if (!executor_options_.scripting_enum_parameters_dynamic_ || !params.allow_dynamic_scripting_enums) { + return create_rejected( + "Cannot set scripting enum '" + enum_key + "', because the 'Dynamic scripting enums' option is disabled"); + } + if (!updateScriptingEnumsWithParameterValues({{enum_key, p.get_parameter_value()}}, true)) { + return create_rejected( + "Type of scripting enum must be bool or int. Tried to set enum '" + enum_key + "' with value '" + + p.value_to_string() + "' (Type: " + p.get_type_name() + ")"); + } + continue; + } + + // Check if parameter is a blackboard parameter + if (const std::string entry_key = stripPrefixFromParameterName(BLACKBOARD_PARAM_PREFIX, param_name); + !entry_key.empty()) { + if (!executor_options_.blackboard_parameters_dynamic_ || !params.allow_dynamic_blackboard) { + return create_rejected( + "Cannot set blackboard entry '" + entry_key + "', because the 'Dynamic blackboard' option is disabled"); + } + if (!updateGlobalBlackboardWithParameterValues({{entry_key, p.get_parameter_value()}}, true)) { + return create_rejected( + "Type of blackboard entries must not change. Tried to set entry '" + entry_key + + "' (Type: " + getGlobalBlackboardPtr()->getEntry(entry_key)->info.typeName() + ") with value '" + + p.value_to_string() + "' (Type: " + p.get_type_name() + ")"); + } + continue; + } + + // Check if parameter is known + if (!auto_apms_util::contains(TREE_EXECUTOR_EXPLICITLY_ALLOWED_PARAMETERS, param_name)) { + return create_rejected("Parameter is unknown"); + } + + // Check if the parameter is allowed to change during execution + if (isBusy() && !auto_apms_util::contains(TREE_EXECUTOR_EXPLICITLY_ALLOWED_PARAMETERS_WHILE_BUSY, param_name)) { + return create_rejected("Parameter is not allowed to change while tree executor is running"); + } + + // Check if build handler is allowed to change and valid + if (param_name == _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_BUILD_HANDLER) { + if (!params.allow_other_build_handlers) { + return create_rejected( + "This executor operates with tree build handler '" + executor_param_listener_.get_params().build_handler + + "' and doesn't allow other build handlers to be loaded since the 'Allow other build handlers' option is " + "disabled"); + } + const std::string class_name = p.as_string(); + if (class_name != PARAM_VALUE_NO_BUILD_HANDLER && !build_handler_loader_ptr_->isClassAvailable(class_name)) { + return create_rejected( + "Cannot load build handler '" + class_name + + "' because no corresponding ament_index resource was found. Make sure that you spelled the build handler's " + "name correctly " + "and registered it by calling auto_apms_behavior_tree_register_build_handlers() in the CMakeLists.txt of " + "the " + "corresponding package"); + } + } + + // At this point, if the parameter hasn't been declared, we do not support it. + if (!node_ptr_->has_parameter(param_name)) { + return create_rejected("Parameter '" + param_name + "' is not supported"); + } + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} + +void GenericTreeExecutorNode::parameter_event_callback_(const rcl_interfaces::msg::ParameterEvent & event) +{ + std::regex re(node_ptr_->get_fully_qualified_name()); + if (std::regex_match(event.node, re)) { + for (const rclcpp::Parameter & p : rclcpp::ParameterEventHandler::get_parameters_from_event(event)) { + const std::string param_name = p.get_name(); + + if (const std::string enum_key = stripPrefixFromParameterName(SCRIPTING_ENUM_PARAM_PREFIX, param_name); + !enum_key.empty()) { + updateScriptingEnumsWithParameterValues({{enum_key, p.get_parameter_value()}}); + } + + if (const std::string entry_key = stripPrefixFromParameterName(BLACKBOARD_PARAM_PREFIX, param_name); + !entry_key.empty()) { + updateGlobalBlackboardWithParameterValues({{entry_key, p.get_parameter_value()}}, false); + } + + if (param_name == _AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_BUILD_HANDLER) { + loadBuildHandler(p.as_string()); + } + } + } +} + +rclcpp_action::GoalResponse GenericTreeExecutorNode::handle_command_goal_( + const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr goal_ptr) +{ + if (command_timer_ptr_ && !command_timer_ptr_->is_canceled()) { + RCLCPP_WARN(logger_, "Request for setting tree executor command rejected, because previous one is still busy."); + return rclcpp_action::GoalResponse::REJECT; + } + + const auto execution_state = getExecutionState(); + switch (goal_ptr->command) { + case CommandActionContext::Goal::COMMAND_RESUME: + if (execution_state == ExecutionState::PAUSED || execution_state == ExecutionState::HALTED) { + RCLCPP_INFO(logger_, "Tree with ID '%s' will RESUME.", getTreeName().c_str()); + } else { + RCLCPP_WARN( + logger_, "Requested to RESUME with executor being in state %s. Rejecting request.", + toStr(execution_state).c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + break; + case CommandActionContext::Goal::COMMAND_PAUSE: + if (execution_state == ExecutionState::STARTING || execution_state == ExecutionState::RUNNING) { + RCLCPP_INFO(logger_, "Tree with ID '%s' will PAUSE", getTreeName().c_str()); + } else { + RCLCPP_INFO( + logger_, "Requested to PAUSE with executor already being inactive (State: %s).", + toStr(execution_state).c_str()); + } + break; + case CommandActionContext::Goal::COMMAND_HALT: + if ( + execution_state == ExecutionState::STARTING || execution_state == ExecutionState::RUNNING || + execution_state == ExecutionState::PAUSED) { + RCLCPP_INFO(logger_, "Tree with ID '%s' will HALT.", getTreeName().c_str()); + } else { + RCLCPP_INFO( + logger_, "Requested to HALT with executor already being inactive (State: %s).", + toStr(execution_state).c_str()); + } + break; + case CommandActionContext::Goal::COMMAND_TERMINATE: + if (isBusy()) { + RCLCPP_INFO(logger_, "Executor will TERMINATE tree '%s'.", getTreeName().c_str()); + } else { + RCLCPP_INFO( + logger_, "Requested to TERMINATE with executor already being inactive (State: %s).", + toStr(execution_state).c_str()); + } + break; + default: + RCLCPP_WARN(logger_, "Executor command %i is undefined. Rejecting request.", goal_ptr->command); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GenericTreeExecutorNode::handle_command_cancel_( + std::shared_ptr /*goal_handle_ptr*/) +{ + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GenericTreeExecutorNode::handle_command_accept_(std::shared_ptr goal_handle_ptr) +{ + const auto command_request = goal_handle_ptr->get_goal()->command; + ExecutionState requested_state; + switch (command_request) { + case CommandActionContext::Goal::COMMAND_RESUME: + setControlCommand(ControlCommand::RUN); + requested_state = ExecutionState::RUNNING; + break; + case CommandActionContext::Goal::COMMAND_PAUSE: + setControlCommand(ControlCommand::PAUSE); + requested_state = ExecutionState::PAUSED; + break; + case CommandActionContext::Goal::COMMAND_HALT: + setControlCommand(ControlCommand::HALT); + requested_state = ExecutionState::HALTED; + break; + case CommandActionContext::Goal::COMMAND_TERMINATE: + setControlCommand(ControlCommand::TERMINATE); + requested_state = ExecutionState::IDLE; + break; + default: + throw std::logic_error("command_request is unknown"); + } + + command_timer_ptr_ = node_ptr_->create_wall_timer( + std::chrono::duration(executor_param_listener_.get_params().tick_rate), + [this, requested_state, goal_handle_ptr, action_result_ptr = std::make_shared()]() { + if (goal_handle_ptr->is_canceling()) { + goal_handle_ptr->canceled(action_result_ptr); + command_timer_ptr_->cancel(); + return; + } + + const auto current_state = getExecutionState(); + + if (requested_state != ExecutionState::IDLE && current_state == ExecutionState::IDLE) { + RCLCPP_ERROR( + logger_, "Failed to reach requested state %s due to cancellation of execution timer. Aborting.", + toStr(requested_state).c_str()); + goal_handle_ptr->abort(action_result_ptr); + command_timer_ptr_->cancel(); + return; + } + + if (current_state != requested_state) return; + + goal_handle_ptr->succeed(action_result_ptr); + command_timer_ptr_->cancel(); + }); +} + +bool GenericTreeExecutorNode::onTick() +{ + const ExecutorParameters params = executor_param_listener_.get_params(); + getStateObserver().setLogging(params.state_change_logger); + return true; +} + +bool GenericTreeExecutorNode::afterTick() +{ + const ExecutorParameters params = executor_param_listener_.get_params(); + + // Synchronize parameters with new blackboard entries if enabled + if (executor_options_.blackboard_parameters_dynamic_ && params.allow_dynamic_blackboard) { + TreeBlackboardSharedPtr bb_ptr = getGlobalBlackboardPtr(); + std::vector new_parameters; + for (const BT::StringView & str : bb_ptr->getKeys()) { + const std::string key = std::string(str); + const BT::TypeInfo * type_info = bb_ptr->entryInfo(key); + const BT::Any * any = bb_ptr->getAnyLocked(key).get(); + + if (any->empty()) continue; + + if (translated_global_blackboard_entries_.find(key) == translated_global_blackboard_entries_.end()) { + const BT::Expected expected = + createParameterValueFromAny(*any, rclcpp::PARAMETER_NOT_SET); + if (expected) { + new_parameters.push_back(rclcpp::Parameter(BLACKBOARD_PARAM_PREFIX + "." + key, expected.value())); + translated_global_blackboard_entries_[key] = expected.value(); + } else { + RCLCPP_WARN( + logger_, "Failed to translate new blackboard entry '%s' (Type: %s) to parameters: %s", key.c_str(), + type_info->typeName().c_str(), expected.error().c_str()); + } + } else { + const BT::Expected expected = + createParameterValueFromAny(*any, translated_global_blackboard_entries_[key].get_type()); + if (expected) { + if (expected.value() != translated_global_blackboard_entries_[key]) { + new_parameters.push_back(rclcpp::Parameter(BLACKBOARD_PARAM_PREFIX + "." + key, expected.value())); + } + } else { + RCLCPP_WARN( + logger_, "Failed to translate blackboard entry '%s' (Type: %s) to parameters: %s", key.c_str(), + type_info->typeName().c_str(), expected.error().c_str()); + } + } + } + if (!new_parameters.empty()) { + const rcl_interfaces::msg::SetParametersResult result = node_ptr_->set_parameters_atomically(new_parameters); + if (!result.successful) { + throw exceptions::TreeExecutorError( + "Unexpectedly failed to set parameters inferred from global blackboard. Reason: " + result.reason); + } + } + } + + return true; +} + +} // namespace auto_apms_behavior_tree diff --git a/auto_apms_behavior_tree/src/executor/options.cpp b/auto_apms_behavior_tree/src/executor/options.cpp new file mode 100644 index 00000000..c8506c4a --- /dev/null +++ b/auto_apms_behavior_tree/src/executor/options.cpp @@ -0,0 +1,84 @@ +// Copyright 2026 Robin Müller +// +// 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 "auto_apms_behavior_tree/executor/options.hpp" + +namespace auto_apms_behavior_tree +{ + +// --- TreeExecutorNodeOptions --- + +TreeExecutorNodeOptions::TreeExecutorNodeOptions(const rclcpp::NodeOptions & ros_node_options) +: ros_node_options_(ros_node_options) +{ +} + +TreeExecutorNodeOptions & TreeExecutorNodeOptions::enableCommandAction(bool enable) +{ + enable_command_action_ = enable; + return *this; +} + +TreeExecutorNodeOptions & TreeExecutorNodeOptions::enableClearBlackboardService(bool enable) +{ + enable_clear_blackboard_service_ = enable; + return *this; +} + +TreeExecutorNodeOptions & TreeExecutorNodeOptions::enableScriptingEnumParameters(bool from_overrides, bool dynamic) +{ + scripting_enum_parameters_from_overrides_ = from_overrides; + scripting_enum_parameters_dynamic_ = dynamic; + return *this; +} + +TreeExecutorNodeOptions & TreeExecutorNodeOptions::enableGlobalBlackboardParameters(bool from_overrides, bool dynamic) +{ + blackboard_parameters_from_overrides_ = from_overrides; + blackboard_parameters_dynamic_ = dynamic; + return *this; +} + +TreeExecutorNodeOptions & TreeExecutorNodeOptions::setDefaultBuildHandler(const std::string & name) +{ + custom_default_parameters_[_AUTO_APMS_BEHAVIOR_TREE__EXECUTOR_PARAM_BUILD_HANDLER] = rclcpp::ParameterValue(name); + return *this; +} + +TreeExecutorNodeOptions & TreeExecutorNodeOptions::setCommandActionName(const std::string & name) +{ + command_action_name_ = name; + return *this; +} + +TreeExecutorNodeOptions & TreeExecutorNodeOptions::setClearBlackboardServiceName(const std::string & name) +{ + clear_blackboard_service_name_ = name; + return *this; +} + +rclcpp::NodeOptions TreeExecutorNodeOptions::getROSNodeOptions() const +{ + rclcpp::NodeOptions opt(ros_node_options_); + opt.automatically_declare_parameters_from_overrides( + scripting_enum_parameters_from_overrides_ || blackboard_parameters_from_overrides_); + opt.allow_undeclared_parameters(scripting_enum_parameters_dynamic_ || blackboard_parameters_dynamic_); + + // Default configuration + opt.enable_logger_service(true); + + return opt; +} + +} // namespace auto_apms_behavior_tree \ No newline at end of file diff --git a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/builder.hpp b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/builder.hpp index a0bd83e7..bcc06cb6 100644 --- a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/builder.hpp +++ b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/builder.hpp @@ -114,15 +114,26 @@ class TreeBuilder : public TreeDocument template TreeBuilder & setScriptingEnumsFromType(); + /** + * @brief Create the behavior tree. + * + * This creates an instance of `BT::Tree` which holds the memory of all node callbacks and enables the user to + * actually execute the behavior tree. The entry point is set to the name of @p tree_ele. + * @param tree_ele Behavior tree element that should be instantiated as a behavior tree. + * @param bb_ptr Optional pointer to a blackboard. This will be associated with the root tree. + * @return Instance of `BT::Tree` representing the configured behavior tree. + * @throw auto_apms_behavior_tree::exceptions::TreeBuildError if the tree cannot be instantiated. + */ + Tree instantiate(const TreeElement & tree_ele, TreeBlackboardSharedPtr bb_ptr = TreeBlackboard::create()); + /** * @brief Create the behavior tree. * * This creates an instance of `BT::Tree` which holds the memory of all node callbacks and enables the user to * actually execute the behavior tree. The entry point is determined by @p root_tree_name. * @param root_tree_name Name of an existing tree that should be the root tree. - * @param bb_ptr Optional pointer to the parent blackboard. + * @param bb_ptr Optional pointer to a blackboard. This will be associated with the root tree. * @return Instance of `BT::Tree` representing the configured behavior tree. - * @throw auto_apms_behavior_tree::exceptions::TreeBuildError if there's no tree named @p root_tree_name. * @throw auto_apms_behavior_tree::exceptions::TreeBuildError if the tree cannot be instantiated. */ Tree instantiate(const std::string & root_tree_name, TreeBlackboardSharedPtr bb_ptr = TreeBlackboard::create()); @@ -132,7 +143,7 @@ class TreeBuilder : public TreeDocument * * This creates an instance of `BT::Tree` which holds the memory of all node callbacks and enables the user to * actually execute the behavior tree. The entry point will be the underlying document's root tree. - * @param bb_ptr Optional pointer to the parent blackboard. + * @param bb_ptr Optional pointer to a blackboard. This will be associated with the root tree. * @return Instance of `BT::Tree` representing the configured behavior tree. * @throw auto_apms_behavior_tree::exceptions::TreeBuildError if it's not defined which one of the existing trees is * the root tree. diff --git a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/node_registration_options.hpp b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/node_registration_options.hpp index 0f74adf6..9930972c 100644 --- a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/node_registration_options.hpp +++ b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/node_registration_options.hpp @@ -270,8 +270,8 @@ struct convert continue; } - // Unkown parameter - throw auto_apms_util::exceptions::YAMLFormatError("Unkown parameter name '" + key + "'."); + // Unknown parameter + throw auto_apms_util::exceptions::YAMLFormatError("Unknown parameter name '" + key + "'."); } return true; } diff --git a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_action_node.hpp b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_action_node.hpp index d5e1473b..70eae506 100644 --- a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_action_node.hpp +++ b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_action_node.hpp @@ -677,7 +677,7 @@ template inline std::string RosActionNode::getActionName() const { if (client_instance_) return client_instance_->name; - return "unkown"; + return "unknown"; } template diff --git a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_service_node.hpp b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_service_node.hpp index 5bd44561..6663e066 100644 --- a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_service_node.hpp +++ b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_service_node.hpp @@ -440,7 +440,7 @@ template inline std::string RosServiceNode::getServiceName() const { if (client_instance_) return client_instance_->name; - return "unkown"; + return "unknown"; } template diff --git a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_subscriber_node.hpp b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_subscriber_node.hpp index 90bbe17a..3f8de44a 100644 --- a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_subscriber_node.hpp +++ b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_subscriber_node.hpp @@ -366,7 +366,7 @@ template inline std::string RosSubscriberNode::getTopicName() const { if (sub_instance_) return sub_instance_->name; - return "unkown"; + return "unknown"; } template diff --git a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/tree/tree_document.hpp b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/tree/tree_document.hpp index db352be7..e1c087f8 100644 --- a/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/tree/tree_document.hpp +++ b/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/tree/tree_document.hpp @@ -684,13 +684,13 @@ class TreeDocument : private tinyxml2::XMLDocument * @brief Populate the the node's data ports. * * This method verifies that @p port_values only refers to implemented ports and throws an exception if any - * values for unkown port names are provided. + * values for unknown port names are provided. * * @note This method only works as intended if the node represented by this element has already been registered. * Otherwise, the node's model is not known and therefore the implemented ports cannot be determined. * @param port_values Port values to be used to populate the corresponding attributes of the node element. * @return Reference to the modified instance. - * @throws auto_apms_behavior_tree::exceptions::TreeDocumentError if @p port_values contains any unkown keys, e.g. + * @throws auto_apms_behavior_tree::exceptions::TreeDocumentError if @p port_values contains any unknown keys, e.g. * names for ports that are not implemented. */ NodeElement & setPorts(const PortValues & port_values); diff --git a/auto_apms_behavior_tree_core/src/builder.cpp b/auto_apms_behavior_tree_core/src/builder.cpp index 4e3b9ef4..59c59604 100644 --- a/auto_apms_behavior_tree_core/src/builder.cpp +++ b/auto_apms_behavior_tree_core/src/builder.cpp @@ -67,6 +67,20 @@ TreeBuilder & TreeBuilder::setScriptingEnum(const std::string & enum_name, int v return *this; } +Tree TreeBuilder::instantiate(const TreeElement & tree_ele, TreeBlackboardSharedPtr bb_ptr) +{ + const std::string tree_name = tree_ele.getName(); + if (hasTreeName(tree_name) && getTree(tree_name) == tree_ele) { + // If tree already exists and the given element points to the same tree, we can directly instantiate it + return instantiate(tree_name, bb_ptr); + } + // If the given tree element is not part of this document, we throw (users must use the associated builder for + // instantiating it) + throw exceptions::TreeBuildError( + "Cannot instantiate tree with name '" + tree_name + + "' because it doesn't belong to this TreeBuilder. Make sure to merge the tree before trying to instantiate it."); +} + Tree TreeBuilder::instantiate(const std::string & root_tree_name, TreeBlackboardSharedPtr bb_ptr) { if (!hasTreeName(root_tree_name)) { diff --git a/auto_apms_behavior_tree_core/src/cli/create_node_reference_markdown.cpp b/auto_apms_behavior_tree_core/src/cli/create_node_reference_markdown.cpp index 04efcbd7..bc64df3c 100644 --- a/auto_apms_behavior_tree_core/src/cli/create_node_reference_markdown.cpp +++ b/auto_apms_behavior_tree_core/src/cli/create_node_reference_markdown.cpp @@ -84,7 +84,7 @@ int main(int argc, char ** argv) node_manifest.add(registration_name, native_node_options); package_for_class[node_manifest[registration_name].class_name] = native_package_name; } else { - throw std::runtime_error("Package for node '" + registration_name + "' is unkown."); + throw std::runtime_error("Package for node '" + registration_name + "' is unknown."); } } } diff --git a/auto_apms_behavior_tree_core/src/tree/tree_document.cpp b/auto_apms_behavior_tree_core/src/tree/tree_document.cpp index 49619b3d..80a1340a 100644 --- a/auto_apms_behavior_tree_core/src/tree/tree_document.cpp +++ b/auto_apms_behavior_tree_core/src/tree/tree_document.cpp @@ -124,7 +124,7 @@ TreeDocument::NodeElement TreeDocument::NodeElement::insertNode( { if (const std::set names = doc_ptr_->getRegisteredNodeNames(true); names.find(name) == names.end()) { throw exceptions::TreeDocumentError( - "Cannot insert unkown node <" + name + + "Cannot insert unknown node <" + name + ">. Before inserting a new node, the associated document must register the corresponding behavior tree " "node. Consider using a signature of insertNode() that does this automatically."); } @@ -255,7 +255,7 @@ TreeDocument::NodeElement TreeDocument::NodeElement::insertTreeFromDocument( for (const NodeElement & ele : found) names.push_back(ele.getFullyQualifiedName()); throw exceptions::TreeDocumentError( "Cannot insert tree '" + tree_name + "' because the following nodes found in tree '" + name + - "' are unkown to the builder:\n\t- " + auto_apms_util::join(names, "\n\t- ")); + "' are unknown to the builder:\n\t- " + auto_apms_util::join(names, "\n\t- ")); } } @@ -390,14 +390,14 @@ TreeDocument::NodeElement::PortValues TreeDocument::NodeElement::getPorts() cons TreeDocument::NodeElement & TreeDocument::NodeElement::setPorts(const PortValues & port_values) { // Verify port_values - std::vector unkown_keys; + std::vector unknown_keys; for (const auto & [key, _] : port_values) { - if (!auto_apms_util::contains(port_names_, key)) unkown_keys.push_back(key); + if (!auto_apms_util::contains(port_names_, key)) unknown_keys.push_back(key); } - if (!unkown_keys.empty()) { + if (!unknown_keys.empty()) { throw exceptions::TreeDocumentError( "Cannot set ports. According to the node model, the following ports are not implemented by '" + - std::string(ele_ptr_->Name()) + "': [ " + auto_apms_util::join(unkown_keys, ", ") + " ]."); + std::string(ele_ptr_->Name()) + "': [ " + auto_apms_util::join(unknown_keys, ", ") + " ]."); } // Populate attributes according to the content of port_values @@ -610,7 +610,7 @@ TreeDocument::TreeElement & TreeDocument::TreeElement::setName(const std::string std::string TreeDocument::TreeElement::getName() const { if (const char * name = ele_ptr_->Attribute(TREE_NAME_ATTRIBUTE_NAME)) return name; - return "unkown"; + return "unknown"; } TreeDocument::TreeElement & TreeDocument::TreeElement::makeRoot() @@ -1305,7 +1305,7 @@ NodeModelMap TreeDocument::getNodeModel(tinyxml2::XMLDocument & doc, const NodeM port_info.port_direction = BT::PortDirection::INOUT; } else { throw exceptions::TreeDocumentError( - "Unkown port direction in node model for '" + std::string(node_name) + "': " + direction); + "Unknown port direction in node model for '" + std::string(node_name) + "': " + direction); } if (const char * c = port_ele->Attribute("name")) { port_info.port_name = c; diff --git a/auto_apms_behavior_tree_core/src/tree/tree_resource.cpp b/auto_apms_behavior_tree_core/src/tree/tree_resource.cpp index dedc0297..4a9ca9fb 100644 --- a/auto_apms_behavior_tree_core/src/tree/tree_resource.cpp +++ b/auto_apms_behavior_tree_core/src/tree/tree_resource.cpp @@ -127,7 +127,7 @@ std::string TreeResource::getRootTreeName() const throw auto_apms_util::exceptions::ResourceError( "Cannot get root tree name of tree resource '" + unique_identity_.str() + "'. Since there is no XML attribute named '" + TreeDocument::ROOT_TREE_ATTRIBUTE_NAME + - "' and the resource identity doesn't specify , the root tree is unkown."); + "' and the resource identity doesn't specify , the root tree is unknown."); } TreeResourceIdentity TreeResource::createIdentityForTree(const std::string & tree_name) const diff --git a/auto_apms_mission/config/orchestrator_base.xml b/auto_apms_mission/config/orchestrator_base.xml index d5a02eff..2fe4bfaa 100644 --- a/auto_apms_mission/config/orchestrator_base.xml +++ b/auto_apms_mission/config/orchestrator_base.xml @@ -56,7 +56,7 @@ level="WARN"/> - diff --git a/auto_apms_mission/include/auto_apms_mission/mission_config.hpp b/auto_apms_mission/include/auto_apms_mission/mission_config.hpp index 6a13dfc6..d514a6df 100644 --- a/auto_apms_mission/include/auto_apms_mission/mission_config.hpp +++ b/auto_apms_mission/include/auto_apms_mission/mission_config.hpp @@ -211,8 +211,8 @@ struct convert continue; } - // Unkown parameter - throw auto_apms_util::exceptions::YAMLFormatError("Unkown parameter name '" + key + "'."); + // Unknown parameter + throw auto_apms_util::exceptions::YAMLFormatError("Unknown parameter name '" + key + "'."); } return true; }