state_machine_gui_connector.h

Go to the documentation of this file.

#pragma once

#include <geometry_msgs/PoseStamped.h>
#include <glog/logging.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <tf_conversions/tf_eigen.h>

#include <aerial_autonomy/types/position_yaw.h>

template <class EventManagerT, class LogicStateMachineT>
class StateMachineGUIConnector {
public:
  StateMachineGUIConnector(ros::NodeHandle &nh, EventManagerT &event_manager,
                           LogicStateMachineT &logic_state_machine)
      : event_manager_(event_manager),
        logic_state_machine_(logic_state_machine) {
    event_manager_sub_ = nh.subscribe(
        "event_manager", 1, &StateMachineGUIConnector::eventCallback, this);
    pose_command_sub_ =
        nh.subscribe("goal_pose_command", 1,
                     &StateMachineGUIConnector::poseCommandCallback, this);
  }

  bool isEventManagerConnected() {
    return event_manager_sub_.getNumPublishers() > 0;
  }

  bool isPoseCommandConnected() {
    return pose_command_sub_.getNumPublishers() > 0;
  }

private:
  void eventCallback(const std_msgs::StringConstPtr &event_data) {
    VLOG(1) << "Triggering event: " << event_data;
    event_manager_.triggerEvent(event_data->data, logic_state_machine_);
  }

  void poseCommandCallback(const geometry_msgs::PoseStamped &pose) {
    PositionYaw pose_command;
    pose_command.x = pose.pose.position.x;
    pose_command.y = pose.pose.position.y;
    pose_command.z = pose.pose.position.z;
    pose_command.yaw = tf::getYaw(pose.pose.orientation);
    VLOG(1) << "Received Pose command: " << pose_command.x << "\t"
            << pose_command.y << "\t" << pose_command.z << "\t"
            << pose_command.yaw;
    logic_state_machine_.process_event(pose_command);
  }

  ros::Subscriber pose_command_sub_;
  ros::Subscriber event_manager_sub_;
  EventManagerT &event_manager_;
  LogicStateMachineT &logic_state_machine_;
};