land_functors.h

Go to the documentation of this file.

#pragma once
#include <aerial_autonomy/actions_guards/base_functors.h>
#include <aerial_autonomy/logic_states/base_state.h>
#include <aerial_autonomy/robot_systems/uav_system.h>
#include <aerial_autonomy/types/completed_event.h>
#include <aerial_autonomy/types/manual_control_event.h>
#include <parsernode/common.h>

template <class LogicStateMachineT>
struct LandTransitionActionFunctor_
    : EventAgnosticActionFunctor<UAVSystem, LogicStateMachineT> {
  void run(UAVSystem &robot_system) {
    // Need to iterate through enum class HardwareType
    VLOG(1) << "Aborting UAV Controllers";
    robot_system.abortController(HardwareType::UAV);
    VLOG(1) << "Landing";
    robot_system.land();
  }
};

template <class LogicStateMachineT>
struct LandInternalActionFunctor_
    : InternalActionFunctor<UAVSystem, LogicStateMachineT> {
  bool run(UAVSystem &robot_system, LogicStateMachineT &logic_state_machine) {
    parsernode::common::quaddata data = robot_system.getUAVData();
    // If hardware is not allowing us to control UAV
    if (data.localpos.z < robot_system.getConfiguration().landing_height()) {
      VLOG(1) << "Completed Landing";
      logic_state_machine.process_event(Completed());
      return false;
    }
    return true;
  }
};

template <class LogicStateMachineT>
struct LandedInternalActionFunctor_
    : InternalActionFunctor<UAVSystem, LogicStateMachineT> {
  bool run(UAVSystem &robot_system, LogicStateMachineT &logic_state_machine) {
    parsernode::common::quaddata data = robot_system.getUAVData();
    // If hardware is not allowing us to control UAV
    if (!data.rc_sdk_control_switch) {
      VLOG(1) << "Switching to Manual UAV state";
      logic_state_machine.process_event(ManualControlEvent());
      return false;
    }
    return true;
  }
};

template <class LogicStateMachineT>
using Landing_ = BaseState<UAVSystem, LogicStateMachineT,
                           LandInternalActionFunctor_<LogicStateMachineT>>;
template <class LogicStateMachineT>
using Landed_ = BaseState<UAVSystem, LogicStateMachineT,
                          LandedInternalActionFunctor_<LogicStateMachineT>>;