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 <optional>
 
#include "auto_apms_px4/mode.hpp"
#include "auto_apms_px4/vehicle_command_client.hpp"
#include "auto_apms_util/action_wrapper.hpp"
#include "px4_msgs/msg/mode_completed.hpp"
#include "px4_msgs/msg/vehicle_status.hpp"
#include "px4_ros2/components/wait_for_fmu.hpp"
#include "px4_ros2/utils/message_version.hpp"


namespace auto_apms_px4
{

template <class ActionT>
class ModeExecutor : public auto_apms_util::ActionWrapper<ActionT>
{
  enum class State : uint8_t
  {
    REQUEST_ACTIVATION,
    WAIT_FOR_ACTIVATION,
    WAIT_FOR_COMPLETION_SIGNAL,
    COMPLETE
  };
 
public:
  using VehicleCommandClient = auto_apms_px4::VehicleCommandClient;
  using FlightMode = VehicleCommandClient::FlightMode;
  using typename auto_apms_util::ActionWrapper<ActionT>::ActionContextType;
  using typename auto_apms_util::ActionWrapper<ActionT>::Goal;
  using typename auto_apms_util::ActionWrapper<ActionT>::Feedback;
  using typename auto_apms_util::ActionWrapper<ActionT>::Result;
  using ActionStatus = auto_apms_util::ActionStatus;
 
  explicit ModeExecutor(
    const std::string & action_name, rclcpp::Node::SharedPtr node_ptr,
    std::shared_ptr<ActionContextType> action_context_ptr, uint8_t mode_id,
    FlightMode deactivation_flight_mode = FlightMode::Hold, bool disarm_after_completion = false);
  explicit ModeExecutor(
    const std::string & action_name, const rclcpp::NodeOptions & options, uint8_t mode_id,
    FlightMode deactivation_flight_mode = FlightMode::Hold, bool disarm_after_completion = false);
  explicit ModeExecutor(
    const std::string & action_name, const rclcpp::NodeOptions & options, FlightMode flight_mode,
    FlightMode deactivation_flight_mode = FlightMode::Hold, bool disarm_after_completion = false);
 
  static bool isExternalMode(uint8_t mode_id);
 
private:
  void setUp();
  auto_apms_util::ActionStatus asyncDeactivateFlightMode();
  bool onGoalRequest(std::shared_ptr<const Goal> goal_ptr) override final;
  bool onCancelRequest(std::shared_ptr<const Goal> goal_ptr, std::shared_ptr<Result> result_ptr) override final;
  auto_apms_util::ActionStatus cancelGoal(
    std::shared_ptr<const Goal> goal_ptr, std::shared_ptr<Result> result_ptr) override final;
  auto_apms_util::ActionStatus executeGoal(
    std::shared_ptr<const Goal> goal_ptr, std::shared_ptr<Feedback> feedback_ptr,
    std::shared_ptr<Result> result_ptr) override final;
 
protected:
  bool isCurrentNavState(uint8_t nav_state);
  virtual bool sendActivationCommand(const VehicleCommandClient & client, std::shared_ptr<const Goal> goal_ptr);
  virtual bool isCompleted(std::shared_ptr<const Goal> goal_ptr, const px4_msgs::msg::VehicleStatus & vehicle_status);
  virtual void setFeedback(std::shared_ptr<Feedback> feedback_ptr, const px4_msgs::msg::VehicleStatus & vehicle_status);
 
