common_system_handler.h

Go to the documentation of this file.

#pragma once

#include <ros/ros.h>
#include <std_msgs/String.h>

#include "common_system_handler_config.pb.h"
#include <aerial_autonomy/actions_guards/base_functors.h>
#include <aerial_autonomy/common/async_timer.h>
#include <aerial_autonomy/common/system_status_publisher.h>
#include <aerial_autonomy/state_machines/state_machine_gui_connector.h>

template <class LogicStateMachineT, class EventManagerT, class RobotSystemT>
class CommonSystemHandler {
public:
  CommonSystemHandler(const CommonSystemHandlerConfig &config,
                      RobotSystemT &robot_system)
      : nh_("~common"), logic_state_machine_(std::ref(robot_system)),
        state_machine_gui_connector_(nh_, event_manager_, logic_state_machine_),
        system_status_pub_(nh_, robot_system, logic_state_machine_),
        status_timer_(
            std::bind(
                &SystemStatusPublisher<LogicStateMachineT>::publishSystemStatus,
                std::ref(system_status_pub_)),
            std::chrono::milliseconds(config.status_timer_duration())),
        logic_state_machine_timer_(
            std::bind(&LogicStateMachineT::template process_event<
                          InternalTransitionEvent>,
                      std::ref(logic_state_machine_),
                      InternalTransitionEvent()),
            std::chrono::milliseconds(config.state_machine_timer_duration())) {}

  CommonSystemHandler(const CommonSystemHandler &) = delete;

  bool isConnected() {
    return state_machine_gui_connector_.isEventManagerConnected() &&
           state_machine_gui_connector_.isPoseCommandConnected();
  }

  void startTimers() {
    logic_state_machine_.start();
    logic_state_machine_timer_.start();
    status_timer_.start();
  }

protected:
  ros::NodeHandle nh_;
  LogicStateMachineT
      logic_state_machine_;     
  EventManagerT event_manager_; 
  StateMachineGUIConnector<EventManagerT, LogicStateMachineT>
      state_machine_gui_connector_; 
  SystemStatusPublisher<LogicStateMachineT>
      system_status_pub_;   
  AsyncTimer status_timer_; 
  AsyncTimer logic_state_machine_timer_; 
};