(WIP) Arm autonomy

software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIKControls.py

@@ -0,0 +1,66 @@
+# coding=utf-8
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets, QtGui
+import sys
+import signal
+import logging
+import rospy
+
+from rover_arm_control.msg import IKControlMessage
+
+#####################################
+# Global Variables
+#####################################
+ARM_CONTROLLER_STATUS = "/rover_arm_control/IKControl/control_status"
+ARM_TOGGLE_STATUS =  "/rover_arm_control/IKControl/button_status"
+
+class ArmIKControls(QtCore.QObject):
+    def __init__(self, shared_objects):
+        controller_start__signal = QtCore.pyqtSignal(str)
+        controller_status__signal = QtCore.pyqtSignal(str)
+
+        # ########## Reference to class init variables ##########
+        super(ArmIKControls, self).__init__()
+        self.shared_objects = shared_objects
+        self.main_screen = self.shared_objects["screens"]["main_screen"]
+
+        self.ik_control_start_button = self.main_screen.ik_control_start_button #type: QtWidgets.QButton
+        self.ik_control_status_label = self.main_screen.ik_control_status_label #type : QtWidgets.QLabel
+
+        # ########## Class Variables ##########
+        self.ik_status_subscriber = rospy.Subscriber(ARM_CONTROLLER_STATUS, IKControlMessage, self.arm_ik_status__callback)
+        self.ik_status_publisher = rospy.Publisher(ARM_TOGGLE_STATUS, IKControlMessage, queue_size =1)
+
+        self.controllers_status = False
+        self.button_status = False
+
+    def arm_ik_status__callback(self,data):
+        self.controllers_status = data.controllers_started
+        self.button_status = data.start_button
+
+        if self.controllers_started is True:
+            ##Indicate that arm IK is started on groundstation 
+            controller_start__signal.emit(COLOR_GREEN)
+        else:
+            ##Indicate that are IK is off on groundstation 
+            controller_start__signal.emit(COLOR_RED)
+
+        if self.button_status is False:
+            message.start_button = False
+            self.ik_status_publisher.publish(message)
+
+    def connect_signals_and_slots(self):
+        self.ik_control_start_button.clicked.connect(self.on_ik_start_button_pressed__slot)
+        self.controller_status__signal.connect(self.ik_status_publisher.setStyleSheet)
+
+    def on_ik_start_button_pressed__slot(self):
+        message = IKControlMessage()
+        message.start_button = True
+        self.button_status = True
+        self.ik_status_publisher.publish(message)
+
+
+

software/ros_packages/ground_station/src/Framework/ArmSystems/MoveItInterface.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+import roslib
+import rospkg
+import sys
+import rospy
+
+#"python_qt_binding" package hides differences between PyQt and PySide
+from python_qt_binding.QtGui import *
+from python_qt_binding.QtCore import *
+from PyQt5.QtWidgets import QApplication, QWidget, \
+    QVBoxLayout, QHBoxLayout
+import rviz
+
+class MoveItInterface(QWidget):
+    def __init__(self, parent=None):
+        QWidget.__init__(self)
+        self.frame = rviz.VisualizationFrame()
+        self.frame.setSplashPath("") #path to image file shown during loading
+        self.frame.initialize()
+
+	#load rviz config file
+        reader = rviz.YamlConfigReader()
+        config = rviz.Config()
+        reader.readFile( config, "moveit.rviz" )
+        self.frame.load( config )
+
+        self.setWindowTitle( config.mapGetChild( "MoveIt" ).getValue() )
+        self.frame.setMenuBar( None )
+        self.frame.setStatusBar( None )
+        self.frame.setHideButtonVisibility( False )
+
+	self.manager = self.frame.getManager()
+        self.grid_display = self.manager.getRootDisplayGroup().getDisplayAt( 0 )
+
+        layout = QVBoxLayout()
+        layout.addWidget( self.frame )
+        h_layout = QHBoxLayout()
+        layout.addLayout( h_layout )
+        self.setLayout( layout )

software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py

@@ -10,6 +10,7 @@
 
 from rover_arm.msg import ArmControlMessage, ArmStatusMessage
 from rover_control.msg import GripperControlMessage
+from rover_arm_control.msg import IKControlMessage
 
 #####################################
 # Global Variables
@@ -20,6 +21,9 @@
 
 GRIPPER_CONTROL_TOPIC = "/rover_control/gripper/control"
 
+ARM_CONTROLLER_STATUS = "/rover_arm_ik/rover_arm_control/IKControl/control_status"
+ARM_TOGGLE_STATUS =  "/rover_arm_ik/rover_arm_control/IKControl/button_status"
+
 THREAD_HERTZ = 5
 
 POSITIONAL_TOLERANCE = 0.02