  uint8_t getModeID() const;
 
private:
  const VehicleCommandClient vehicle_command_client_;
  const uint8_t mode_id_;
  bool is_custom_mode_{false};  // Whether the mode is a custom mode (i.e. not one of the standard PX4 modes)
  FlightMode deactivation_flight_mode_;
  bool disarm_after_completion_;
  rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_sub_ptr_;
  rclcpp::Subscription<px4_msgs::msg::ModeCompleted>::SharedPtr mode_completed_sub_ptr_;
  px4_msgs::msg::VehicleStatus::SharedPtr last_vehicle_status_ptr_;
  std::optional<uint8_t> mode_completed_result_;
  bool deactivation_command_sent_{false};
  State state_{State::REQUEST_ACTIVATION};
  rclcpp::Time activation_command_sent_time_;
  rclcpp::Duration activation_timeout_{0, 0};
};

template <class ActionT, class ModeT>
class ModeExecutorFactory
{
public:
  ModeExecutorFactory(
    const std::string & action_name, const rclcpp::NodeOptions & options,
    VehicleCommandClient::FlightMode deactivation_flight_mode = VehicleCommandClient::FlightMode::Hold,
    bool disarm_after_completion = false);
 
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface();
 
private:
  rclcpp::Node::SharedPtr node_ptr_;  // It's necessary to also store the node pointer here for successful destruction
  std::unique_ptr<ModeBase<ActionT>> mode_ptr_;
  std::shared_ptr<ModeExecutor<ActionT>> mode_executor_ptr_;
};
 
// #####################################################################################################################
// ################################              DEFINITIONS              ##############################################
// #####################################################################################################################
 
template <class ActionT>
ModeExecutor<ActionT>::ModeExecutor(
  const std::string & action_name, rclcpp::Node::SharedPtr node_ptr,
  std::shared_ptr<ActionContextType> action_context_ptr, uint8_t mode_id, FlightMode deactivation_flight_mode,
  bool disarm_after_completion)
: auto_apms_util::ActionWrapper<ActionT>(action_name, node_ptr, action_context_ptr),
  vehicle_command_client_(*node_ptr),
  mode_id_(mode_id),
  deactivation_flight_mode_(deactivation_flight_mode),
  disarm_after_completion_(disarm_after_completion)
{
  setUp();
}
 
template <class ActionT>
ModeExecutor<ActionT>::ModeExecutor(
  const std::string & action_name, const rclcpp::NodeOptions & options, uint8_t mode_id,
  FlightMode deactivation_flight_mode, bool disarm_after_completion)
: auto_apms_util::ActionWrapper<ActionT>(action_name, options),
  vehicle_command_client_(*this->node_ptr_),
  mode_id_(mode_id),
  is_custom_mode_(isExternalMode(mode_id_)),
  deactivation_flight_mode_(deactivation_flight_mode),
  disarm_after_completion_(disarm_after_completion)
{
  setUp();
}
 
template <class ActionT>
ModeExecutor<ActionT>::ModeExecutor(
  const std::string & action_name, const rclcpp::NodeOptions & options, FlightMode flight_mode,
  FlightMode deactivation_flight_mode, bool disarm_after_completion)
: ModeExecutor<ActionT>(
    action_name, options, static_cast<uint8_t>(flight_mode), deactivation_flight_mode, disarm_after_completion)
{
}
 
template <class ActionT>
inline bool ModeExecutor<ActionT>::isExternalMode(uint8_t mode_id)
{
  const uint8_t first_external_mode_nav_state = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_EXTERNAL1;
  const uint8_t max_external_mode_nav_state = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_EXTERNAL8 + 1;
  return mode_id >= first_external_mode_nav_state && mode_id < max_external_mode_nav_state;
}
 
template <class ActionT>
void ModeExecutor<ActionT>::setUp()
{
  vehicle_status_sub_ptr_ = this->node_ptr_->template create_subscription<px4_msgs::msg::VehicleStatus>(
    "fmu/out/vehicle_status" + px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleStatus>(),
    rclcpp::QoS(1).best_effort(),
    [this](px4_msgs::msg::VehicleStatus::UniquePtr msg) { last_vehicle_status_ptr_ = std::move(msg); });
 
  mode_completed_sub_ptr_ = this->node_ptr_->template create_subscription<px4_msgs::msg::ModeCompleted>(
    "fmu/out/mode_completed" + px4_ros2::getMessageNameVersion<px4_msgs::msg::ModeCompleted>(),
    rclcpp::QoS(1).best_effort(), [this](px4_msgs::msg::ModeCompleted::UniquePtr msg) {
      bool is_relevant = msg->nav_state == this->mode_id_;
      RCLCPP_DEBUG(
        this->node_ptr_->get_logger(),
        "Flight mode completion signal received by mode %i. Signal was: timestamp=%li, mode_id=%i, result=%i (is "
        "relevant for this mode: %i)",
        this->mode_id_, msg->timestamp, msg->nav_state, msg->result, is_relevant);
      if (is_relevant) {
        this->mode_completed_result_ = msg->result;
      }
    });
}
 
template <class ActionT>
auto_apms_util::ActionStatus ModeExecutor<ActionT>::asyncDeactivateFlightMode()
{
  // If currently waiting for flight mode activation and the deactivation mode is already active, we need to wait for
  // the nav state to change before starting deactivation. Otherwise, we'll misinterpret the current nav state and
  // return success immediately.
  bool is_deactivation_mode_active = isCurrentNavState(static_cast<uint8_t>(deactivation_flight_mode_));
  if (state_ == State::WAIT_FOR_ACTIVATION) {
    if (is_deactivation_mode_active) {
      auto & clock = *this->node_ptr_->get_clock();
      RCLCPP_DEBUG_THROTTLE(
        this->node_ptr_->get_logger(), clock, 250, "Waiting for flight mode %i to become active before deactivating...",
        mode_id_);
      return ActionStatus::RUNNING;
    } else {
      state_ = State::COMPLETE;  // Change state to indicate that mode has been activated
    }
  }
 
  if (is_deactivation_mode_active) {
    RCLCPP_DEBUG(
      this->node_ptr_->get_logger(), "Deactivated flight mode successfully (deactivation mode %i is active)",
      static_cast<int>(deactivation_flight_mode_));
    return ActionStatus::SUCCESS;
  } else {
    // Only send command if not in deactivation mode already
    if (!deactivation_command_sent_) {
      if (!vehicle_command_client_.syncActivateFlightMode(deactivation_flight_mode_)) {
        RCLCPP_ERROR(
          this->node_ptr_->get_logger(), "Failed to send command to activate deactivation flight mode %i",
          static_cast<int>(deactivation_flight_mode_));
        return ActionStatus::FAILURE;
      }
      // Force to consider only new status messages after sending new command
      last_vehicle_status_ptr_ = nullptr;
      deactivation_command_sent_ = true;
    }
  }
 
  return ActionStatus::RUNNING;
}
 
template <class ActionT>
bool ModeExecutor<ActionT>::onGoalRequest(const std::shared_ptr<const Goal> /*goal_ptr*/)
{
  state_ = State::REQUEST_ACTIVATION;
  deactivation_command_sent_ = false;
  mode_completed_result_ = std::nullopt;
  activation_timeout_ = rclcpp::Duration::from_seconds(fmin(this->param_listener_.get_params().loop_rate * 15, 1.5));
  return true;
}
 
template <class ActionT>
bool ModeExecutor<ActionT>::onCancelRequest(
  std::shared_ptr<const Goal> /*goal_ptr*/, std::shared_ptr<Result> /*result_ptr*/)
{
  RCLCPP_INFO(this->node_ptr_->get_logger(), "Cancellation requested!");
  return true;
}
 
template <class ActionT>
auto_apms_util::ActionStatus ModeExecutor<ActionT>::cancelGoal(
  std::shared_ptr<const Goal> /*goal_ptr*/, std::shared_ptr<Result> /*result_ptr*/)
{
  // The custom mode is responsible for managing the lifecycle of the cancellation via the onGoalCanceled callback.
  // Wait until the mode signals completion (by calling completed()) or PX4 deactivates it externally. For standard
  // modes, continue immediately with deactivation since we can't rely on the mode_completed topic for standard modes as
  // they usually don't publish to it.
  if (is_custom_mode_ && state_ != State::COMPLETE) {
    // During cancellation, we bypass the custom isCompleted method and simply wait for any mode completion signal
    // regardless the value
    if (mode_completed_result_.has_value()) {
      state_ = State::COMPLETE;
    } else {
      if (!isCurrentNavState(mode_id_)) {
        RCLCPP_WARN(
          this->node_ptr_->get_logger(), "Flight mode %i was deactivated externally during cancellation", mode_id_);
        // In this case we don't have to do anything afterwards
        return ActionStatus::SUCCESS;
      }
      return ActionStatus::RUNNING;
    }
  }
 
  ActionStatus ret = ActionStatus::SUCCESS;
  if (deactivation_flight_mode_ != FlightMode::Unset) {
    ret = asyncDeactivateFlightMode();
  }
 
  if (ret != ActionStatus::RUNNING) {
    if (disarm_after_completion_ && !vehicle_command_client_.disarm()) {
      RCLCPP_WARN(this->node_ptr_->get_logger(), "Failed to disarm after flight mode %i cancellation", mode_id_);
    }
    RCLCPP_INFO(this->node_ptr_->get_logger(), "Flight mode %i cancellation complete", mode_id_);
  }
  return ret;
}
 
template <class ActionT>
bool ModeExecutor<ActionT>::isCurrentNavState(uint8_t nav_state)
{
  if (last_vehicle_status_ptr_ && last_vehicle_status_ptr_->nav_state == nav_state) {
    return true;
  }
  return false;
}
 
template <class ActionT>
auto_apms_util::ActionStatus ModeExecutor<ActionT>::executeGoal(
  std::shared_ptr<const Goal> goal_ptr, std::shared_ptr<Feedback> feedback_ptr, std::shared_ptr<Result> /*result_ptr*/)
{
  switch (state_) {
    case State::REQUEST_ACTIVATION:
      if (!sendActivationCommand(vehicle_command_client_, goal_ptr)) {
        RCLCPP_ERROR(
          this->node_ptr_->get_logger(), "Failed to send activation command for flight mode %i. Aborting...", mode_id_);
        return ActionStatus::FAILURE;
      }
      // Force to consider only new status messages after sending new command
      last_vehicle_status_ptr_ = nullptr;
      state_ = State::WAIT_FOR_ACTIVATION;
      activation_command_sent_time_ = this->node_ptr_->now();
      RCLCPP_DEBUG(
        this->node_ptr_->get_logger(), "Activation command for flight mode %i was sent successfully", mode_id_);
      return ActionStatus::RUNNING;
    case State::WAIT_FOR_ACTIVATION:
      if (isCurrentNavState(mode_id_)) {
        RCLCPP_DEBUG(this->node_ptr_->get_logger(), "Flight mode %i is active", mode_id_);
        state_ = State::WAIT_FOR_COMPLETION_SIGNAL;
      } else if (this->node_ptr_->now() - activation_command_sent_time_ > activation_timeout_) {
        RCLCPP_ERROR(this->node_ptr_->get_logger(), "Timeout activating flight mode %i. Aborting...", mode_id_);
        return ActionStatus::FAILURE;
      }
      return ActionStatus::RUNNING;
    case State::WAIT_FOR_COMPLETION_SIGNAL:
      // Populate feedback message
      setFeedback(feedback_ptr, *last_vehicle_status_ptr_);
 
      // Check if execution should be terminated
      if (isCompleted(goal_ptr, *last_vehicle_status_ptr_)) {
        state_ = State::COMPLETE;
        if (deactivation_flight_mode_ != FlightMode::Unset) {
          RCLCPP_DEBUG(
            this->node_ptr_->get_logger(),
            "Flight mode %i complete! Will deactivate before termination (switching to flight mode %i)...", mode_id_,
            static_cast<int>(deactivation_flight_mode_));
        } else {
          RCLCPP_DEBUG(
            this->node_ptr_->get_logger(),
            "Flight mode %i complete! Will leave current navigation state as is. User is "
            "responsible for initiating the next flight mode...",
            mode_id_);
        }
 
        // Don't return to complete in same iteration
        break;
      }
      // Check if nav state changed
      if (!isCurrentNavState(mode_id_)) {
        RCLCPP_WARN(this->node_ptr_->get_logger(), "Flight mode %i was deactivated externally. Aborting...", mode_id_);
        return ActionStatus::FAILURE;
      }
      return ActionStatus::RUNNING;
    case State::COMPLETE:
      break;
  }
 
  if (deactivation_flight_mode_ != FlightMode::Unset) {
    const auto deactivation_state = asyncDeactivateFlightMode();
    if (deactivation_state != ActionStatus::SUCCESS) {
      return deactivation_state;
    }
    // Don't return to complete in same iteration
  }
 
  RCLCPP_INFO(this->node_ptr_->get_logger(), "Flight mode %i execution complete", mode_id_);
  if (disarm_after_completion_ && !vehicle_command_client_.disarm()) {
    RCLCPP_WARN(this->node_ptr_->get_logger(), "Failed to disarm after flight mode %i completion", mode_id_);
  }
  return ActionStatus::SUCCESS;
}
 
template <class ActionT>
bool ModeExecutor<ActionT>::sendActivationCommand(
  const VehicleCommandClient & client, std::shared_ptr<const Goal> /*goal_ptr*/)
{
  return client.syncActivateFlightMode(mode_id_);
}
 
template <class ActionT>
bool ModeExecutor<ActionT>::isCompleted(
  std::shared_ptr<const Goal> /*goal_ptr*/, const px4_msgs::msg::VehicleStatus & /*vehicle_status*/)
{
  if (!mode_completed_result_.has_value()) {
    return false;
  }
  if (*mode_completed_result_ == px4_msgs::msg::ModeCompleted::RESULT_SUCCESS) {
    return true;
  } else {
    RCLCPP_INFO(
      this->node_ptr_->get_logger(), "Flight mode %i completed unsuccessfully (result: %i). Aborting...", mode_id_,
      static_cast<int>(*mode_completed_result_));
    this->action_context_ptr_->abort();
  }
  return false;
}
 
template <class ActionT>
void ModeExecutor<ActionT>::setFeedback(
  std::shared_ptr<Feedback> /*feedback_ptr*/, const px4_msgs::msg::VehicleStatus & /*vehicle_status*/)
{
  return;
}
 
template <class ActionT>
uint8_t ModeExecutor<ActionT>::getModeID() const
{
  return mode_id_;
}
 
template <class ActionT, class ModeT>
ModeExecutorFactory<ActionT, ModeT>::ModeExecutorFactory(
  const std::string & action_name, const rclcpp::NodeOptions & options,
  VehicleCommandClient::FlightMode deactivation_flight_mode, bool disarm_after_completion)
: node_ptr_(std::make_shared<rclcpp::Node>(action_name + "_node", options))
{
  static_assert(
    std::is_base_of<ModeBase<ActionT>, ModeT>::value,
    "Template argument ModeT must inherit auto_apms::ModeBase<ActionT> as public and with same type "
    "ActionT as auto_apms::ActionWrapper<ActionT>");
 
  const auto action_context_ptr = std::make_shared<auto_apms_util::ActionContext<ActionT>>(node_ptr_->get_logger());
 
  mode_ptr_ = std::make_unique<ModeT>(*node_ptr_, px4_ros2::ModeBase::Settings(action_name), action_context_ptr);
 
  constexpr int max_retries = 5;
  bool fmu_available = false;
  for (int attempt = 0; attempt < max_retries; ++attempt) {
    if (px4_ros2::waitForFMU(*node_ptr_, std::chrono::seconds(3))) {
      fmu_available = true;
      RCLCPP_DEBUG(node_ptr_->get_logger(), "FMU availability test successful (attempt %d).", attempt + 1);
      break;
    }
    RCLCPP_WARN(node_ptr_->get_logger(), "No message from FMU (attempt %d/%d). Retrying...", attempt + 1, max_retries);
  }
  if (!fmu_available) {
    throw std::runtime_error("No message from FMU after multiple attempts");
  }
 
  if (!mode_ptr_->doRegister()) {
    RCLCPP_FATAL(node_ptr_->get_logger(), "Registration of mode with action_name: '%s' failed.", action_name.c_str());
    throw std::runtime_error("Mode registration failed");
  }
 
  // AFTER (!) registration, the mode id can be queried to set up the executor
  mode_executor_ptr_ = std::make_shared<ModeExecutor<ActionT>>(
    action_name, node_ptr_, action_context_ptr, mode_ptr_->id(), deactivation_flight_mode, disarm_after_completion);
}
 
template <class ActionT, class ModeT>
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ModeExecutorFactory<ActionT, ModeT>::get_node_base_interface()
{
  return node_ptr_->get_node_base_interface();
}
 
}  // namespace auto_apms_px4