uav_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_system.h>
#include <aerial_autonomy/system_handlers/common_system_handler.h>

#include "uav_system_handler_config.pb.h"

template <class LogicStateMachineT, class EventManagerT>
class UAVSystemHandler {
public:
  UAVSystemHandler(UAVSystemHandlerConfig &config)
      : nh_uav_("~uav"), parser_loader_("parsernode", "parsernode::Parser"),
        uav_hardware_(
            parser_loader_.createUnmanagedInstance(config.uav_parser_type())),
        uav_system_(*uav_hardware_, config.uav_system_config()),
        common_handler_(config.base_config(), uav_system_),
        uav_controller_timer_(
            std::bind(&UAVSystem::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();
  }

  UAVSystemHandler(const UAVSystemHandler &) = 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_; 
  pluginlib::ClassLoader<parsernode::Parser>
      parser_loader_; 
  std::unique_ptr<parsernode::Parser> uav_hardware_; 
  UAVSystem uav_system_;                             
  CommonSystemHandler<LogicStateMachineT, EventManagerT, UAVSystem>
      common_handler_;              
  AsyncTimer uav_controller_timer_; 
};