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 <functional>
#include <memory>
#include <string>
 
#include "auto_apms_behavior_tree/executor/generic_executor_node.hpp"
#include "auto_apms_px4/mode_registration.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"
 
namespace auto_apms_px4
{

struct BehaviorSpec
{
  std::string build_request;  
  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
  {
    BehaviorSpec spec;                                           
    CompletionReaction on_completion{CompletionReaction::HOLD};  
    CompletionReaction on_failure{CompletionReaction::RTL};      
    bool defer_failsafes{false};  
  };

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

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

  void onExecutionResult(ExecutionResult result);
 
private:
  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_;  
};

class BehaviorModeExecutorNode : public auto_apms_behavior_tree::GenericTreeExecutorNode
{
public:
  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);
 
  std::unique_ptr<BehaviorOwnedMode> owned_mode_ptr_;
  std::unique_ptr<BehaviorModeExecutor> executor_ptr_;
  ModeRegistrationHandler registration_handler_;
};
 
}  // namespace auto_apms_px4