mission_executor.hpp

/****************************************************************************
 * Copyright (c) 2024 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
#include <memory>
#include <px4_ros2/components/mode.hpp>
#include <px4_ros2/components/mode_executor.hpp>
#include <px4_ros2/mission/actions/action.hpp>
#include <px4_ros2/mission/mission.hpp>
#include <px4_ros2/mission/trajectory/trajectory_executor.hpp>
#include <px4_ros2/vehicle_state/land_detected.hpp>
#include <vector>
 
namespace px4_ros2 {
class AsyncFunctionCalls;
class ActionStateKeeper;
 
class MissionExecutor {
 public:
  struct Configuration {
    std::vector<std::function<std::shared_ptr<ActionInterface>(ModeBase& mode)>>
        custom_actions_factory;
    std::set<std::string> default_actions{
        "changeSettings", "onResume", "onFailure", "takeoff", "rtl", "land", "hold"};
    std::function<std::shared_ptr<TrajectoryExecutorInterface>(ModeBase& mode)>
        trajectory_executor_factory;
    std::string persistence_filename;
 
    template <class ActionType, typename... Args>
    Configuration& addCustomAction(Args&&... args)
    {
      custom_actions_factory.push_back(
          [args = std::tie(std::forward<Args>(args)...)](ModeBase& mode) mutable {
            return std::apply(
                [&mode](auto&&... args) {
                  return std::make_shared<ActionType>(mode, std::forward<Args>(args)...);
                },
                std::move(args));
          });
      return *this;
    }
    template <typename TrajectoryExecutorType, typename... Args>
    Configuration& withTrajectoryExecutor(Args&&... args)
    {
      trajectory_executor_factory = [args = std::tie(std::forward<Args>(args)...)](
                                        ModeBase& mode) mutable {
        return std::apply(
            [&mode](auto&&... args) {
              return std::make_shared<TrajectoryExecutorType>(mode, std::forward<Args>(args)...);
            },
            std::move(args));
      };
      return *this;
    }
    Configuration& withPersistenceFile(const std::string& filename)
    {
      persistence_filename = filename;
      return *this;
    }
  };
 
  MissionExecutor(const std::string& mode_name, const Configuration& configuration,
                  rclcpp::Node& node, const std::string& topic_namespace_prefix = "");
 
  virtual ~MissionExecutor();
 
  bool doRegister();
 
  void setMission(const Mission& mission);
  void resetMission();
 
  const Mission& mission() const { return *_mission; }
 
  void onActivated(const std::function<void()>& callback) { _on_activated = callback; }
  void onDeactivated(const std::function<void()>& callback) { _on_deactivated = callback; }
  void onProgressUpdate(const std::function<void(int)>& callback)
  {
    _on_progress_update = callback;
  }
  void onCompleted(const std::function<void()>& callback) { _on_completed = callback; }
  void onFailsafeDeferred(const std::function<void()>& callback);
  void onReadynessUpdate(
      const std::function<void(bool ready, const std::vector<std::string>& errors)>& callback);
 
  void onActivityInfoChange(const std::function<void(const std::optional<std::string>&)>& callback)
  {
    _on_activity_info_change = callback;
  }
 
  void setActivityInfo(const std::optional<std::string>& activity_info);
  bool deferFailsafes(bool enabled, int timeout_s = 0);
 
  bool controlAutoSetHome(bool enabled);
 
  void abort();
 
  void setSkipMessageCompatibilityCheck() { _mode_executor->setSkipMessageCompatibilityCheck(); }
 
 protected:
  class MissionMode : public ModeBase {
   public:
    MissionMode(rclcpp::Node& node, const Settings& settings,
                const std::string& topic_namespace_prefix, MissionExecutor& mission_executor)
        : ModeBase(node, settings, topic_namespace_prefix), _mission_executor(mission_executor)
    {
    }
 
    explicit MissionMode(const ModeBase& mode_base) = delete;
 
    ~MissionMode() override = default;
    void checkArmingAndRunConditions(HealthAndArmingCheckReporter& reporter) override
    {
      _mission_executor.checkArmingAndRunConditions(reporter);
    }
    void updateSetpoint(float dt_s) override { _mission_executor.updateSetpoint(); }
 
    void disableWatchdogTimer()  // NOLINT we just want to change the methods visibility
    {
      ModeBase::disableWatchdogTimer();
    }
 
   private:
    MissionExecutor& _mission_executor;
  };
 
  class MissionModeExecutor : public ModeExecutorBase {
   public:
    MissionModeExecutor(rclcpp::Node& node, const Settings& settings, ModeBase& owned_mode,
                        const std::string& topic_namespace_prefix,
                        MissionExecutor& mission_executor)
        : ModeExecutorBase(settings, owned_mode), _mission_executor(mission_executor)
    {
    }
 
    explicit MissionModeExecutor(const ModeExecutorBase& mode_executor_base) = delete;
 
    ~MissionModeExecutor() override = default;
    void onActivate() override { _mission_executor.onActivate(); }
    void onDeactivate(DeactivateReason reason) override { _mission_executor.onDeactivate(reason); }
    void onFailsafeDeferred() override
    {
      if (on_failsafe_deferred) {
        on_failsafe_deferred();
      }
    }
 
    Result sendCommandSync(uint32_t command, float param1, float param2, float param3, float param4,
                           float param5, float param6, float param7) override
    {
      if (command_handler) {
        return command_handler(command, param1) ? Result::Success : Result::Rejected;
      }
      return ModeExecutorBase::sendCommandSync(command, param1, param2, param3, param4, param5,
                                               param6, param7);
    }
 
    void setRegistration(const std::shared_ptr<Registration>& registration)
    {
      setSkipMessageCompatibilityCheck();
      overrideRegistration(registration);
    }
 
    void setSkipMessageCompatibilityCheck()
    {
      ModeExecutorBase::setSkipMessageCompatibilityCheck();
    }
 
    std::function<bool(uint32_t, float)> command_handler{nullptr};
    std::function<void()> on_failsafe_deferred{nullptr};
 
   private:
    MissionExecutor& _mission_executor;
  };
 
  virtual bool doRegisterImpl(MissionMode& mode, MissionModeExecutor& executor_base);
 
  void setCommandHandler(const std::function<bool(uint32_t, float)>& command_handler)
  {
    _mode_executor->command_handler = command_handler;
  }
 
  ModeBase::ModeID modeId() const { return _mode->id(); }
 
  ModeExecutorBase& modeExecutor() { return *_mode_executor; }
 
  std::shared_ptr<LandDetected> _land_detected;
 
 private:
  using ActionID = int;
  enum class AbortReason {
    Other,
    ModeFailure,
    TrajectoryFailure,
    NoValidMission,
    ActionDoesNotExist,
  };
  static std::string abortReasonStr(AbortReason reason);
 
  void checkArmingAndRunConditions(HealthAndArmingCheckReporter& reporter);
  void checkReadynessAndReport();
  void updateSetpoint();
  void onActivate();
  void onDeactivate(ModeExecutorBase::DeactivateReason reason);
 
  bool isReady() const { return _has_valid_mission && _actions_ready; }
 
  void runMode(ModeBase::ModeID mode_id, const std::function<void()>& on_completed,
               const std::function<void()>& on_failure = nullptr);
  void runModeTakeoff(float altitude, float heading, const std::function<void()>& on_completed,
                      const std::function<void()>& on_failure = nullptr);
  void runAction(const std::string& action_name, const ActionArguments& arguments,
                 const std::function<void()>& on_completed);
  void runTrajectory(const std::shared_ptr<Mission>& trajectory, int start_index, int end_index,
                     const std::function<void(int)>& on_index_reached, bool stop_at_last_item);
 
  void runNextMissionItem();
  void runCurrentMissionItem(bool resuming);
  void resetMissionState();
  std::pair<int, bool> getNextTrajectorySegment(int start_index) const;
  bool currentActionSupportsResumeFromLanded() const;
 
  void setTrajectoryOptions(const TrajectoryOptions& options);
  void clearTrajectoryOptions();
 
  void setCurrentMissionIndex(int index);
 
  void abort(AbortReason reason);
  void invalidateActionHandler();
 
  void savePersistentState();
  void clearPersistentState() const;
  bool tryLoadPersistentState();
  nlohmann::json getOnResumeStateAndClear();
  void runOnResumeStoreState();
 
  struct ActionState {
    std::string name;
    ActionArguments arguments;
  };
  std::unique_ptr<ActionStateKeeper> addContinousAction(const ActionState& state);
  void removeContinousAction(ActionID id);
  void runStoredActions();
  void deactivateAllActions();
 
  struct PersistentState {
    std::optional<int> current_index;
    std::string mission_checksum;
 
    std::map<ActionID, ActionState> continuous_actions;
 
    void toJson(nlohmann::json& j) const;
    void fromJson(const nlohmann::json& j);
  };
 
  PersistentState _state;
  int _next_continuous_action_id{0};
  const std::string _persistence_filename;
 
  enum class MissionItemState {
    Trajectory,  
    Other,
  };
  MissionItemState _mission_item_state{MissionItemState::Other};
  ModeBase::ModeID _active_mode{};
  bool _is_active{false};
 
  std::optional<rclcpp::Time> _trajectory_update_warn;
 
  std::shared_ptr<TrajectoryExecutorInterface> _trajectory_executor;
  std::map<std::string, std::shared_ptr<ActionInterface>> _actions;
  std::shared_ptr<Mission> _mission;
  TrajectoryOptions _trajectory_options;  
  bool _has_valid_mission{false};
  static const std::vector<std::string> kNoMissionErrors;
  std::vector<std::string> _mission_errors{kNoMissionErrors};
  bool _actions_ready{false};
  rclcpp::TimerBase::SharedPtr _readyness_timer;
  int _abort_recursion_level{
      0};  
 
  rclcpp::Node& _node;
  std::unique_ptr<MissionMode> _mode;
  std::unique_ptr<MissionModeExecutor> _mode_executor;
  std::shared_ptr<ActionHandler> _action_handler;
 
  std::unique_ptr<AsyncFunctionCalls> _reporting;  // Report asynchronously to avoid potential
                                                   // recursive calls and state changes in between
  std::function<void()> _on_activated;
  std::function<void()> _on_deactivated;
  std::function<void(int)> _on_progress_update;
  std::function<void()> _on_completed;
  std::function<void(const std::optional<std::string>&)> _on_activity_info_change;
  std::function<void(bool ready, const std::vector<std::string>& errors)> _on_readyness_update;
 
  friend class ActionHandler;
  friend class ActionStateKeeper;
};
 
class ActionStateKeeper {
 public:
  ActionStateKeeper(int id, MissionExecutor& mission_executor)
      : _id(id), _mission_executor(mission_executor)
  {
  }
 
  ~ActionStateKeeper() { _mission_executor.removeContinousAction(_id); }
 
 private:
  const int _id;
  MissionExecutor& _mission_executor;
};
 
class ActionHandler {
 public:
  explicit ActionHandler(MissionExecutor& mission_executor) : _mission_executor(mission_executor) {}
 
  void runMode(ModeBase::ModeID mode_id, const std::function<void()>& on_completed,
               const std::function<void()>& on_failure = nullptr)
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return;
    }
    _mission_executor.runMode(mode_id, on_completed, on_failure);
  }
  void runModeTakeoff(float altitude, float heading, const std::function<void()>& on_completed,
                      const std::function<void()>& on_failure = nullptr)
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return;
    }
    _mission_executor.runModeTakeoff(altitude, heading, on_completed, on_failure);
  }
  void runAction(const std::string& action_name, const ActionArguments& arguments,
                 const std::function<void()>& on_completed)
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return;
    }
    _mission_executor.runAction(action_name, arguments, on_completed);
  }
  void runTrajectory(const std::shared_ptr<Mission>& trajectory,
                     const std::function<void()>& on_completed, bool stop_at_last_item = true);
 
  TrajectoryOptions getTrajectoryOptions() const { return _mission_executor._trajectory_options; }
  void clearTrajectoryOptions()
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return;
    }
    _mission_executor.clearTrajectoryOptions();
  }
  void setTrajectoryOptions(const TrajectoryOptions& options)
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return;
    }
    _mission_executor.setTrajectoryOptions(options);
  }
 
  void setActvityInfo(const std::string& activity_info)
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return;
    }
    _mission_executor.setActivityInfo(activity_info);
  }
 
  void clearActivityInfo()
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return;
    }
    _mission_executor.setActivityInfo(std::nullopt);
  }
 
  std::optional<int> getCurrentMissionIndex() const;
  void setCurrentMissionIndex(int index);
 
  bool currentActionSupportsResumeFromLanded() const;
 
  std::unique_ptr<ActionStateKeeper> storeState(const std::string& action_name,
                                                const ActionArguments& arguments)
  {
    if (!_valid) {
      return nullptr;
    }
    return _mission_executor.addContinousAction(
        MissionExecutor::ActionState{action_name, arguments});
  }
 
  const Mission& mission() const { return _mission_executor.mission(); }
 
  bool deferFailsafes(bool enabled, int timeout_s = 0)
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return false;
    }
    return _mission_executor.deferFailsafes(enabled, timeout_s);
  }
 
  bool controlAutoSetHome(bool enabled)
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return false;
    }
    return _mission_executor.controlAutoSetHome(enabled);
  }
 
  void onFailsafeDeferred(const std::function<void()>& callback)
  {
    if (!_valid) {
      RCLCPP_WARN(_mission_executor._node.get_logger(), "ActionHandler is not valid anymore");
      return;
    }
    _mission_executor.onFailsafeDeferred(callback);
  }
 
  bool isValid() const { return _valid; }
 
 private:
  friend class MissionExecutor;
  void setInvalid() { _valid = false; }
 
  bool _valid{true};
  MissionExecutor& _mission_executor;
};
 
} /* namespace px4_ros2 */