mode.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <cstdint>
#include <px4_msgs/msg/mode_completed.hpp>
#include <px4_msgs/msg/setpoint_config.hpp>
#include <px4_msgs/msg/setpoint_config_reply.hpp>
#include <px4_msgs/msg/vehicle_status.hpp>
#include <px4_ros2/common/context.hpp>
#include <px4_ros2/common/setpoint_base.hpp>
#include <px4_ros2/components/shared_subscription.hpp>
#include <rclcpp/rclcpp.hpp>
 
#include "health_and_arming_checks.hpp"
#include "manual_control_input.hpp"
#include "overrides.hpp"
 
class Registration;
struct RegistrationSettings;
 
namespace px4_ros2 {
enum class Result {
  Success = 0,
  Rejected,     
  Interrupted,  
  Timeout,
  Deactivated,  
 
  // Mode-specific results
  ModeFailureOther = 100,
};
 
static_assert(static_cast<int>(Result::ModeFailureOther) ==
                  static_cast<int>(px4_msgs::msg::ModeCompleted::RESULT_FAILURE_OTHER),
              "definition mismatch");
 
constexpr inline const char* resultToString(Result result) noexcept
{
  switch (result) {
    case Result::Success:
      return "Success";
 
    case Result::Rejected:
      return "Rejected";
 
    case Result::Interrupted:
      return "Interrupted";
 
    case Result::Timeout:
      return "Timeout";
 
    case Result::Deactivated:
      return "Deactivated";
 
    case Result::ModeFailureOther:
      return "Mode Failure (generic)";
  }
 
  return "Unknown";
}
 
class ModeBase : public Context {
 public:
  using ModeID = uint8_t;  
  static constexpr ModeID kModeIDInvalid = 0xff;
 
  static constexpr ModeID kModeIDPosctl = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_POSCTL;
  static constexpr ModeID kModeIDTakeoff =
      px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_TAKEOFF;
  static constexpr ModeID kModeIDDescend = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND;
  static constexpr ModeID kModeIDLand = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_LAND;
  static constexpr ModeID kModeIDRtl = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_RTL;
  static constexpr ModeID kModeIDPrecisionLand =
      px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_PRECLAND;
  static constexpr ModeID kModeIDLoiter =
      px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_LOITER;
 
  struct Settings {
    // NOLINTNEXTLINE allow implicit conversion
    Settings(std::string mode_name) : name(std::move(mode_name)) {}
 
    const std::string name;  
    bool activate_even_while_disarmed{
        false};  
    ModeID replace_internal_mode{kModeIDInvalid};  
    bool prevent_arming{false};                    
    bool user_selectable{true};                    
 
    Settings& activateEvenWhileDisarmed(bool activate)
    {
      activate_even_while_disarmed = activate;
      return *this;
    }
    Settings& replaceInternalMode(ModeID mode)
    {
      replace_internal_mode = mode;
      return *this;
    }
    Settings& preventArming(bool prevent)
    {
      prevent_arming = prevent;
      return *this;
    }
    Settings& userSelectable(bool selectable)
    {
      user_selectable = selectable;
      return *this;
    }
  };
 
  ModeBase(rclcpp::Node& node, Settings settings, const std::string& topic_namespace_prefix = "");
  ModeBase(const ModeBase&) = delete;
  virtual ~ModeBase();
 
  bool doRegister();
 
  virtual void checkArmingAndRunConditions(HealthAndArmingCheckReporter& reporter) {}
 
  virtual void onActivate() {}
 
  virtual void onDeactivate() {}
 
  void setSetpointUpdateRate(float rate_hz);
 
  virtual void updateSetpoint(float dt_s) {}
 
  void completed(Result result);
 
  // Properties & state
 
  ModeID id() const;
 
  bool isArmed() const { return _is_armed; }
 
  bool isActive() const { return _is_active; }
 
  ConfigOverrides& configOverrides() { return _config_overrides; }
 
  RequirementFlags& modeRequirements() { return _health_and_arming_checks.modeRequirements(); }
 
 protected:
  void setSkipMessageCompatibilityCheck() { _skip_message_compatibility_check = true; }
  void setSkipSetpointCheck() { _skip_setpoint_check = true; }
  void overrideRegistration(const std::shared_ptr<Registration>& registration);
 
  void disableWatchdogTimer() { _health_and_arming_checks.disableWatchdogTimer(); }
 
  void setWatchdogTimeoutCallback(HealthAndArmingChecks::WatchdogTimeoutCallback callback)
  {
    _health_and_arming_checks.setWatchdogTimeoutCallback(std::move(callback));
  }
 
  bool defaultMessageCompatibilityCheck();
 
 private:
  void addSetpointType(SetpointBase* setpoint) override;
  void setRequirement(const RequirementFlags& requirement_flags) override;
 
  friend class ModeExecutorBase;
  RegistrationSettings getRegistrationSettings() const;
  void onAboutToRegister();
  bool onRegistered();
 
  void unsubscribeVehicleStatus();
  void vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr& msg,
                            bool do_not_activate = false);
 
  void callOnActivate();
  void callOnDeactivate();
 
  void updateSetpointUpdateTimer();
 
  void checkSetpointCompatibilityAndRequirements();
  void setSetpointUpdateRateFromSetpointTypes();
  void activateSetpointType(const std::shared_ptr<SetpointBase>& setpoint);
  void deactivateAllSetpointTypes();
 
  std::shared_ptr<Registration> _registration;
 
  const Settings _settings;
  bool _skip_message_compatibility_check{false};
  bool _skip_setpoint_check{false};  
 
  HealthAndArmingChecks _health_and_arming_checks;
 
  rclcpp::Publisher<px4_msgs::msg::ModeCompleted>::SharedPtr _mode_completed_pub;
  rclcpp::Publisher<px4_msgs::msg::SetpointConfig>::SharedPtr _setpoint_config_pub;
  SharedSubscriptionCallbackInstance _setpoint_config_reply_sub_cb;
 
  SharedSubscriptionCallbackInstance _vehicle_status_sub_cb;
 
  bool _is_active{false};  
  bool _is_armed{false};   
  bool _completed{false};  
 
  float _setpoint_update_rate_hz{0.f};
  rclcpp::TimerBase::SharedPtr _setpoint_update_timer;
  rclcpp::Time _last_setpoint_update{};
 
  ConfigOverrides _config_overrides;
 
  std::vector<std::shared_ptr<SetpointBase>> _setpoint_types;
  std::vector<SetpointBase*>
      _new_setpoint_types;  
  std::shared_ptr<SetpointBase> _current_activating_setpoint;  
};
 
}  // namespace px4_ros2