Merge pull request #82 from cpaxton/devel

UI fixes

costar_instructor/instructor_core/nodes/instructor_view.py

@@ -764,7 +764,7 @@ def run_tree(self):
                 self.sound_pub.publish(String("notify_4"))
                 self.running__ = True
                 self.run_timer_.start(5)
-                self.info_textbox.notify('INSTRUCTOR: Task Tree STARTING')
+                self.info_textbox.notify('Task Tree starting')
                 self.run_button.setStyleSheet('''QPushButton#run_button{border: 2px solid #F62459;border-radius: 0px;background-color: #F62459;color:#ffffff}QPushButton#run_button:pressed{border: 2px solid #F62459;border-radius: 0px;background-color: #F62459;color:#ffffff}''')
                 self.run_button.setText('STOP EXECUTION')
 
@@ -774,7 +774,7 @@ def run(self):
         #rospy.logwarn(result)
         # self.regenerate_tree()
         if result[:7] == 'SUCCESS':
-            self.notification_dialog.notify('INSTRUCTOR: Task Tree FINISHED WITH SUCCESS: %s'%result)
+            self.notification_dialog.notify('Task Tree finished with %s'%result)
             self.sound_pub.publish(String("notify_4_succeed"))
             self.run_timer_.stop()
             self.running__ = False
@@ -783,7 +783,7 @@ def run(self):
             self.run_button.setText('EXECUTE PLAN')
             self.regenerate_tree()
         elif result[:7] == 'FAILURE':
-            self.notification_dialog.notify('INSTRUCTOR: Task Tree FINISHED WITH FAILURE: %s'%result,'error')
+            self.notification_dialog.notify('Task Tree finished with %s'%result,'error')
             self.run_timer_.stop()
             self.running__ = False
             # self.root_node.reset()
@@ -793,7 +793,7 @@ def run(self):
             rospy.sleep(.5)
             self.sound_pub.publish(String("notify_4_fail"))
         elif result == 'NODE_ERROR':
-            self.notification_dialog.notify('INSTRUCTOR: Task Tree ERROR','error')
+            self.notification_dialog.notify('Task Tree error','error')
             self.run_timer_.stop()
             self.running__ = False
             # self.root_node.reset()
@@ -805,7 +805,7 @@ def run(self):
             pass
 
     def stop_tree(self):
-        self.notification_dialog.notify('INSTRUCTOR: Task Tree STOPPED')
+        self.notification_dialog.notify('Task Tree stopped')
         self.stop_robot_trajectory_cb()
         self.run_timer_.stop()
         self.running__ = False
@@ -935,8 +935,8 @@ def load_selected_subtree(self):
 
                     self.info_textbox.notify('Tree ' + self.selected_subtree.upper() + ' loaded.')
                 except Exception as e:
-                    self.notification_dialog.notify(load_error_message, 'error')
-                    rospy.logerr(str(e))
+                    # TODO(ahundt): set the correct error message here
+                    self.notification_dialog.notify('Failed to load tree with %s'%str(e), 'error')
 
             # Finish
             self.load_sub_btn.hide()

costar_instructor/instructor_core/src/instructor_core/instructor_gui_components.py

@@ -375,11 +375,11 @@ def notify(self, message, severity='warn'):
              self.show_hide_slide()
 
         if severity is 'warn':
-            rospy.logwarn(message)
+            rospy.logwarn("Instructor: " + message)
         elif severity is 'error':
-            rospy.logerr(message)
+            rospy.logerr("Instructor: " + message)
         else:
-            rospy.loginfo(message)
+            rospy.loginfo("Instructor: " + message)
 
     def show_hide_slide(self):
         if self.isVisible():

costar_instructor/instructor_core/src/instructor_core/robot_interface.py

@@ -66,49 +66,50 @@ def teach(self):
             try:
                 rospy.wait_for_service('/costar/SetTeachMode',2)
             except rospy.ROSException as e:
