visual_servoing_controller_drone_connector.h

Go to the documentation of this file.

#pragma once
#include "aerial_autonomy/controller_hardware_connectors/base_controller_hardware_connector.h"
#include "aerial_autonomy/controllers/constant_heading_depth_controller.h"
#include "aerial_autonomy/trackers/base_tracker.h"
#include "aerial_autonomy/types/position_yaw.h"
#include "aerial_autonomy/types/velocity_yaw.h"
#include "uav_vision_system_config.pb.h"

#include <parsernode/parser.h>

#include <tf/tf.h>

class VisualServoingControllerDroneConnector
    : public ControllerHardwareConnector<PositionYaw, Position,
                                         VelocityYawRate> {
public:
  VisualServoingControllerDroneConnector(
      BaseTracker &tracker, parsernode::Parser &drone_hardware,
      ConstantHeadingDepthController &controller,
      tf::Transform camera_transform)
      : ControllerHardwareConnector(controller, HardwareType::UAV),
        drone_hardware_(drone_hardware), tracker_(tracker),
        camera_transform_(camera_transform) {}
  virtual ~VisualServoingControllerDroneConnector() {}

  bool getTrackingVectorGlobalFrame(Position &tracking_vector);

protected:
  virtual bool extractSensorData(PositionYaw &sensor_data);

  virtual void sendHardwareCommands(VelocityYawRate controls);

private:
  tf::Matrix3x3 getBodyFrameRotation();

  parsernode::Parser &drone_hardware_;
  BaseTracker &tracker_;
  tf::Transform camera_transform_;
};