Merge pull request #85 from cpaxton/feature/ui-fixes

Fix UI issues and text

costar_instructor/instructor_core/nodes/instructor_view.py

@@ -567,7 +567,7 @@ def contract_all(self):
         self.info_textbox.hide()
 
     def calibrate_cb(self):
-        self.info_textbox.notify('Calibrating...')
+        #self.info_textbox.notify('Calibrating...')
         self.toast('Please wait... calibrating...')
         self.calibrate_button.setEnabled(False)
         service_name = '/calibrate'
@@ -576,21 +576,21 @@ def calibrate_cb(self):
         self.calibrate_button.setEnabled(True)
 
     def detect_objects_cb(self):
-        self.info_textbox.notify('Detecting objects...')
+        #self.info_textbox.notify('Detecting objects...')
         self.toast('Please wait... Detecting Objects')
         self.detect_objects_button.setEnabled(False)
         service_name = '/SPServer/SPSegmenter'
         self.send_service_command(service_name)
-        self.info_textbox.notify('Detecting objects...DONE')
+        #self.info_textbox.notify('Detecting objects...DONE')
         self.detect_objects_button.setEnabled(True)
 
     def update_scene_cb(self):
-        self.info_textbox.notify('Updating Scene objects...')
+        #self.info_textbox.notify('Updating Scene objects...')
         self.toast('Please wait... Updating Scene')
         self.update_scene_button.setEnabled(False)
         service_name = '/planningSceneGenerator/planningSceneGenerator'
         self.send_service_command(service_name)
-        self.info_textbox.notify('Updating Scene...DONE')
+        #self.info_textbox.notify('Updating Scene...DONE')
         self.update_scene_button.setEnabled(True)
 
     def stop_robot_trajectory_cb(self):
@@ -804,7 +804,7 @@ def run(self):
             self.regenerate_tree(True)
             pass
 
-    def stop_tree(self):
+    def stop_tree(self, notify=True):
         self.notification_dialog.notify('Task Tree stopped')
         self.stop_robot_trajectory_cb()
         self.run_timer_.stop()
@@ -1316,7 +1316,7 @@ def add_node_prime_gui(self):
 
     def add_child_cb(self):
         if self.current_node_type != None:
-            self.info_textbox.notify('adding node of type ' + str(self.current_node_type))
+            self.info_textbox.notify('Adding node of type ' + str(self.current_node_type))
             if self.root_node is None:
                 self.notification_dialog.notify('First create a root node under BUILDING BLOCKS', 'warn')
             if self.left_selected_node == None:
@@ -1339,7 +1339,7 @@ def add_child_cb(self):
                         self.node_leftclick_cb(current_name)
 
     def add_sibling_before_cb(self):
-        print 'adding sibling node of type ' + self.current_node_type
+        self.info_textbox.notify('Adding sibling node of type ' + self.current_node_type)
         if self.current_node_type != None:
             if self.left_selected_node == None:
               self.notification_dialog.notify('There is no left sibling node selected')
@@ -1439,7 +1439,7 @@ def clear_all(self):
             # Stop Execution if running
             if self.root_node is not None:
                 self.dot_widget.zoom_image(1, center=True)
-                self.stop_tree()
+                self.stop_tree(notify=False)
 
                 # self.root_node = None
 

costar_instructor/instructor_core/nodes/instructor_waypoint_manager.py

@@ -190,48 +190,49 @@ def check_landmarks(self):
         pass
 
     def add_waypoint(self,msg):
-        # rospy.logwarn("### ADD WAYPOINT ###")
+        name = str(msg.name).strip('/')
         if msg.relative_frame_name == '': # frame in world coordinates
-            rospy.logwarn('Adding Waypoint: ['+msg.name+']')
+            rospy.logwarn('Adding Waypoint: ['+name+']')
             if msg.name not in self.waypoints.keys():
                 self.waypoints[msg.name] = msg.world_pose
                 self.add_waypoint_marker(msg.name)
-                self.save_service(id=str(msg.name).strip('/'),type='instructor_waypoint',text=yaml.dump(msg.world_pose))
+                self.save_service(id=str(name),type='instructor_waypoint',text=yaml.dump(msg.world_pose))
                 # rospy.logwarn("### DONE ###")
-                return 'SUCCESS IN ADDING WAYPOINT'
+                return 'Added Waypoint ['+name+']'
             else:
                 # rospy.logwarn("### DONE ###")
-                return 'DUPLICATE, NOT ADDED'
+                return 'Waypoint ['+name+'] already exists!'
 
         else: # frame in relative coordinates
             if msg.name not in self.relative_waypoints.keys():
