uav_vision_system_handler.h

Go to the documentation of this file.

#pragma once

#include <ros/ros.h>

#include <parsernode/parser.h>
#include <pluginlib/class_loader.h>

#include <aerial_autonomy/actions_guards/base_functors.h>
#include <aerial_autonomy/robot_systems/uav_vision_system.h>
#include <aerial_autonomy/system_handlers/common_system_handler.h>
#include <aerial_autonomy/trackers/alvar_tracker.h>
#include <aerial_autonomy/trackers/roi_to_position_converter.h>

#include "uav_system_handler_config.pb.h"

template <class LogicStateMachineT, class EventManagerT>
class UAVVisionSystemHandler {
public:
  UAVVisionSystemHandler(UAVSystemHandlerConfig &config)
      : nh_uav_("~uav"), nh_tracker_("~tracker"),
        parser_loader_("parsernode", "parsernode::Parser"),
        uav_hardware_(
            parser_loader_.createUnmanagedInstance(config.uav_parser_type())),
        tracker_(
            config.uav_system_config()
                        .uav_vision_system_config()
                        .tracker_type() == "ROI"
                ? dynamic_cast<BaseTracker *>(
                      new RoiToPositionConverter(nh_tracker_))
                : dynamic_cast<BaseTracker *>(new AlvarTracker(nh_tracker_))),
        uav_system_(*tracker_, *uav_hardware_, config.uav_system_config()),
        common_handler_(config.base_config(), uav_system_),
        uav_controller_timer_(
            std::bind(&UAVVisionSystem::runActiveController,
                      std::ref(uav_system_), HardwareType::UAV),
            std::chrono::milliseconds(config.uav_controller_timer_duration())) {
    // Initialize UAV plugin
    uav_hardware_->initialize(nh_uav_);

    // Get the party started
    common_handler_.startTimers();
    uav_controller_timer_.start();
  }

  UAVVisionSystemHandler(const UAVVisionSystemHandler &) = delete;

  parsernode::common::quaddata getUAVData() {
    parsernode::common::quaddata quad_data;
    uav_hardware_->getquaddata(quad_data);
    return quad_data;
  }

  bool isConnected() { return common_handler_.isConnected(); }

private:
  ros::NodeHandle nh_uav_;     
  ros::NodeHandle nh_tracker_; 
  pluginlib::ClassLoader<parsernode::Parser>
      parser_loader_; 
  std::unique_ptr<parsernode::Parser> uav_hardware_; 
  std::unique_ptr<BaseTracker> tracker_;             
  UAVVisionSystem uav_system_;                       
  CommonSystemHandler<LogicStateMachineT, EventManagerT, UAVVisionSystem>
      common_handler_;              
  AsyncTimer uav_controller_timer_; 
};