Add GUI to trigger events, receive status

Add rqt plugin to take in an event file and generate GUI with
buttons corresponding to event names. The GUI also contains three
textboxes to receive string data for status of Quad, arm, state machine
The arm textbox is usually hidden and can be shown using --use_arm option

.gitignore

@@ -0,0 +1 @@
+CMakeLists.txt.user

CMakeLists.txt

@@ -19,7 +19,7 @@ find_package(Boost REQUIRED COMPONENTS system)
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -121,9 +121,9 @@ catkin_package(
 ## The header file contains the structs to define events
 ## It also adds an event manager class templated on StateMachine
 ## to trigger events based on name:
-##	void triggerEvent(event_name, state_machine)
+##  void triggerEvent(event_name, state_machine)
 ## It also lets you print all the event names created:
-##	void printEventList()
+##  void printEventList()
 generate_event_targets(
 basic_events
 visual_servoing_events
@@ -216,3 +216,6 @@ add_dependencies(${PROJECT_NAME}-evnt-mngr-test ${${PROJECT_NAME}_EXPORTED_TARGE
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+install(FILES plugin.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+        )

events/basic_events

@@ -1,4 +1,4 @@
 BasicEventManager:
-	Land
-	Takeoff
-	Abort
+  Land
+  Takeoff
+  Abort

events/visual_servoing_events

@@ -1,5 +1,5 @@
 VisualServoingEventManager:
-	Land
-	Takeoff
-	Abort
-	FollowTrajectory
+  Land
+  Takeoff
+  Abort
+  FollowTrajectory

package.xml

@@ -42,13 +42,18 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>rqt_gui_py</build_depend>
+  <run_depend>python-rospkg</run_depend>
   <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend version_gte="0.2.12">rqt_gui</run_depend>
+  <run_depend>rqt_gui_py</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
+    <rqt_gui plugin="${prefix}/plugin.xml"/>
   </export>
-</package>
+</package>

plugin.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<library path="src">
+  <class name="aerial_autonomy/aerial_autonomy_gui" type="aerial_autonomy.aerial_autonomy_gui.EventTransmissionGUI" base_class_type="rqt_gui_py::Plugin">
+    <description>
+      GUI to trigger events to state machine and receive status updates on quad, arm, and state machine
+    </description>
+    <qtgui>
+      <label>AerialAutonomyGUI</label>
+      <icon type="theme">utilities-system-monitor</icon>
+      <statustip>Plugin to trigger events to state machine</statustip>
+    </qtgui>
+  </class>
+</library>

scripts/rqt_aerial_autonomy_gui.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+# Copyright (c) 2013, Oregon State University
+# All rights reserved.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Oregon State University nor the
+#       names of its contributors may be used to endorse or promote products
+#       derived from this software without specific prior written permission.
+
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL OREGON STATE UNIVERSITY BE LIABLE FOR ANY
+# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+# Author Dan Lazewatsky/lazewatd@engr.orst.edu
+
+import sys
+
+from aerial_autonomy.aerial_autonomy_gui import EventTransmissionGUI
+from rqt_gui.main import Main
+
+plugin_name = 'aerial_autonomy.aerial_autonomy_gui.EventTransmissionGUI'
+main = Main()
+sys.exit(
+    main.main(
+        standalone=plugin_name,
+        plugin_argument_provider=EventTransmissionGUI.add_arguments))

setup.py

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(packages=['aerial_autonomy'],
+                             package_dir={'': 'src'}
+                             )
+
+setup(**d)

src/aerial_autonomy/aerial_autonomy_gui.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python
+"""
+Generate a GUI to trigger events for state machine
+
+@author: gowtham
+"""
+import argparse
+from qt_gui.plugin import Plugin
+from python_qt_binding.QtWidgets import (QLabel, QVBoxLayout,
+                                         QGridLayout, QWidget,
+                                         QTextEdit, QPushButton)
+from ros_event_trigger import RosEventTrigger
+from argparse import ArgumentParser
+from functools import partial
+
+# %%
+
+
+class EventTransmissionGUI(Plugin):
+
+    def __init__(self, context):
+        """
+        Create Qt GUI using the event file
+        """
+        super(EventTransmissionGUI, self).__init__(context)
+        self.setObjectName('ManualEventTriggerGUI')
+        parser = ArgumentParser()
+
+        # Add argument(s) to the parser.
+        args = self._parse_args(context.argv())
+
+        # Create Event trigger
+        self.event_trigger = RosEventTrigger(args.event_file)
+
+        # Set Layout
+        self._container = QWidget()
+        self._container.setWindowTitle(self.event_trigger.event_manager_name)
+        self._layout = QVBoxLayout()
+        self._container.setLayout(self._layout)
+
+        # Create Textboxes and add to Layout
+        self._layout.addWidget(QLabel('State Machine state'))
+        self.state_machine_textbox = QTextEdit()
+        self.state_machine_textbox.setReadOnly(True)
+        self._layout.addWidget(self.state_machine_textbox)
+
+        self._layout.addWidget(QLabel('Quad Status'))
+        self.quad_textbox = QTextEdit()
+        self.quad_textbox.setReadOnly(True)
+        self._layout.addWidget(self.quad_textbox)
+
+        if args.use_arm:
+            self._layout.addWidget(QLabel('Arm Status'))
+            self.arm_textbox = QTextEdit()
+            self.arm_textbox.setReadOnly(True)
+            self._layout.addWidget(self.arm_textbox)
+
+        # Define and connect buttons
+        self._layout.addWidget(QLabel('Event Triggers'))
+        self.button_container = QWidget()
+        self.push_buttons = list()
+        self.button_layout = QGridLayout()
+        self.button_container.setLayout(self.button_layout)
+        button_index = 0
+        for event_name in self.event_trigger.event_names_list:
+            self.push_buttons.append(QPushButton(event_name))
+            partial_fun = partial(self.event_trigger.triggerEvent,
+                                  event_name=event_name)
+            self.push_buttons[-1].clicked.connect(partial_fun)
+            row, col = self.get_row_col(button_index, args.grid_cols)
+            self.button_layout.addWidget(self.push_buttons[-1], row, col)
+            button_index += 1
+        self._layout.addWidget(self.button_container)
+
+        context.add_widget(self._container)
+
+        # Add textboxes to update hooks from eventTrigger class
+        # Define Partial callbacks
+        stateMachineStatusCallback = partial(
+            self.updateStatus, text_box=self.state_machine_textbox)
+        quadStatusCallback = partial(
+            self.updateStatus, text_box=self.quad_textbox)
+        # Connect Event Triggers
+        self.event_trigger.state_machine_signal.connect(
+            stateMachineStatusCallback)
+        self.event_trigger.quad_signal.connect(quadStatusCallback)
+        # Same for arm
+        if args.use_arm:
+            armStatusCallback = partial(
+                self.updateStatus, text_box=self.arm_textbox)
+            self.event_trigger.arm_signal.connect(armStatusCallback)
+
+    def _parse_args(self, argv):
+        parser = argparse.ArgumentParser(
+            prog='aerial_autonomy', add_help=False)
+        EventTransmissionGUI.add_arguments(parser)
+        return parser.parse_args(argv)
+
+    @staticmethod
+    def add_arguments(parser):
+        group = parser.add_argument_group(
+            'Options for aerial autonomy gui plugin')
+        group.add_argument("-e", "--event_file", type=str,
+                           default='', help="Event file")
+        group.add_argument("-c", "--grid_cols", type=int,
+                           default=3, help="Number of columns in grid")
+        group.add_argument("--use_arm", action='store_true',
+                           help="To add arm state textbox")
+
+    def get_row_col(self, button_index, ncols):
+        col_index = button_index % ncols
+        row_index = int((button_index - col_index) / ncols)
+        return(row_index, col_index)
+
+    def updateStatus(self, status, text_box):
+        text_box.setText(status)

src/aerial_autonomy/ros_event_trigger.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+"""
+Created on Tue Feb  7 01:21:27 2017
+
+@author: gowtham
+"""
+import rospy
+import rospkg
+import os
+from std_msgs.msg import String
+from functools import partial
+from python_qt_binding.QtCore import QObject, pyqtSignal
+
+
+class RosEventTrigger(QObject):
+
+    # Define signals
+    quad_signal = pyqtSignal(str, name='quadStatus')
+    arm_signal = pyqtSignal(str, name='armStatus')
+    state_machine_signal = pyqtSignal(str, name='stateMachineStatus')
+
+    '''
+    Class that loads an event file from a parameter
+    It provides method to triggerEvents using ros publisher
+    '''
+
+    def __init__(self, event_file_name):
+        super(RosEventTrigger, self).__init__()
+        # Create ros node
+        if not event_file_name:
+            # Get default event file name
+            rospack = rospkg.RosPack()
+            aerial_autonomy_path = rospack.get_path('aerial_autonomy')
+            default_path = os.path.join(aerial_autonomy_path,
+                                        'events/basic_events')
+            if rospy.has_param("event_file"):
+                print "Found event file: ", rospy.get_param('event_file')
+            event_file_name = rospy.get_param('event_file', default_path)
+            print "Event file name: ", event_file_name
+
+        event_file = file(event_file_name, 'r')
+        # Parse event file to save event list and event manager name
+        event_line_list = [event_name.strip()
+                           for event_name in event_file.read().splitlines()]
+        event_file.close()
+        # Define event manager name etc
+        self.event_manager_name = event_line_list[0][:-1]
+        self.event_names_list = event_line_list[1:]
+        self.event_pub = rospy.Publisher('event_trigger',
+                                         String, queue_size=1)
+        # Define partial callbacks
+        quadCallback = partial(self.statusCallback, signal=self.quad_signal)
+        armCallback = partial(self.statusCallback, signal=self.arm_signal)
+        stateMachineCallback = partial(self.statusCallback,
+                                       signal=self.state_machine_signal)
+        # Subscribers for quad arm and state machine updates
+        rospy.Subscriber("quad_status", String, quadCallback)
+        rospy.Subscriber("arm_status", String, armCallback)
+        rospy.Subscriber("stat_machine_status", String, stateMachineCallback)
+
+    def statusCallback(self, msg, signal):
+        signal.emit(str(msg.data))
+
+    def triggerEvent(self, event_name):
+        if not rospy.is_shutdown():
+            msg = String()
+            msg.data = event_name
+            self.event_pub.publish(msg)

tests/event_manager_tests/event_manager_tests.cpp

@@ -8,43 +8,42 @@ using namespace std;
 
 //// \brief Definitions
 struct LSM {
-	template<class Event>
-	void process_event(Event &evt) {
-		std::cout<<"Event type : "<<typeid(evt).name()<<std::endl;
-	}
+  template <class Event> void process_event(Event &evt) {
+    std::cout << "Event type : " << typeid(evt).name() << std::endl;
+  }
 };
 ////
 
 /// \brief TEST
 /// All the tests are defined here
 namespace basic_events{
-	TEST(BasicEventManagerTest, InstantiateManager) {
-		ASSERT_NO_THROW(new BasicEventManager<LSM>());
-	}
-	TEST(BasicEventManagerTest, PrintEventMap) {
-		BasicEventManager<LSM> evt_manager;
-		ASSERT_NO_THROW(evt_manager.printEventList());
-	}
-	TEST(BasicEventManagerTest, TriggerEvents) {
-		BasicEventManager<LSM> evt_manager;
-    LSM lsm;
-	  evt_manager.triggerEvent("Land", lsm);
-	  evt_manager.triggerEvent("Takeoff", lsm);
-	  evt_manager.triggerEvent("Abort", lsm);
-	}
+TEST(BasicEventManagerTest, InstantiateManager) {
+  ASSERT_NO_THROW(new BasicEventManager<LSM>());
+}
+TEST(BasicEventManagerTest, PrintEventMap) {
+  BasicEventManager<LSM> evt_manager;
+  ASSERT_NO_THROW(evt_manager.printEventList());
+}
+TEST(BasicEventManagerTest, TriggerEvents) {
+  BasicEventManager<LSM> evt_manager;
+  LSM lsm;
+  evt_manager.triggerEvent("Land", lsm);
+  evt_manager.triggerEvent("Takeoff", lsm);
+  evt_manager.triggerEvent("Abort", lsm);
+}
 }
 namespace visual_servoing_events{
-	TEST(VisualServoingEventManagerTest, InstantiateManager) {
-		ASSERT_NO_THROW(new VisualServoingEventManager<LSM>());
-	}
-	TEST(VisualServoingEventManagerTest, TriggerEvents) {
-		VisualServoingEventManager<LSM> evt_manager;
-    LSM lsm;
-	  evt_manager.triggerEvent("Land", lsm);
-	  evt_manager.triggerEvent("Takeoff", lsm);
-	  evt_manager.triggerEvent("Abort", lsm);
-	  evt_manager.triggerEvent("FollowTrajectory", lsm);
-	}
+TEST(VisualServoingEventManagerTest, InstantiateManager) {
+  ASSERT_NO_THROW(new VisualServoingEventManager<LSM>());
+}
+TEST(VisualServoingEventManagerTest, TriggerEvents) {
+  VisualServoingEventManager<LSM> evt_manager;
+  LSM lsm;
+  evt_manager.triggerEvent("Land", lsm);
+  evt_manager.triggerEvent("Takeoff", lsm);
+  evt_manager.triggerEvent("Abort", lsm);
+  evt_manager.triggerEvent("FollowTrajectory", lsm);
+}
 }
 ///