base_controller_hardware_connector.h

Go to the documentation of this file.

#pragma once
#include <aerial_autonomy/common/atomic.h>
#include <aerial_autonomy/common/controller_status.h>
#include <aerial_autonomy/controllers/base_controller.h>
#include <glog/logging.h>

enum class HardwareType {
  UAV,         
  Arm,         
  First = UAV, // This should always point to the first in the list
  Last = Arm   // This should always point to the last in the list
};

struct AbstractControllerHardwareConnector {
public:
  virtual void run() = 0;

  virtual ControllerStatus getStatus() const = 0;

  virtual ~AbstractControllerHardwareConnector() {}
};

template <class SensorDataType, class GoalType, class ControlType>
class ControllerHardwareConnector : public AbstractControllerHardwareConnector {
public:
  ControllerHardwareConnector(
      Controller<SensorDataType, GoalType, ControlType> &controller,
      HardwareType hardware_type)
      : AbstractControllerHardwareConnector(), hardware_type_(hardware_type),
        controller_(controller) {}

  virtual void run() {
    // Get latest sensor data
    // run the controller
    // send the data back to hardware manager
    SensorDataType sensor_data;
    ControlType control;
    if (!extractSensorData(sensor_data)) {
      status_ = ControllerStatus(ControllerStatus::Critical,
                                 "Cannot extract sensor data");
      return;
    }
    if (!controller_.run(sensor_data, control)) {
      status_ =
          ControllerStatus(ControllerStatus::Critical, "Cannot run controller");
      return;
    }
    sendHardwareCommands(control);
    status_ = controller_.isConverged(sensor_data);
  }
  void setGoal(GoalType goal) {
    status_ = ControllerStatus(ControllerStatus::Active);
    controller_.setGoal(goal);
  }
  GoalType getGoal() const { return controller_.getGoal(); }

  HardwareType getHardwareType() { return hardware_type_; }

  ControllerStatus getStatus() const { return status_; }

protected:
  virtual bool extractSensorData(SensorDataType &sensor_data) = 0;

  virtual void sendHardwareCommands(ControlType controls) = 0;

protected:
  HardwareType hardware_type_;

private:
  Controller<SensorDataType, GoalType, ControlType> &controller_;
  Atomic<ControllerStatus> status_;
};