takeoff_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/uav_basic_events.h>
#include <glog/logging.h>
#include <parsernode/common.h>

template <class LogicStateMachineT>
struct TakeoffTransitionActionFunctor_
    : EventAgnosticActionFunctor<UAVSystem, LogicStateMachineT> {
  void run(UAVSystem &robot_system) {
    VLOG(1) << "Takingoff!";
    robot_system.takeOff();
  }
};

template <class LogicStateMachineT>
struct TakeoffAbortActionFunctor_
    : EventAgnosticActionFunctor<UAVSystem, LogicStateMachineT> {
  void run(UAVSystem &robot_system) {
    VLOG(1) << "Aborting Takeoff!";
    robot_system.land();
  }
};

template <class LogicStateMachineT>
struct TakeoffTransitionGuardFunctor_
    : EventAgnosticGuardFunctor<UAVSystem, LogicStateMachineT> {
  bool guard(UAVSystem &robot_system) {
    parsernode::common::quaddata data = robot_system.getUAVData();
    bool result = false;
    // Check for voltage
    if (data.batterypercent >
        robot_system.getConfiguration().minimum_battery_percent()) {
      result = true;
    } else {
      LOG(WARNING) << "Battery too low! " << data.batterypercent
                   << "\% SoCannot takeoff";
    }
    return result;
  }
};

template <class LogicStateMachineT>
struct TakeoffInternalActionFunctor_
    : 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 >=
        // Transition to hovering state once reached high altitude
        robot_system.getConfiguration().minimum_takeoff_height()) {
      VLOG(1) << "Completed Takeoff";
      logic_state_machine.process_event(Completed());
      return false;
    }
    return true;
  }
};

template <class LogicStateMachineT>
using TakingOff_ = BaseState<UAVSystem, LogicStateMachineT,
                             TakeoffInternalActionFunctor_<LogicStateMachineT>>;