-                rospy.logwarn('Adding Relative Waypoint: ['+msg.name+'] with landmark ['+msg.relative_frame_name+']')
+                rospy.logwarn('Adding Relative Waypoint: ['+name+'] with landmark ['+msg.relative_frame_name+']')
                 self.relative_waypoints[msg.name] = [msg.relative_pose, msg.relative_frame_name]
                 self.add_relative_waypoint_marker(msg.name)
-                self.save_service(id=str(msg.name).strip('/'),type='instructor_relative_waypoint',text=yaml.dump([msg.relative_pose, msg.relative_frame_name]))
+                self.save_service(id=name,type='instructor_relative_waypoint',text=yaml.dump([msg.relative_pose, msg.relative_frame_name]))
                 # rospy.logwarn("### DONE ###")
-                return 'SUCCESS IN ADDING RELATIVE WAYPOINT'
+                return 'Added Waypoint ['+name+'] relative to  ['+msg.relative_frame_name+']'
             else:
                 # rospy.logwarn("### DONE ###")
-                return 'DUPLICATE, NOT ADDED'
+                return 'Waypoint ['+name+'] already exists!'
 
     def remove_waypoint(self,msg):
         # rospy.logwarn("### REMOVING WAYPOINT ###")
+        name = str(msg.name).strip('/')
         if msg.name in self.waypoints.keys():
             self.waypoints.pop(msg.name)
             self.remove_waypoint_marker(msg.name)
-            self.delete_service(id=str(msg.name).strip('/'),type='instructor_waypoint')
+            self.delete_service(id=name,type='instructor_waypoint')
             # rospy.logwarn("### DONE ###")
-            return 'SUCCESS IN REMOVING WAYPOINT'
+            return 'Removed Waypoint ['+name+']'
         elif msg.name in self.relative_waypoints.keys():
             self.relative_waypoints.pop(msg.name)
             self.remove_relative_waypoint_marker(msg.name)
-            self.delete_service(id=str(msg.name).strip('/'),type='instructor_relative_waypoint')
+            self.delete_service(id=name,type='instructor_relative_waypoint')
             # rospy.logwarn("### DONE ###")
-            return 'SUCCESS IN REMOVING RELATIVE WAYPOINT'
+            return 'Removed Relative Waypoint ['+name+']'
         else:
             # rospy.logwarn("### DONE ###")
-            return 'FAILED, WAPOINT NOT IN MANAGER'
+            return 'Waypoint ['+name+'] does not exist!'
 
     def get_landmark_list(self,msg):
         # rospy.logwarn("### GETTING LANDMARKS ###")

costar_instructor/instructor_core/src/instructor_core/instructor_gui_components.py

@@ -389,7 +389,9 @@ def show_hide_slide(self):
             if self.saved_geom is not None:
                 self.move(self.saved_geom.x(),self.saved_geom.y())
             else:
-                self.move(self.geometry().x()+self.geometry().width()/2-self.geometry().width()/2,self.geometry().y()+self.geometry().height()/2)
+                self.move(
+                    self.geometry().x()+self.geometry().width()/2-self.geometry().width()/2,
+                    self.geometry().y()+self.geometry().height()/2)
             self.show()
 
     def done(self):

costar_instructor/instructor_core/src/instructor_core/instructor_qt.py