@@ -54,6 +58,9 @@
 # UbiquitiRadioSettings Class Definition
 #####################################
 class MiscArm(QtCore.QThread):
+    # ########### define signals for slots ############
+    controller_status__signal = QtCore.pyqtSignal(str)
+
     def __init__(self, shared_objects):
         super(MiscArm, self).__init__()
 
@@ -75,6 +82,10 @@ def __init__(self, shared_objects):
 
         self.gripper_toggle_laser_button = self.left_screen.gripper_toggle_laser_button  # type:QtWidgets.QPushButton
 
+        self.ik_control_start_button = self.left_screen.ik_control_start_button #type: QtWidgets.QPushButton
+        self.ik_control_stop_button = self.left_screen.ik_control_stop_button #type: QtWidgets.QPushButton
+        self.ik_control_status_label = self.left_screen.ik_control_status_label #type : QtWidgets.QLabel
+
         # ########## Get the settings instance ##########
         self.settings = QtCore.QSettings()
 
@@ -90,13 +101,17 @@ def __init__(self, shared_objects):
         self.arm_status_subscriber = rospy.Subscriber(ARM_STATUS_TOPIC, ArmStatusMessage,
                                                       self.new_arm_status_message_received__callback)
 
+        self.ik_status_subscriber = rospy.Subscriber(ARM_CONTROLLER_STATUS, IKControlMessage, self.arm_ik_status__callback)
+
         self.arm_relative_control_publisher = rospy.Publisher(ARM_RELATIVE_CONTROL_TOPIC, ArmControlMessage,
                                                               queue_size=1)
         self.arm_absolute_control_publisher = rospy.Publisher(ARM_ABSOLUTE_CONTROL_TOPIC, ArmControlMessage,
                                                               queue_size=1)
 
         self.gripper_control_publisher = rospy.Publisher(GRIPPER_CONTROL_TOPIC, GripperControlMessage, queue_size=1)
 
+        self.ik_status_publisher = rospy.Publisher(ARM_TOGGLE_STATUS, IKControlMessage, queue_size =1)
+
         self.base_position = 0
         self.shoulder_position = 0
         self.elbow_position = 0
@@ -110,6 +125,9 @@ def __init__(self, shared_objects):
 
         self.should_package_drop = False
 
+        self.controllers_status = False
+        self.toggle_status = False
+
     def run(self):
         self.logger.debug("Starting MiscArm Thread")
 
@@ -218,6 +236,10 @@ def connect_signals_and_slots(self):
         self.gripper_toggle_light_button.clicked.connect(self.on_gripper_toggle_light_pressed)
         self.gripper_toggle_laser_button.clicked.connect(self.on_gripper_toggle_laser_pressed)
 
+        self.ik_control_start_button.clicked.connect(self.on_ik_start_button_pressed__slot)
+        self.ik_control_stop_button.clicked.connect(self.on_ik_stop_button_pressed__slot)
+        self.controller_status__signal.connect(self.ik_control_status_label.setStyleSheet)
+
     def on_upright_zeroed_button_pressed__slot(self):
         self.process_absolute_move_command([0 for _ in range(6)])
 
@@ -248,6 +270,18 @@ def on_cobra_arm_button_pressed__slot(self):
     def on_package_drop_button_pressed__slot(self):
         self.should_package_drop = True
 
+    def on_ik_start_button_pressed__slot(self):
+        message = IKControlMessage()
+        message.start_button = True
+        self.toggle_status = True
+        self.ik_status_publisher.publish(message)
+
+    def on_ik_stop_button_pressed__slot(self):
+        message = IKControlMessage()
+        message.stop_button = True
+        self.toggle_status = False
+        self.ik_status_publisher.publish(message)
+
     def on_gripper_home_pressed(self):
         message = GripperControlMessage()
         message.should_home = True
@@ -274,6 +308,17 @@ def new_arm_status_message_received__callback(self, data):
         self.wrist_pitch_position = data.wrist_pitch
         self.wrist_roll_position = data.wrist_roll
 
+    def arm_ik_status__callback(self,data):
+        self.controllers_status = data.controllers_started
+        self.button_status = data.start_button
+
+        if self.controllers_started is True:
+            ##Indicate that arm IK is started on groundstation 
+            controller_start__signal.emit(COLOR_GREEN)
+        else:
+            ##Indicate that are IK is off on groundstation 
+            controller_start__signal.emit(COLOR_RED)
+
     def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
         start_signal.connect(self.start)
         signals_and_slots_signal.connect(self.connect_signals_and_slots)