mode_executor.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <functional>
#include <px4_msgs/msg/mode_completed.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_command_ack.hpp>
#include <px4_msgs/msg/vehicle_status.hpp>
#include <px4_ros2/components/shared_subscription.hpp>
#include <rclcpp/rclcpp.hpp>
 
#include "mode.hpp"
#include "overrides.hpp"
 
class Registration;
 
namespace px4_ros2 {
class ModeExecutorBase {
 public:
  using CompletedCallback = std::function<void(Result)>;
 
  struct Settings {
    enum class Activation {
      ActivateOnlyWhenArmed,  
      ActivateAlways,  
      ActivateImmediately,  
    };
    Activation activation{Activation::ActivateOnlyWhenArmed};
 
    Settings& activate(Activation activation_option)
    {
      activation = activation_option;
      return *this;
    }
  };
 
  enum class DeactivateReason { FailsafeActivated, Other };
 
  ModeExecutorBase(const Settings& settings, ModeBase& owned_mode);
  ModeExecutorBase(const ModeExecutorBase&) = delete;
  virtual ~ModeExecutorBase();
 
  bool doRegister();
 
  virtual void onActivate() = 0;
 
  virtual void onDeactivate(DeactivateReason reason) = 0;
 
  virtual void onFailsafeDeferred() {}
 
  // NOLINTNEXTLINE(google-default-arguments)
  virtual Result sendCommandSync(uint32_t command, float param1 = NAN, float param2 = NAN,
                                 float param3 = NAN, float param4 = NAN, float param5 = NAN,
                                 float param6 = NAN, float param7 = NAN);
 
  void scheduleMode(ModeBase::ModeID mode_id, const CompletedCallback& on_completed,
                    bool forced = false);
 
  void takeoff(const CompletedCallback& on_completed, float altitude = NAN, float heading = NAN);
  void land(const CompletedCallback& on_completed);
  void rtl(const CompletedCallback& on_completed);
 
  void arm(const CompletedCallback& on_completed, bool run_preflight_checks = true);
  void disarm(const CompletedCallback& on_completed, bool forced = false);
  void waitReadyToArm(const CompletedCallback& on_completed);
  void waitUntilDisarmed(const CompletedCallback& on_completed);
 
  bool isInCharge() const { return _is_in_charge; }
 
  bool isArmed() const { return _is_armed; }
 
  ModeBase& ownedMode() { return _owned_mode; }
 
  int id() const;
 
  rclcpp::Node& node() { return _node; }
 
  ConfigOverrides& configOverrides() { return _config_overrides; }
 
  bool deferFailsafesSync(bool enabled, int timeout_s = 0);
 
  bool controlAutoSetHome(bool enabled);
 
 protected:
  void setSkipMessageCompatibilityCheck() { _skip_message_compatibility_check = true; }
  void overrideRegistration(const std::shared_ptr<Registration>& registration);
 
 private:
  class ScheduledMode {
   public:
    ScheduledMode(rclcpp::Node& node, const std::string& topic_namespace_prefix);
 
    bool active() const { return _mode_id != ModeBase::kModeIDInvalid; }
    void activate(ModeBase::ModeID mode_id, const CompletedCallback& on_completed);
    void cancel();
    ModeBase::ModeID modeId() const { return _mode_id; }
 
   private:
    void reset() { _mode_id = ModeBase::kModeIDInvalid; }
 
    ModeBase::ModeID _mode_id{ModeBase::kModeIDInvalid};
    CompletedCallback _on_completed_callback;
    rclcpp::Subscription<px4_msgs::msg::ModeCompleted>::SharedPtr _mode_completed_sub;
  };
 
  class WaitForVehicleStatusCondition {
   public:
    using RunCheckCallback =
        std::function<bool(const px4_msgs::msg::VehicleStatus::UniquePtr& msg)>;
    WaitForVehicleStatusCondition() = default;
 
    bool active() const { return _on_completed_callback != nullptr; }
    void update(const px4_msgs::msg::VehicleStatus::UniquePtr& msg);
 
    void activate(const RunCheckCallback& run_check_callback,
                  const CompletedCallback& on_completed);
    void cancel();
 
   private:
    CompletedCallback _on_completed_callback;
    RunCheckCallback _run_check_callback;
  };
 
  void onRegistered();
 
  void callOnActivate();
  void callOnDeactivate(DeactivateReason reason);
 
  void vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr& msg);
 
  void scheduleMode(ModeBase::ModeID mode_id, const px4_msgs::msg::VehicleCommand& cmd,
                    const ModeExecutorBase::CompletedCallback& on_completed, bool forced = false);
 
  rclcpp::Node& _node;
  const std::string _topic_namespace_prefix;
  const Settings _settings;
  bool _skip_message_compatibility_check{false};
  ModeBase& _owned_mode;
 
  std::shared_ptr<Registration> _registration;
 
  rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr _vehicle_command_pub;
 
  SharedSubscriptionCallbackInstance _vehicle_status_sub_cb;
 
  ScheduledMode _current_scheduled_mode;
  WaitForVehicleStatusCondition _current_wait_vehicle_status;
 
  bool _is_in_charge{false};
  bool _is_armed{false};
  bool _was_never_activated{true};
  ModeBase::ModeID _prev_nav_state{ModeBase::kModeIDInvalid};
  uint8_t _prev_failsafe_defer_state{px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED};
 
  ConfigOverrides _config_overrides;
};
 
}  // namespace px4_ros2