-                print 'Could not find teach service'
+                rospy.logerr('Could not find teach service')
                 return
             try:
                 teach_mode_service = rospy.ServiceProxy('/costar/SetTeachMode',SetTeachMode)
                 result = teach_mode_service(True)
-                # self.sound_pub.publish(String("low_up"))
-                rospy.logwarn(result.ack)
                 self.teach_btn.set_color(colors['gray_light'])
             except rospy.ServiceException, e:
-                print e
+                rospy.logerr(str(e))
         elif self.driver_status == 'TEACH':
-            try:
-                rospy.wait_for_service('/costar/SetTeachMode',2)
-            except rospy.ROSException as e:
-                print 'Could not find teach service'
-                return
-            try:
-                teach_mode_service = rospy.ServiceProxy('/costar/SetTeachMode',SetTeachMode)
-                result = teach_mode_service(False)
-                rospy.logwarn(result.ack)
-                # self.sound_pub.publish(String("low_down"))
-                self.teach_btn.set_color(colors['gray'])
-            except rospy.ServiceException, e:
-                print e
+            self.disable_teach_mode()
         else:
-            rospy.logwarn('FAILED, driver is in ['+self.driver_status+'] mode.')
+            rospy.logerr('FAILED, driver is in ['+self.driver_status+'] mode.')
             self.toast('Driver is in ['+self.driver_status+'] mode!')
 
+    def disable_teach_mode(self):
+        try:
+            rospy.wait_for_service('/costar/SetTeachMode',2)
+        except rospy.ROSException as e:
+            rospy.logerr('Could not find teach service')
+            return
+        try:
+            teach_mode_service = rospy.ServiceProxy('/costar/SetTeachMode',SetTeachMode)
+            result = teach_mode_service(False)
+            rospy.logwarn(result.ack)
+            self.teach_btn.set_color(colors['gray'])
+        except rospy.ServiceException, e:
+            rospy.logerr(str(e))
+
     def servo(self):
+        if self.driver_status == 'TEACH':
+            self.disable_teach_mode()
         if self.driver_status == 'IDLE' or self.driver_status == 'TEACH':
             try:
                 rospy.wait_for_service('/costar/SetServoMode',2)
             except rospy.ROSException as e:
-                print 'Could not find SetServoMode service'
+                rospy.logerr('Could not find SetServoMode service')
                 return
             try:
                 servo_mode_service = rospy.ServiceProxy('/costar/SetServoMode',SetServoMode)
                 result = servo_mode_service('SERVO')
                 rospy.logwarn(result.ack)
                 self.servo_btn.set_color(colors['gray_light'])
             except rospy.ServiceException, e:
-                print e
-
+                rospy.logerr(str(e))
         elif self.driver_status == 'SERVO':
             try:
                 rospy.wait_for_service('/costar/SetServoMode',2)

costar_instructor/instructor_plugins/src/instructor_plugins/service_node.py

@@ -28,6 +28,7 @@ class ServiceNode(Node):
 
     def __init__(self, name, label, color,service_description, display_name = None):
         super(ServiceNode,self).__init__(name,label,color)
+        self.ready_color = color
         self.status_msg = ''
         self.running = False
         self.finished_with_success = None
@@ -46,7 +47,7 @@ def reset_self(self):
         self.running = False
         self.finished_with_success = None
         self.needs_reset = False
-        self.set_color(self.color_)
+        self.set_color(self.ready_color)
 
     def execute(self):
         if self.display_name is not None:
@@ -89,6 +90,6 @@ def execute(self):
                         self.needs_reset = True
                         return self.set_status('FAILURE -- %s'%self.status_msg)
 
-    def make_service_call(self,request,*args):
-        raise NotImplementedError('make_service_call must be implemented in child class')
+    #def make_service_call(self,request,*args):
+    #    raise NotImplementedError('make_service_call must be implemented in child class')