behavior_mode_executor.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
//
//      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 <chrono>
#include <functional>
#include <map>
#include <memory>
#include <string>
 
#include "auto_apms_behavior_tree/executor/generic_executor_node.hpp"
#include "auto_apms_interfaces/action/start_tree_executor.hpp"
#include "auto_apms_interfaces/srv/set_behavior.hpp"
#include "auto_apms_px4/mode_registration.hpp"
#include "auto_apms_px4/vehicle_command_client.hpp"
#include "auto_apms_util/action_context.hpp"
#include "px4_ros2/components/mode.hpp"
#include "px4_ros2/components/mode_executor.hpp"
#include "px4_ros2/control/setpoint_types/direct_actuators.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
 
namespace auto_apms_px4
{

struct BehaviorSpec
{
  std::string build_request;  
  std::string build_handler;  
  std::string entry_point;    
  std::string node_manifest;  
};

class BehaviorOwnedMode : public px4_ros2::ModeBase
{
public:
  BehaviorOwnedMode(rclcpp::Node & node, const px4_ros2::ModeBase::Settings & settings);
 
  void onActivate() override {}
  void onDeactivate() override {}
  void updateSetpoint(float dt_s) override;

  void finish(px4_ros2::Result result) { completed(result); }
 
private:
  std::shared_ptr<px4_ros2::DirectActuatorsSetpointType> actuator_setpoint_ptr_;
};

class BehaviorModeExecutor : public px4_ros2::ModeExecutorBase
{
public:
  using ExecutionResult = auto_apms_behavior_tree::TreeExecutorBase::ExecutionResult;

  enum class CompletionReaction
  {
    HOLD,      
    RTL,       
    LAND,      
    DISARM,    
    COMPLETE,  
    NONE       
  };
 
  struct Config
  {
    CompletionReaction on_completion{CompletionReaction::HOLD};  
    CompletionReaction on_failure{CompletionReaction::RTL};      
    bool defer_failsafes{false};  
  };

  static constexpr std::chrono::milliseconds DEFAULT_ACTIVATION_TIMEOUT{5000};

  BehaviorModeExecutor(
    BehaviorOwnedMode & owned_mode, const px4_ros2::ModeExecutorBase::Settings & settings,
    std::function<Config()> config_provider, BehaviorSpec initial_spec,
    auto_apms_behavior_tree::GenericTreeExecutorNode & engine);
 
  void onActivate() override;
  void onDeactivate(DeactivateReason reason) override;

  void prepareTree();

  bool setBehavior(const BehaviorSpec & candidate, std::string & message);

  bool requestAndVerifyActivation(std::chrono::milliseconds timeout = DEFAULT_ACTIVATION_TIMEOUT);

  static CompletionReaction reactionFromString(const std::string & str);

  void onExecutionResult(ExecutionResult result);
 
private:
  std::unique_ptr<auto_apms_behavior_tree::Tree> buildTree(const BehaviorSpec & spec);
 
  void performReaction(CompletionReaction reaction, ExecutionResult result);

  void scheduleLoiter();
 
  rclcpp::Node & node_;
  BehaviorOwnedMode & owned_mode_;
  auto_apms_behavior_tree::GenericTreeExecutorNode & behavior_executor_;
  std::function<Config()> config_provider_;
  Config config_;              
  BehaviorSpec current_spec_;  
  VehicleCommandClient vehicle_command_client_;  
  std::unique_ptr<auto_apms_behavior_tree::Tree> prepared_tree_ptr_;
};

class BehaviorModeExecutorNode : public auto_apms_behavior_tree::GenericTreeExecutorNode
{
public:
  using StartAction = auto_apms_interfaces::action::StartTreeExecutor;
  using StartActionContext = auto_apms_util::ActionContext<StartAction>;
  using StartGoalHandle = rclcpp_action::ServerGoalHandle<StartAction>;
  using SetBehaviorSrv = auto_apms_interfaces::srv::SetBehavior;
 
  explicit BehaviorModeExecutorNode(const rclcpp::NodeOptions & options);
 
protected:
  void onTermination(const ExecutionResult & result) override;
 
private:
  static px4_ros2::ModeExecutorBase::Settings::Activation activationFromString(const std::string & str);

  bool applyBehavior(BehaviorSpec candidate, std::string & message);
 
  /* set_behavior service */
  void handleSetBehavior(
    const std::shared_ptr<SetBehaviorSrv::Request> request, std::shared_ptr<SetBehaviorSrv::Response> response);
 
  /* StartTreeExecutor action */
  rclcpp_action::GoalResponse handleStartGoal(
    const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const StartAction::Goal> goal_ptr);
  rclcpp_action::CancelResponse handleStartCancel(std::shared_ptr<StartGoalHandle> goal_handle_ptr);
  void handleStartAccept(std::shared_ptr<StartGoalHandle> goal_handle_ptr);
 
  std::unique_ptr<BehaviorOwnedMode> owned_mode_ptr_;
  std::unique_ptr<BehaviorModeExecutor> mode_executor_ptr_;
  ModeRegistrationHandler registration_handler_;
 
  rclcpp::Service<SetBehaviorSrv>::SharedPtr set_behavior_service_ptr_;
  rclcpp_action::Server<StartAction>::SharedPtr start_action_ptr_;
  StartActionContext start_action_context_;
  std::map<rclcpp_action::GoalUUID, bool> pending_attach_;
  std::map<rclcpp_action::GoalUUID, bool> pending_clear_blackboard_;
};
 
}  // namespace auto_apms_px4