visual_servoing_controller_arm_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/relative_pose_controller.h"
#include "aerial_autonomy/trackers/base_tracker.h"

#include <arm_parsers/arm_parser.h>

#include <parsernode/parser.h>

#include <tf/tf.h>

class VisualServoingControllerArmConnector
    : public ControllerHardwareConnector<
          std::tuple<tf::Transform, tf::Transform>, tf::Transform,
          tf::Transform> {
public:
  VisualServoingControllerArmConnector(BaseTracker &tracker,
                                       parsernode::Parser &drone_hardware,
                                       ArmParser &arm_hardware,
                                       RelativePoseController &controller,
                                       tf::Transform camera_transform,
                                       tf::Transform arm_transform)
      : ControllerHardwareConnector(controller, HardwareType::Arm),
        drone_hardware_(drone_hardware), arm_hardware_(arm_hardware),
        tracker_(tracker), camera_transform_(camera_transform),
        arm_transform_(arm_transform) {}
  virtual ~VisualServoingControllerArmConnector() {}

  bool getTrackingPoseArmFrame(tf::Transform &tracking_pose);

protected:
  virtual bool
  extractSensorData(std::tuple<tf::Transform, tf::Transform> &sensor_data);

  virtual void sendHardwareCommands(tf::Transform controls);

private:
  parsernode::Parser &drone_hardware_;
  ArmParser &arm_hardware_;
  BaseTracker &tracker_;
  tf::Transform camera_transform_;
  tf::Transform arm_transform_;
};