@@ -65,8 +65,8 @@ def __init__(self, name, label, txtsz=12, color=Color('#222222','#ffffff'),paren
         QTextEdit.__init__(self, parent)
         self.document().contentsChanged.connect(self.sizeChange)
 
-        self.heightMin = 0
-        self.heightMax = 65000
+        self.heightMin = 100
+        self.heightMax = 300
         # self.setMouseTracking(True)
         self.name = name
         self.label = label
@@ -80,15 +80,13 @@ def __init__(self, name, label, txtsz=12, color=Color('#222222','#ffffff'),paren
         # self.setCurrentFont(self.font)
         self.setFocusPolicy(QtCore.Qt.NoFocus)
         self.setMinimumHeight(30)
+
     def set_color(self,color):
         self.color = color
         self.default_style = 'border: solid 4px #ffffff; background-color:'+self.color.normal+';color:'+'#ffffff'+';border:none;'
         self.hover_style = 'border: solid 4px #ffffff; background-color:'+self.color.hover+';color:'+'#ffffff'+';border:none;'
         self.setStyleSheet(self.default_style)
-    # def enterEvent(self, event):
-    #     self.setStyleSheet(self.hover_style)
-    # def leaveEvent(self, event):
-    #     self.setStyleSheet(self.default_style)
+
     def notify(self, message, severity='info'):
         self.show()
         self.setText(message)
@@ -98,10 +96,11 @@ def notify(self, message, severity='info'):
             rospy.logerr(message)
         else:
             rospy.loginfo(message)
+        self.sizeChange()
 
     def sizeChange(self):
         docHeight = self.document().size().height()
-        if self.heightMin <= docHeight <= self.heightMax:
+        if self.heightMin <= docHeight and docHeight <= self.heightMax:
             self.setMaximumHeight(docHeight)
 
 

costar_instructor/instructor_core/src/instructor_core/waypoint_manager_dialog.py

@@ -39,9 +39,9 @@ def __init__(self, parent=None):
         self.page_button_layout.addWidget(self.relative_page_btn)
 
         # INFO #
-        self.info_textbox = TextEdit(self,'INFO_TEXTBOX','',color=colors['gray_light'])
+        self.info_textbox = TextEdit(self,'','',color=colors['gray_light'])
         self.info_textbox.setReadOnly(True)
-        self.info_textbox.hide()
+        #self.info_textbox.hide()
         self.info_textbox.setMaximumSize(QtCore.QSize(16777215, 200))
         self.button_layout.addWidget(self.info_textbox)
 
@@ -84,15 +84,16 @@ def __init__(self, parent=None):
         self.found_rel_waypoints = []
         # Initialize
         self.update_waypoints()
+        self.show_waypoint_page()
 
     def show_waypoint_page(self):
-        self.info_textbox.hide()
+        #self.info_textbox.hide()
         self.stack.setCurrentIndex(0)
         self.mode = 'FIXED'
         self.update_waypoints()
 
     def show_relative_page(self):
-        self.info_textbox.hide()
+        #self.info_textbox.hide()
         self.stack.setCurrentIndex(1)
         self.mode = 'RELATIVE'
         self.update_waypoints()
@@ -104,13 +105,15 @@ def waypoint_name_cb(self,t):
             self.new_relative_name = str(t)
 
     def relative_waypoint_selected(self,item):
-        self.info_textbox.hide()
+        #self.info_textbox.hide()
+        pass
 
     def fixed_waypoint_selected(self,item):
-        self.info_textbox.hide()
+        #self.info_textbox.hide()
+        pass
 
     def landmark_selected(self,item):
-        self.info_textbox.hide()
+        #self.info_textbox.hide()
         if item == None:
             self.relative_page_widget.landmark_field.setText('NONE')
             self.relative_page_widget.landmark_field.setStyleSheet('background-color:'+colors['gray'].normal+';color:#ffffff')
@@ -145,7 +148,7 @@ def add_waypoint(self):
                     msg = AddWaypointRequest()
                     msg.name = '/' + self.new_fixed_name
                     msg.world_pose = tf_c.toMsg(F_waypoint)
-                    self.info_textbox.notify(add_waypoint_proxy(msg))
+                    self.info_textbox.notify(add_waypoint_proxy(msg).ack)
                     self.update_waypoints()
                     self.waypoint_page_widget.name_field.setText('')
                 except rospy.ServiceException, e:
@@ -156,7 +159,7 @@ def add_waypoint(self):
             self.info_textbox.notify('Adding Relative Waypoint with landmark ['+str(self.selected_landmark)+'] and waypoint ['+str(self.new_relative_name)+']')
             if self.selected_landmark != None:
                 landmark_frame = self.landmarks[str(self.selected_landmark)].strip('/')
-                self.info_textbox.notify(landmark_frame)
+                self.info_textbox.notify('Selected landmark [%s]'%str(landmark_frame))
                 try:
                     F_landmark = tf_c.fromTf(self.listener_.lookupTransform('/world',landmark_frame,rospy.Time(0)))
                 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
@@ -193,7 +196,7 @@ def add_waypoint(self):
                 msg.name = '/' + self.new_relative_name
                 msg.relative_pose = tf_c.toMsg(F_landmark_endpoint)
                 msg.relative_frame_name = '/'+landmark_frame
-                self.info_textbox.notify(add_waypoint_proxy(msg))
+                self.info_textbox.notify(add_waypoint_proxy(msg).ack)
                 self.update_waypoints()
                 # Reset
                 self.relative_page_widget.name_field.setText('')
@@ -228,7 +231,7 @@ def add_waypoint_seq(self):
                 # Look for existing sequence and get index if present
                 wp = [w.split('--')[0].replace('/','') for w in self.found_waypoints]
                 index = 0
-                self.info_textbox.notify('wp: '+str(wp) + '\nname: '+self.new_fixed_name)
+                self.info_textbox.notify('Waypoint: '+str(wp) + '\nname: '+self.new_fixed_name)
                 if self.new_fixed_name in wp:
                     self.info_textbox.notify('name found... ' + self.new_fixed_name)
                     index = -1
@@ -243,7 +246,7 @@ def add_waypoint_seq(self):
                     msg = AddWaypointRequest()
                     msg.name = '/' + self.new_fixed_name + '--' + str(index)
                     msg.world_pose = tf_c.toMsg(F_waypoint)
-                    self.info_textbox.notify(add_waypoint_proxy(msg))
+                    self.info_textbox.notify(add_waypoint_proxy(msg).ack)
                     self.update_waypoints()
                     # self.waypoint_page_widget.name_field.setText('')
                 except rospy.ServiceException, e:
@@ -302,7 +305,7 @@ def add_waypoint_seq(self):
                 msg.name = '/' + self.new_relative_name + '--' + str(index)
                 msg.relative_pose = tf_c.toMsg(F_landmark_endpoint)
                 msg.relative_frame_name = '/'+landmark_frame
-                self.info_textbox.notify(add_waypoint_proxy(msg))
+                self.info_textbox.notify(add_waypoint_proxy(msg).ack)
                 self.update_waypoints()
                 # Reset
                 # self.relative_page_widget.name_field.setText('')
@@ -328,7 +331,7 @@ def remove_waypoint(self):
                 remove_waypoint_proxy = rospy.ServiceProxy('/instructor_core/RemoveWaypoint',RemoveWaypoint)
                 msg = RemoveWaypointRequest()
                 msg.name = str('/' + current_selected_name)
-                self.info_textbox.notify(remove_waypoint_proxy(msg))
+                self.info_textbox.notify(remove_waypoint_proxy(msg).ack)
                 self.update_waypoints()
             except rospy.ServiceException, e:
                 self.info_textbox.notify(e,'error')
@@ -400,16 +403,16 @@ def servo_to_waypoint(self):
 
                 msg = costar_robot_msgs.srv.ServoToPoseRequest()
                 msg.target = tf_c.toMsg(F_command)
-                msg.vel = .25
-                msg.accel = .25
+                msg.vel = .75
+                msg.accel = .5
                 # Send Servo Command
-                self.info_textbox.notify('Single Servo Move Started')
+                self.info_textbox.notify('Single Servo move started...')
                 result = pose_servo_proxy(msg)
                 if 'FAILURE' in str(result.ack):
-                    self.info_textbox.notify('Servo failed with reply: '+ str(result.ack))
+                    self.info_textbox.notify('Servo move failed with reply: '+ str(result.ack))
                     return
                 else:
-                    self.info_textbox.notify('Single Servo Move Finished' + 'Robot driver reported: '+str(result.ack))
+                    self.info_textbox.notify('Single Servo move finished\n' + 'Robot driver reported: '+str(result.ack))
                     return
 
             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, rospy.ServiceException), e:

costar_instructor/instructor_plugins/src/instructor_plugins/instructor_action_relative_waypoint.py

@@ -152,7 +152,7 @@ def make_service_call(self,request,*args):
             # Send Servo Command
             rospy.loginfo('Single Servo Move Started')
             result = pose_servo_proxy(msg)
-            if 'FAILED' in str(result.ack):
+            if 'FAILURE' in str(result.ack):
                 rospy.logwarn('Servo failed with reply: '+ str(result.ack))
                 self.finished_with_success = False
                 return

costar_instructor/instructor_plugins/src/instructor_plugins/instructor_action_save_frame.py

@@ -149,7 +149,7 @@ def make_service_call(self,request,*args):
             # Send Servo Command
             rospy.loginfo('Single Servo Move Started')
             result = pose_servo_proxy(msg)
-            if 'FAILED' in str(result.ack):
+            if 'FAILURE' in str(result.ack):
                 rospy.logwarn('Servo failed with reply: '+ str(result.ack))
                 self.finished_with_success = False
                 return

costar_instructor/instructor_plugins/src/instructor_plugins/instructor_plan_relative_waypoint.py

@@ -154,7 +154,7 @@ def make_service_call(self,request,*args):
             # Send Servo Command
             rospy.logwarn('Single Planned Move Started')
             result = pose_servo_proxy(msg)
-            if 'FAILED' in str(result.ack):
+            if 'FAILURE' in str(result.ack):
                 rospy.logwarn('Servo failed with reply: '+ str(result.ack))
                 self.finished_with_success = False
                 return

costar_instructor/instructor_plugins/src/instructor_plugins/instructor_plan_waypoint.py

@@ -152,7 +152,7 @@ def make_service_call(self,request,*args):
             # Send Servo Command
             rospy.loginfo('Single Planned Move Started')
             result = pose_servo_proxy(msg)
-            if 'FAILED' in str(result.ack):
+            if 'FAILURE' in str(result.ack):
                 rospy.logwarn('Planned move failed with reply: '+ str(result.ack))
                 self.finished_with_success = False
                 return

costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py

@@ -380,7 +380,7 @@ def send_and_publish_planning_result(self,res,stamp,acceleration,velocity):
 
         else:
             rospy.logerr('DRIVER -- PLANNING failed')
-            return 'FAILURE -- not in servo mode'
+            return 'FAILURE -- planning failed'
 
 
     '''