arm_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/arm_system.h>
#include <aerial_autonomy/types/completed_event.h>
#include <aerial_autonomy/uav_basic_events.h>
#include <glog/logging.h>
#include <parsernode/common.h>

namespace be = uav_basic_events;

template <class LogicStateMachineT>
struct ArmFoldTransitionActionFunctor_
    : EventAgnosticActionFunctor<ArmSystem, LogicStateMachineT> {
  void run(ArmSystem &robot_system) {
    VLOG(1) << "Folding Arm!";
    robot_system.foldArm();
  }
};

template <class LogicStateMachineT>
struct ArmRightFoldTransitionActionFunctor_
    : EventAgnosticActionFunctor<ArmSystem, LogicStateMachineT> {
  void run(ArmSystem &robot_system) {
    VLOG(1) << "Folding Arm to right angle!";
    robot_system.rightArm();
  }
};

template <class LogicStateMachineT>
struct ArmPoweroffTransitionActionFunctor_
    : EventAgnosticActionFunctor<ArmSystem, LogicStateMachineT> {
  void run(ArmSystem &robot_system) {
    VLOG(1) << "Powering off Arm!";
    robot_system.power(false);
  }
};

template <class LogicStateMachineT>
struct ArmPoweronTransitionActionFunctor_
    : EventAgnosticActionFunctor<ArmSystem, LogicStateMachineT> {
  void run(ArmSystem &robot_system) {
    VLOG(1) << "Powering on Arm!";
    robot_system.power(true);
  }
};

template <class LogicStateMachineT>
struct ArmEnabledGuardFunctor_
    : EventAgnosticGuardFunctor<ArmSystem, LogicStateMachineT> {
  bool guard(ArmSystem &robot_system_) {
    if (!robot_system_.enabled()) {
      LOG(WARNING) << "Robot system not enabled!";
      return false;
    }
    return true;
  }
};

template <class LogicStateMachineT>
struct AbortArmController_
    : EventAgnosticActionFunctor<ArmSystem, LogicStateMachineT> {
  void run(ArmSystem &robot_system) {
    LOG(WARNING) << "Aborting arm controller";
    robot_system.abortController(HardwareType::Arm);
  }
};

template <class LogicStateMachineT>
struct ArmStatusInternalActionFunctor_
    : InternalActionFunctor<ArmSystem, LogicStateMachineT> {
  bool run(ArmSystem &robot_system, LogicStateMachineT &logic_state_machine) {
    // Check if arm finally powered on
    if (!robot_system.enabled()) {
      LOG(WARNING) << "Arm not enabled! Aborting!";
      logic_state_machine.process_event(be::Abort());
      return false;
    }
    return true;
  }
};
template <class LogicStateMachineT>
struct ArmFoldInternalActionFunctor_
    : InternalActionFunctor<ArmSystem, LogicStateMachineT> {
  bool run(ArmSystem &robot_system, LogicStateMachineT &logic_state_machine) {
    if (robot_system.getCommandStatus()) {
      VLOG(1) << "Completed Folding arm!";
      logic_state_machine.process_event(Completed());
      return false;
    } else if (!robot_system.enabled()) {
      LOG(WARNING) << "Arm not enabled!";
      logic_state_machine.process_event(be::Abort());
      return false;
    }
    return true;
  }
};

template <class LogicStateMachineT>
using ArmFolding_ =
    BaseState<ArmSystem, LogicStateMachineT,
              ArmFoldInternalActionFunctor_<LogicStateMachineT>>;
template <class LogicStateMachineT>
class ArmPreTakeoffFolding_ : public ArmFolding_<LogicStateMachineT> {};
template <class LogicStateMachineT>
class ArmPreLandingFolding_ : public ArmFolding_<LogicStateMachineT> {};