executor_server.hpp

Go to the documentation of this file.

// 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 "rclcpp/rclcpp.hpp"
#include "auto_apms_core/action_context.hpp"
#include "auto_apms_interfaces/action/start_tree_executor.hpp"
#include "auto_apms_interfaces/action/command_tree_executor.hpp"
#include "auto_apms_behavior_tree/executor/executor.hpp"
#include "auto_apms_behavior_tree/resource/tree_builder_class_loader.hpp"
 
namespace auto_apms_behavior_tree
{
 
class TreeExecutorServer : public TreeExecutor
{
public:
  using StartActionContext = auto_apms_core::ActionContext<auto_apms_interfaces::action::StartTreeExecutor>;
  using CommandActionContext = auto_apms_core::ActionContext<auto_apms_interfaces::action::CommandTreeExecutor>;
 
  static const std::string PARAM_NAME_TREE_BUILDER;
  static const std::string PARAM_NAME_TICK_RATE;
  static const std::string PARAM_NAME_GROOT2_PORT;
  static const std::string PARAM_NAME_STATE_CHANGE_LOGGER;
  inline static const std::string DEFAULT_NODE_NAME = "tree_executor";
 
  TreeExecutorServer(const std::string& name, const rclcpp::NodeOptions& options);
 
  TreeExecutorServer(const rclcpp::NodeOptions& options);
 
  CreateTreeCallback makeCreateTreeCallback(const std::string& tree_builder_name, const std::string& tree_build_request,
                                            const NodeManifest& node_overrides = {});
 
private:
  rcl_interfaces::msg::SetParametersResult
  on_set_parameters_callback_(const std::vector<rclcpp::Parameter>& parameters);
 
  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);
 
  bool onTick() override;
 
  void onTermination(const ExecutionResult& result) override;
 
  const rclcpp::Logger logger_;
  const executor_params::ParamListener executor_param_listener_;
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_;
  TreeBuilderClassLoader tree_build_director_loader_;
  CreateTreeCallback create_tree_callback_;
  rclcpp_action::Server<StartActionContext::Type>::SharedPtr start_action_ptr_;
  StartActionContext start_action_context_;
  rclcpp_action::Server<CommandActionContext::Type>::SharedPtr command_action_ptr_;
  CommandActionContext command_action_context_;
  rclcpp::TimerBase::SharedPtr command_timer_ptr_;
};
 
}  // namespace auto_apms_behavior_tree