executor_node.hpp

// Copyright 2024 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
//
//      https://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_params.hpp" // TODO Uncomment if generate_parameter_library 0.4.0 is
// available
#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_interfaces/action/start_tree_executor.hpp"
#include "auto_apms_util/action_context.hpp"
#include "executor_params.hpp"  // TODO Remove if generate_parameter_library 0.4.0 is available
#include "rclcpp/rclcpp.hpp"
 
namespace auto_apms_behavior_tree
{

class TreeExecutorNodeOptions
{
public:
  TreeExecutorNodeOptions(const rclcpp::NodeOptions & ros_node_options);

  TreeExecutorNodeOptions & enableScriptingEnumParameters(bool from_overrides, bool dynamic);

  TreeExecutorNodeOptions & enableGlobalBlackboardParameters(bool from_overrides, bool dynamic);

  TreeExecutorNodeOptions & setDefaultBuildHandler(const std::string & name);

  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<std::string, rclcpp::ParameterValue> custom_default_parameters_;
};

class TreeExecutorNode : public TreeExecutorBase
{
public:
  using Options = TreeExecutorNodeOptions;
  using ExecutorParameters = executor_params::Params;
  using ExecutorParameterListener = executor_params::ParamListener;
  using StartActionContext = auto_apms_util::ActionContext<auto_apms_interfaces::action::StartTreeExecutor>;
  using CommandActionContext = auto_apms_util::ActionContext<auto_apms_interfaces::action::CommandTreeExecutor>;
  using TreeBuilder = core::TreeBuilder;
 
  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";

  TreeExecutorNode(const std::string & name, TreeExecutorNodeOptions executor_options);

  explicit TreeExecutorNode(rclcpp::NodeOptions options);
 
  virtual ~TreeExecutorNode() override = default;
 
  using TreeExecutorBase::startExecution;

  std::shared_future<ExecutionResult> startExecution(const std::string & tree_build_request);
 
private:
  /* Virtual methods */

  virtual void setUpBuilder(TreeBuilder & builder, const core::NodeManifest & node_manifest);
 
protected:
  /* Utility methods */

  std::map<std::string, rclcpp::ParameterValue> getParameterValuesWithPrefix(const std::string & prefix);

  static std::string stripPrefixFromParameterName(const std::string & prefix, const std::string & param_name);

  bool updateScriptingEnumsWithParameterValues(
    const std::map<std::string, rclcpp::ParameterValue> & value_map, bool simulate = false);

  bool updateGlobalBlackboardWithParameterValues(
    const std::map<std::string, rclcpp::ParameterValue> & value_map, bool simulate = false);

  void loadBuildHandler(const std::string & name);

  TreeConstructor makeTreeConstructor(
    const std::string & build_handler_request, const std::string & root_tree_name,
    const core::NodeManifest & node_manifest = {}, const core::NodeManifest & node_overrides = {});
 
private:
  /* Executor specific virtual overrides */
 
  bool onTick() override final;
 
  bool afterTick() override final;
 
  void onTermination(const ExecutionResult & result) override final;
 
  /* Internal callbacks */
 
  rcl_interfaces::msg::SetParametersResult on_set_parameters_callback_(
    const std::vector<rclcpp::Parameter> & 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<const StartActionContext::Goal> goal_ptr);
  rclcpp_action::CancelResponse handle_start_cancel_(std::shared_ptr<StartActionContext::GoalHandle> goal_handle_ptr);
  void handle_start_accept_(std::shared_ptr<StartActionContext::GoalHandle> goal_handle_ptr);
 
  rclcpp_action::GoalResponse handle_command_goal_(
    const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const CommandActionContext::Goal> goal_ptr);
  rclcpp_action::CancelResponse handle_command_cancel_(
    std::shared_ptr<CommandActionContext::GoalHandle> goal_handle_ptr);
  void handle_command_accept_(std::shared_ptr<CommandActionContext::GoalHandle> goal_handle_ptr);
 
  const TreeExecutorNodeOptions executor_options_;
  ExecutorParameterListener executor_param_listener_;
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_ptr_;
  std::shared_ptr<rclcpp::ParameterEventHandler> 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_;
  TreeBuilder::UniquePtr builder_ptr_;
  TreeBuildHandler::UniquePtr build_handler_ptr_;
  std::string current_build_handler_name_;
  std::map<std::string, int> scripting_enums_;
  std::map<std::string, rclcpp::ParameterValue> translated_global_blackboard_entries_;
  TreeConstructor tree_constructor_;
 
  // Interface objects
  rclcpp_action::Server<StartActionContext::Type>::SharedPtr start_action_ptr_;
  StartActionContext start_action_context_;
  rclcpp_action::Server<CommandActionContext::Type>::SharedPtr command_action_ptr_;
  rclcpp::TimerBase::SharedPtr command_timer_ptr_;
};
 
}  // namespace auto_apms_behavior_tree