object_function.py
Go to the documentation of this file.
00001 #!/bin/env python
00002 
00003 from functools import partial
00004 import math
00005 import os.path
00006 from python_qt_binding.QtCore import QPoint, QPointF, QRect, Qt
00007 from python_qt_binding.QtGui import QLabel, QLineEdit, QPainter, QPolygon, QPushButton
00008 import rospy
00009 import yaml
00010 
00011 from .utils import clearLayoutAndFixHeight, transformPointToPixelCoordinates, transformPointToRealWorldCoordinates
00012 
00013 class Object(object):
00014 
00015     def __init__(self,
00016                  location,
00017                  orientation):
00018 
00019         self.location = location
00020         self.orientation = orientation
00021 
00022     def clone(self):
00023         return Object(QPoint(self.location), self.orientation)
00024 
00025 class ObjectFunction(object):
00026 
00027     EDIT_OBJECT_PROPERITIES = 'Edit Object Properties'
00028     ADD_NEW_OBJECT = 'Add Object'
00029     EDIT_EXISTING_OBJECT = 'Edit Object'
00030 
00031     ORIENTATION_LENGTH = 10
00032 
00033     def __init__(self,
00034                  object_file,
00035                  map,
00036                  location_function,
00037                  widget,
00038                  subfunction_layout,
00039                  configuration_layout,
00040                  image):
00041 
00042         self.edit_object_location_button = None
00043         self.selected_object_color = Qt.blue
00044         self.unselected_object_color = Qt.darkGreen
00045 
00046         # Dictionary that maps object names to the actual object object (string->Object)
00047         self.objects = {}
00048         self.draw_object = {}
00049         self.unique_loc_counter = 1
00050 
00051         self.editing_object_location = False
00052         self.edit_existing_object = None
00053 
00054         self.editing_properties = False
00055         self.edit_properties_object = None
00056 
00057         # Use this to initialize variables.
00058         self.clearCurrentSelection()
00059 
00060         self.is_modified = False
00061 
00062         self.widget = widget
00063         self.subfunction_layout = subfunction_layout
00064         self.image = image
00065         self.image_size = image.overlay_image.size()
00066         self.configuration_layout = configuration_layout
00067 
00068         self.object_file = object_file
00069         self.map = map
00070         self.location_function = location_function
00071         self.readObjectsFromFile()
00072 
00073         self.edit_object_location_button = {}
00074 
00075     def readObjectsFromFile(self):
00076 
00077         if os.path.isfile(self.object_file):
00078             stream = open(self.object_file, 'r')
00079             try:
00080                 contents = yaml.load(stream)
00081                 for object in contents:
00082                     object_key = object["name"]
00083                     object_location = QPointF(object["point"][0], object["point"][1])
00084                     object_location = transformPointToPixelCoordinates(object_location, self.map, self.image_size)
00085                     object_orientation = float(object["point"][2])
00086                     self.objects[object_key] = Object(object_location, object_orientation)
00087                     self.draw_object[object_key] = True
00088             except yaml.YAMLError, KeyError:
00089                 rospy.logerr("File found at " + self.object_file + ", but cannot be parsed by YAML parser. I'm starting objects from scratch.")
00090 
00091             stream.close()
00092         else:
00093             rospy.logwarn("Object file not found at " + self.object_file + ". I'm starting objects from scratch and will attempt to write to this object before exiting.")
00094 
00095     def saveConfiguration(self):
00096         self.writeObjectsToFile()
00097 
00098     def writeObjectsToFile(self):
00099 
00100         out_list = []
00101         for object_name in self.objects:
00102             object = self.objects[object_name]
00103             object_location = transformPointToRealWorldCoordinates(object.location, self.map, self.image_size)
00104             object_dict = {}
00105             object_dict["name"] = object_name
00106             object_dict["point"] = [object_location.x(), object_location.y(), object.orientation]
00107             out_list.append(object_dict)
00108 
00109         stream = open(self.object_file, 'w')
00110         yaml.dump(out_list, stream)
00111         stream.close()
00112 
00113         self.is_modified = False
00114 
00115     def deactivateFunction(self):
00116 
00117         if self.editing_object_location:
00118             self.endObjectLocationEdit("Cancel")
00119         elif self.editing_properties:
00120             self.endPropertyEdit()
00121 
00122         clearLayoutAndFixHeight(self.subfunction_layout)
00123         self.edit_object_location_button.clear()
00124         self.image.enableDefaultMouseHooks()
00125 
00126         # Just in case we were editing a object, that object was not being drawn. 
00127         for object in self.draw_object:
00128             self.draw_object[object] = True
00129 
00130     def activateFunction(self):
00131 
00132         # Add all the necessary buttons to the subfunction layout.
00133         clearLayoutAndFixHeight(self.subfunction_layout)
00134         for button_text in [ObjectFunction.ADD_NEW_OBJECT, 
00135                             ObjectFunction.EDIT_EXISTING_OBJECT]:
00136             button = QPushButton(button_text, self.widget)
00137             button.clicked[bool].connect(partial(self.startObjectLocationEdit, button_text))
00138             button.setCheckable(True)
00139             self.subfunction_layout.addWidget(button)
00140             self.edit_object_location_button[button_text] = button
00141         self.edit_object_location_button[ObjectFunction.EDIT_EXISTING_OBJECT].setEnabled(False)
00142         self.subfunction_layout.addStretch(1)
00143 
00144         # ActivateMouseHooks.
00145         self.image.mousePressEvent = self.mousePressEvent
00146         self.image.mouseMoveEvent = self.mouseMoveEvent
00147         self.image.mouseReleaseEvent = self.mouseReleaseEvent
00148 
00149         self.updateOverlay()
00150 
00151     def getObjectNameFromPoint(self, point):
00152         for object in self.objects:
00153             # Check if the user is clicking on the line between the objects or the two approach points.
00154             if self.getPointDistanceToAnotherPoint(point, self.objects[object].location) <= 3:
00155                 return object
00156         return None
00157 
00158     def startObjectLocationEdit(self, edit_type):
00159 
00160         if self.editing_properties:
00161             self.endPropertyEdit()
00162 
00163         self.editing_object_location = True
00164 
00165         if edit_type == ObjectFunction.ADD_NEW_OBJECT:
00166             self.edit_existing_object = None
00167         # else edit_existing_object was set to the correct object by startPropertyEdit()
00168 
00169         # Make sure all active selections have been cleared.
00170         self.clearCurrentSelection()
00171 
00172         # If we're going to edit an existing area, stop drawing it and copy it to the active selection.
00173         if self.edit_existing_object is not None:
00174             self.draw_object[self.edit_existing_object] = False 
00175             self.current_selection = self.objects[self.edit_existing_object].clone()
00176             self.edit_existing_object = self.edit_existing_object
00177 
00178         # Setup the buttons in the configuration toolbar, and disable the original buttons to edit an area.
00179         clearLayoutAndFixHeight(self.configuration_layout)
00180         for button_text in ["Done", "Cancel"]:
00181             button = QPushButton(button_text, self.widget)
00182             button.clicked[bool].connect(partial(self.endObjectLocationEdit, button_text))
00183             self.configuration_layout.addWidget(button)
00184         self.current_selection_label = QLabel(self.widget)
00185         self.configuration_layout.addWidget(self.current_selection_label)
00186         self.configuration_layout.addStretch(1)
00187 
00188         self.edit_object_location_button[ObjectFunction.ADD_NEW_OBJECT].setEnabled(False)
00189         self.edit_object_location_button[ObjectFunction.EDIT_EXISTING_OBJECT].setEnabled(False)
00190 
00191         self.updateOverlay()
00192 
00193     def clearCurrentSelection(self):
00194 
00195         # Make sure all selections are clear.
00196         self.new_selection_start_point = None
00197         self.new_selection_end_point = None
00198 
00199         # Object
00200         self.current_selection = None
00201         self.current_selection_label = None # Displays which two locations the object is connecting.
00202         self.move_selection = None
00203 
00204     def endObjectLocationEdit(self, button_text):
00205 
00206         edit_properties_object = None
00207 
00208         if (button_text == "Done") and (self.current_selection is not None):
00209 
00210             if self.edit_existing_object == None:
00211                 # We're adding a new object. Generate a new object name and color.
00212                 self.edit_existing_object = self.getUniqueName()
00213             self.objects[self.edit_existing_object] = self.current_selection
00214             self.draw_object[self.edit_existing_object] = True
00215             edit_properties_object = self.edit_existing_object
00216 
00217             # Since a object was added or edited, set file modification to true.
00218             self.is_modified = True
00219         else:
00220             # Cancel was pressed, draw the original object if we were editing as before.
00221             if self.edit_existing_object is not None:
00222                 self.draw_object[self.edit_existing_object] = True
00223 
00224         self.editing_object_location = False
00225         self.edit_existing_object = None
00226         self.clearCurrentSelection()
00227 
00228         # Update the entire image overlay.
00229         self.updateOverlay()
00230 
00231         self.edit_object_location_button[ObjectFunction.ADD_NEW_OBJECT].setEnabled(True)
00232         self.edit_object_location_button[ObjectFunction.ADD_NEW_OBJECT].setChecked(False)
00233         self.edit_object_location_button[ObjectFunction.EDIT_EXISTING_OBJECT].setChecked(False)
00234         clearLayoutAndFixHeight(self.configuration_layout)
00235 
00236         if edit_properties_object is not None:
00237             self.edit_properties_object = edit_properties_object
00238             self.startPropertyEdit()
00239 
00240     def startPropertyEdit(self):
00241 
00242         self.editing_properties = True
00243         self.edit_existing_object = self.edit_properties_object
00244 
00245         self.edit_object_location_button[ObjectFunction.ADD_NEW_OBJECT].setEnabled(True)
00246         self.edit_object_location_button[ObjectFunction.EDIT_EXISTING_OBJECT].setEnabled(True)
00247 
00248         # Construct the configuration layout.
00249         clearLayoutAndFixHeight(self.configuration_layout)
00250 
00251         location_text = self.getObjectLocationText(self.objects[self.edit_properties_object])
00252         self.update_name_label = QLabel("Object (" + self.edit_properties_object + " - " + location_text + ")      New Name: ", self.widget)
00253         self.configuration_layout.addWidget(self.update_name_label)
00254 
00255         self.update_name_textedit = QLineEdit(self.widget)
00256         self.update_name_textedit.setText(self.edit_properties_object)
00257         self.update_name_textedit.textEdited.connect(self.objectNameTextEdited)
00258         self.configuration_layout.addWidget(self.update_name_textedit)
00259 
00260         self.update_name_button = QPushButton("Update object Name", self.widget)
00261         self.update_name_button.clicked[bool].connect(self.updateObjectName)
00262         self.update_name_button.setEnabled(False)
00263         self.configuration_layout.addWidget(self.update_name_button)
00264 
00265         self.remove_object_button = QPushButton("Remove Object", self.widget)
00266         self.remove_object_button.clicked[bool].connect(self.removeCurrentObject)
00267         self.configuration_layout.addWidget(self.remove_object_button)
00268 
00269         self.configuration_layout.addStretch(1)
00270 
00271         self.updateOverlay()
00272 
00273     def endPropertyEdit(self):
00274 
00275         self.edit_object_location_button[ObjectFunction.ADD_NEW_OBJECT].setEnabled(True)
00276         self.edit_object_location_button[ObjectFunction.EDIT_EXISTING_OBJECT].setEnabled(False)
00277 
00278         clearLayoutAndFixHeight(self.configuration_layout)
00279 
00280         self.update_name_label = None
00281         self.update_name_textedit = None
00282         self.update_name_button = None
00283 
00284         self.editing_properties = False
00285 
00286         self.edit_properties_object = None
00287 
00288         self.updateOverlay()
00289 
00290     def objectNameTextEdited(self, text):
00291         if str(text) != self.edit_properties_object and str(text) not in self.objects:
00292             self.update_name_button.setEnabled(True)
00293         else:
00294             self.update_name_button.setEnabled(False)
00295 
00296     def updateObjectName(self):
00297         old_obj_name = self.edit_properties_object
00298         new_obj_name = str(self.update_name_textedit.text())
00299 
00300         # This is a simple rename task.
00301         self.objects[new_obj_name] = self.objects.pop(old_obj_name)
00302         self.draw_object[new_obj_name] = self.draw_object.pop(old_obj_name)
00303 
00304         # Since a object name was modified, set file modification to true.
00305         self.is_modified = True
00306 
00307         # Restart property edit with the updated name.
00308         self.endPropertyEdit()
00309         self.edit_properties_object = new_obj_name
00310         self.startPropertyEdit()
00311 
00312     def removeCurrentObject(self):
00313         old_obj_name = self.edit_properties_object
00314         self.removeObject(old_obj_name)
00315         self.endPropertyEdit()
00316         self.updateOverlay()
00317 
00318         # Since a object was removed, set file modification to true.
00319         self.is_modified = True
00320 
00321     def removeObject(self, obj_name):
00322         if obj_name in self.objects:
00323             self.objects.pop(obj_name)
00324         if obj_name in self.draw_object:
00325             self.draw_object.pop(obj_name)
00326 
00327     def isModified(self):
00328         return self.is_modified
00329 
00330     def mousePressEvent(self, event):
00331         if self.editing_object_location:
00332             old_selection_location = None
00333             if self.current_selection is not None:
00334                 # First make sure we copy the region corresponding to the old point.
00335                 old_selection_location = QPoint(self.current_selection.location)
00336             self.current_selection = Object(event.pos(), 0)
00337             if old_selection_location is None:
00338                 overlay_update_region = self.getRectangularPolygon(self.current_selection.location,
00339                                                                    self.current_selection.location).boundingRect()
00340             else:
00341                 overlay_update_region = self.getRectangularPolygon(old_selection_location,
00342                                                                    self.current_selection.location).boundingRect()
00343 
00344             overlay_update_region.setTopLeft(QPoint(overlay_update_region.topLeft().x() - (ObjectFunction.ORIENTATION_LENGTH + 1),
00345                                                     overlay_update_region.topLeft().y() - (ObjectFunction.ORIENTATION_LENGTH + 1)))
00346             overlay_update_region.setBottomRight(QPoint(overlay_update_region.bottomRight().x() + (ObjectFunction.ORIENTATION_LENGTH + 1),
00347                                                         overlay_update_region.bottomRight().y() + (ObjectFunction.ORIENTATION_LENGTH + 1)))
00348 
00349             self.current_selection_label.setText(self.getObjectLocationText(self.current_selection))
00350             self.updateOverlay(overlay_update_region)
00351         else:
00352             loc = self.getObjectNameFromPoint(event.pos()) 
00353             if loc is not None:
00354                 self.edit_properties_object = loc
00355                 self.startPropertyEdit()
00356             else:
00357                 self.endPropertyEdit()
00358 
00359     def mouseReleaseEvent(self, event):
00360         if self.editing_object_location:
00361             self.mouseMoveEvent(event)
00362 
00363     def mouseMoveEvent(self, event):
00364 
00365         if self.editing_object_location:
00366             diff = event.pos() - self.current_selection.location
00367             self.current_selection.orientation = math.atan2(diff.y(), diff.x())
00368             overlay_update_region = QRect(QPoint(self.current_selection.location.x() - (ObjectFunction.ORIENTATION_LENGTH + 1),
00369                                                  self.current_selection.location.y() - (ObjectFunction.ORIENTATION_LENGTH + 1)),
00370                                           QPoint(self.current_selection.location.x() + (ObjectFunction.ORIENTATION_LENGTH + 1),
00371                                                  self.current_selection.location.y() + (ObjectFunction.ORIENTATION_LENGTH + 1)))
00372             self.updateOverlay(overlay_update_region)
00373 
00374     def updateOverlay(self, rect = None):
00375 
00376         # Redraw the overlay image from scratch using the object image and current object.
00377 
00378         self.image.overlay_image.fill(Qt.transparent)
00379         painter = QPainter(self.image.overlay_image)
00380         painter.setBackgroundMode(Qt.TransparentMode)
00381         painter.setCompositionMode(QPainter.CompositionMode_Source)
00382 
00383         for object in self.objects:
00384             if self.draw_object[object]:
00385                 color = self.unselected_object_color
00386                 if self.edit_properties_object == object and self.editing_properties:
00387                     color = self.selected_object_color
00388                 self.drawObject(self.objects[object], painter, color)
00389 
00390         if self.current_selection is not None:
00391             color = self.selected_object_color
00392             self.drawObject(self.current_selection, painter, color)
00393         painter.end()
00394 
00395         if rect is None:
00396             self.image.update()
00397         else:
00398             self.image.update(rect)
00399 
00400     def getObjectLocationText(self, object):
00401         # Get the two locations this object connects by looking up the locations of the approach points in the 
00402         # object function.
00403         location = self.location_function.getLocationNameFromPoint(object.location)
00404         if location is None:
00405             location = "<None>"
00406         return "Lies in: " + location 
00407 
00408     def getUniqueName(self):
00409         name = "new_object" + str(self.unique_loc_counter)
00410         self.unique_loc_counter += 1
00411         return name
00412 
00413     def getPointDistanceToAnotherPoint(self, pt1, pt2):
00414         diff = pt1 - pt2
00415         return math.sqrt(diff.x() * diff.x() + diff.y() * diff.y())
00416 
00417     def getRectangularPolygon(self, pt1, pt2):
00418         return QPolygon([pt1, QPoint(pt1.x(), pt2.y()), pt2, QPoint(pt2.x(), pt1.y())])
00419 
00420     def drawObject(self, obj, painter, color):
00421         self.drawPoint(obj.location, painter, color)
00422         orientation_pt = obj.location + QPoint(ObjectFunction.ORIENTATION_LENGTH * math.cos(obj.orientation),
00423                                                ObjectFunction.ORIENTATION_LENGTH * math.sin(obj.orientation))
00424         painter.drawLine(obj.location, orientation_pt)
00425 
00426     def drawPoint(self, pt, painter, color):
00427         painter.setPen(color)
00428         painter.drawPoint(pt)
00429         painter.drawEllipse(pt, 3, 3)


bwi_planning_common
Author(s): Piyush Khandelwal
autogenerated on Fri Aug 28 2015 10:14:40