builtin_controller.h

Go to the documentation of this file.

#pragma once
#include "aerial_autonomy/controllers/base_controller.h"
#include "aerial_autonomy/types/position_yaw.h"
#include "aerial_autonomy/types/velocity_yaw.h"
#include "position_controller_config.pb.h"
#include "velocity_controller_config.pb.h"

template <class GoalType>
class BuiltInController : public Controller<GoalType, GoalType, GoalType> {
public:
  virtual ~BuiltInController() {}

protected:
  virtual bool runImplementation(GoalType, GoalType goal, GoalType &control) {
    control = goal;
    return true;
  }
};

class BuiltInPositionController : public BuiltInController<PositionYaw> {
public:
  BuiltInPositionController(PositionControllerConfig config)
      : config_(config) {}

  BuiltInPositionController()
      : BuiltInPositionController(PositionControllerConfig()) {}

protected:
  virtual ControllerStatus
  isConvergedImplementation(PositionYaw current_position_yaw,
                            PositionYaw goal) {
    PositionYaw position_yaw_diff = current_position_yaw - goal;
    ControllerStatus status(ControllerStatus::Active);
    status << "PositionYawDiff: " << position_yaw_diff.x << position_yaw_diff.y
           << position_yaw_diff.z << position_yaw_diff.yaw;
    const config::Position &tolerance_pos = config_.goal_position_tolerance();
    const double &tolerance_yaw = config_.goal_yaw_tolerance();
    // Compare
    if (std::abs(position_yaw_diff.x) < tolerance_pos.x() &&
        std::abs(position_yaw_diff.y) < tolerance_pos.y() &&
        std::abs(position_yaw_diff.z) < tolerance_pos.z() &&
        std::abs(position_yaw_diff.yaw) < tolerance_yaw) {
      VLOG_EVERY_N(1, 50) << "Reached goal";
      status.setStatus(ControllerStatus::Completed, "Reached goal");
    }
    return status;
  }

private:
  PositionControllerConfig config_;
};

class BuiltInVelocityController : public BuiltInController<VelocityYaw> {
public:
  BuiltInVelocityController(VelocityControllerConfig config)
      : config_(config) {}

  BuiltInVelocityController()
      : BuiltInVelocityController(VelocityControllerConfig()) {}

protected:
  virtual ControllerStatus
  isConvergedImplementation(VelocityYaw current_velocity_yaw,
                            VelocityYaw goal) {
    VelocityYaw velocity_yaw_diff = current_velocity_yaw - goal;
    // Add optional description:
    ControllerStatus status(ControllerStatus::Active);
    status << "Error Velocity, Yaw: " << velocity_yaw_diff.x
           << velocity_yaw_diff.y << velocity_yaw_diff.z
           << velocity_yaw_diff.yaw;
    const config::Velocity &tolerance_vel = config_.goal_velocity_tolerance();
    const double &tolerance_yaw = config_.goal_yaw_tolerance();
    // Compare
    if (std::abs(velocity_yaw_diff.x) < tolerance_vel.vx() &&
        std::abs(velocity_yaw_diff.y) < tolerance_vel.vy() &&
        std::abs(velocity_yaw_diff.z) < tolerance_vel.vz() &&
        std::abs(velocity_yaw_diff.yaw) < tolerance_yaw) {
      VLOG_EVERY_N(1, 50) << "Reached goal";
      status.setStatus(ControllerStatus::Completed, "Reached Goal");
    }
    return status;
  }

private:
  VelocityControllerConfig config_;
};