base_robot_system.h

Go to the documentation of this file.

#pragma once

#include <aerial_autonomy/controller_hardware_connectors/base_controller_hardware_connector.h>
// Store type_map
#include <aerial_autonomy/common/type_map.h>
// Iterable Enum
#include <aerial_autonomy/common/iterable_enum.h>
// Boost thread stuff
#include <boost/thread/mutex.hpp>
// Unique ptr
#include <memory>

class BaseRobotSystem {

protected:
  TypeMap<AbstractControllerHardwareConnector>
      controller_hardware_connector_container_;

private:
  std::map<HardwareType, AbstractControllerHardwareConnector *>
      active_controllers_;
  std::map<HardwareType, std::unique_ptr<boost::mutex>> thread_mutexes_;

public:
  BaseRobotSystem() {
    // Initialize active controller map
    for (auto hardware_type : IterableEnum<HardwareType>()) {
      active_controllers_[hardware_type] = nullptr;
      thread_mutexes_[hardware_type] =
          std::unique_ptr<boost::mutex>(new boost::mutex);
    }
  }

  template <class ControllerHardwareConnectorT, class GoalT>
  void setGoal(GoalT goal) {
    ControllerHardwareConnectorT *controller_hardware_connector =
        controller_hardware_connector_container_
            .getObject<ControllerHardwareConnectorT>();
    controller_hardware_connector->setGoal(goal);
    HardwareType hardware_type =
        controller_hardware_connector->getHardwareType();
    if (active_controllers_[hardware_type] != controller_hardware_connector) {
      boost::mutex::scoped_lock lock(*thread_mutexes_[hardware_type]);
      active_controllers_[hardware_type] = controller_hardware_connector;
    }
  }
  template <class ControllerHardwareConnectorT, class GoalT>
  GoalT getGoal() const {
    const ControllerHardwareConnectorT *controller_hardware_connector =
        controller_hardware_connector_container_
            .getObject<ControllerHardwareConnectorT>();
    return controller_hardware_connector->getGoal();
  }

  template <class ControllerHardwareConnectorT>
  ControllerStatus getStatus() const {
    const ControllerHardwareConnectorT *controller_hardware_connector =
        controller_hardware_connector_container_
            .getObject<ControllerHardwareConnectorT>();
    return controller_hardware_connector->getStatus();
  }

  ControllerStatus getActiveControllerStatus(HardwareType hardware_type) const {
    auto active_controller = active_controllers_.find(hardware_type);
    if (active_controller != active_controllers_.end() &&
        active_controller->second != nullptr) {
      return active_controller->second->getStatus();
    } else {
      return ControllerStatus(ControllerStatus::NotEngaged);
    }
  }

  void abortController(HardwareType hardware_type) {
    boost::mutex::scoped_lock lock(*thread_mutexes_[hardware_type]);
    active_controllers_[hardware_type] = nullptr;
  }

  void runActiveController(HardwareType hardware_type) {
    // lock to ensure active_control fcn is not changed
    AbstractControllerHardwareConnector *active_controller =
        active_controllers_[hardware_type];
    if (active_controller != nullptr) {
      boost::mutex::scoped_lock lock(*thread_mutexes_[hardware_type]);
      active_controller->run();
    }
  }

  virtual std::string getSystemStatus() const = 0;
};