uav_arm_system.h

Go to the documentation of this file.

#pragma once
#include "aerial_autonomy/common/math.h"
#include "aerial_autonomy/controller_hardware_connectors/visual_servoing_controller_arm_connector.h"
#include "aerial_autonomy/controllers/relative_pose_controller.h"
#include "aerial_autonomy/robot_systems/arm_system.h"
#include "aerial_autonomy/robot_systems/uav_vision_system.h"
#include <chrono>

class UAVArmSystem : public UAVVisionSystem, public ArmSystem {
public:
  UAVArmSystem(BaseTracker &tracker, parsernode::Parser &drone_hardware,
               ArmParser &arm_hardware, UAVSystemConfig config)
      : UAVVisionSystem(tracker, drone_hardware, config),
        ArmSystem(arm_hardware),
        grip_timeout_(config_.uav_vision_system_config()
                          .uav_arm_system_config()
                          .grip_timeout()),
        arm_goal_transforms_(
            math::getTransformsFromVector(config_.uav_vision_system_config()
                                              .uav_arm_system_config()
                                              .arm_goal_transform())),
        arm_transform_(
            math::getTransformFromVector(config_.uav_vision_system_config()
                                             .uav_arm_system_config()
                                             .arm_transform())),
        relative_pose_controller_(config_.uav_vision_system_config()
                                      .uav_arm_system_config()
                                      .position_controller_config()),
        visual_servoing_arm_connector_(tracker, drone_hardware, arm_hardware,
                                       relative_pose_controller_,
                                       camera_transform_, arm_transform_) {
    controller_hardware_connector_container_.setObject(
        visual_servoing_arm_connector_);
  }

  std::string getSystemStatus() const {
    std::stringstream status;
    status << UAVVisionSystem::getSystemStatus() << std::endl
           << ArmSystem::getSystemStatus();
    return status.str();
  }

  uint32_t gripTimeout() { return grip_timeout_; }

  tf::Transform armGoalTransform(int i) { return arm_goal_transforms_.at(i); }

private:
  uint32_t grip_timeout_;
  std::vector<tf::Transform> arm_goal_transforms_;
  tf::Transform arm_transform_;
  RelativePoseController relative_pose_controller_;
  VisualServoingControllerArmConnector visual_servoing_arm_connector_;
};