location_function.py
Go to the documentation of this file.
00001 #!/bin/env python
00002 
00003 from functools import partial
00004 import os.path
00005 from python_qt_binding.QtCore import QPoint, QSize, Qt
00006 from python_qt_binding.QtGui import QImage, QLabel, QLineEdit, QPainter, QPolygon, QPushButton, QColor
00007 import rospy
00008 import yaml
00009 
00010 from .utils import clearLayoutAndFixHeight, getLocationsImageFileLocationFromDataDirectory, scalePoint, scalePolygon
00011 
00012 class LocationFunction(object):
00013 
00014     EDIT_LOCATION_PROPERITIES = 'Edit Location Properties'
00015     ADD_LOCATION_AREA = 'Add Location'
00016     EDIT_EXISTING_AREA = 'Edit Location'
00017 
00018     def __init__(self,
00019                  location_file,
00020                  map,
00021                  widget,
00022                  subfunction_layout,
00023                  configuration_layout,
00024                  image):
00025 
00026         self.edit_area_button = None
00027         self.edit_area_selection_color = Qt.black
00028 
00029         # Dictionary of polygons
00030         self.locations = {}
00031         # Dictionary that maps location names to their colors
00032         self.location_colors = {}
00033         self.draw_location = {}
00034         self.unique_loc_counter = 1
00035 
00036         self.editing_area = False
00037         self.edit_existing_location = None
00038 
00039         self.editing_properties = False
00040         self.edit_properties_location = None
00041 
00042         # Use this to initialize variables.
00043         self.clearAreaSelection()
00044 
00045         self.is_modified = False
00046 
00047         self.widget = widget
00048         self.subfunction_layout = subfunction_layout
00049         self.image = image
00050         self.image_size = image.overlay_image.size()
00051         self.configuration_layout = configuration_layout
00052 
00053         self.location_file = location_file
00054         self.map_size = QSize(map.map.info.width, map.map.info.height)
00055         self.readLocationsFromFile()
00056 
00057         self.edit_area_button = {}
00058 
00059     def readLocationsFromFile(self):
00060 
00061         if os.path.isfile(self.location_file):
00062             stream = open(self.location_file, 'r')
00063             try:
00064                 contents = yaml.load(stream)
00065                 if "polygons" not in contents or "locations" not in contents:
00066                     rospy.logerr("YAML file found at " + self.location_file + ", but does not seem to have been written by this tool. I'm starting locations from scratch.")
00067                 else:
00068                     location_keys = contents["locations"]
00069                     location_polygons = contents["polygons"]
00070                     for index, location in enumerate(location_keys):
00071                         self.locations[location] = QPolygon()
00072                         self.locations[location].setPoints(location_polygons[index])
00073                         self.locations[location] = scalePolygon(self.locations[location], 
00074                                                                 self.map_size,
00075                                                                 self.image_size)
00076                         (_,self.location_colors[location]) = self.getUniqueNameAndColor()
00077                         self.draw_location[location] = True
00078             except yaml.YAMLError:
00079                 rospy.logerr("File found at " + self.location_file + ", but cannot be parsed by YAML parser. I'm starting locations from scratch.")
00080 
00081             stream.close()
00082         else:
00083             rospy.logwarn("Location file not found at " + self.location_file + ". I'm starting locations from scratch and will attempt to write to this location before exiting.")
00084 
00085     def saveConfiguration(self):
00086         self.writeLocationsToFile()
00087 
00088     def writeLocationsToFile(self):
00089 
00090         out_dict = {}
00091         out_dict["locations"] = self.locations.keys()
00092         out_dict["polygons"] = []
00093         for index, location in enumerate(self.locations):
00094             out_dict["polygons"].append([])
00095             for i in range(self.locations[location].size()):
00096                 pt = self.locations[location].point(i)
00097                 scaled_pt = scalePoint(pt, self.image_size, self.map_size)
00098                 out_dict["polygons"][index].append(scaled_pt.x())
00099                 out_dict["polygons"][index].append(scaled_pt.y())
00100 
00101         data_directory = os.path.dirname(os.path.realpath(self.location_file))
00102         image_file = getLocationsImageFileLocationFromDataDirectory(data_directory)
00103 
00104         # Create an image with the location data, so that C++ programs don't need to rely on determining regions using polygons.
00105         out_dict["data"] = 'locations.pgm'
00106         location_image = QImage(self.map_size, QImage.Format_RGB32)
00107         location_image.fill(Qt.white)
00108         painter = QPainter(location_image) 
00109         for index, location in enumerate(self.locations):
00110             if index > 254:
00111                 rospy.logerr("You have more than 254 locations, which is unsupported by the bwi_planning_common C++ code!")
00112             painter.setPen(Qt.NoPen)
00113             painter.setBrush(QColor(index, index, index))
00114             scaled_polygon = scalePolygon(self.locations[location], self.image_size, self.map_size)
00115             painter.drawPolygon(scaled_polygon)
00116         painter.end()
00117         location_image.save(image_file)
00118 
00119         stream = open(self.location_file, 'w')
00120         yaml.dump(out_dict, stream)
00121         stream.close()
00122 
00123         self.is_modified = False
00124 
00125     def deactivateFunction(self):
00126 
00127         if self.editing_area:
00128             self.endAreaEdit("Cancel")
00129         elif self.editing_properties:
00130             self.endPropertyEdit()
00131 
00132         clearLayoutAndFixHeight(self.subfunction_layout)
00133         self.edit_area_button.clear()
00134         self.image.enableDefaultMouseHooks()
00135 
00136         # Just in case we were editing a location, that location was not being drawn. 
00137         for location in self.draw_location:
00138             self.draw_location[location] = True
00139 
00140     def activateFunction(self):
00141 
00142         # Add all the necessary buttons to the subfunction layout.
00143         clearLayoutAndFixHeight(self.subfunction_layout)
00144         for button_text in [LocationFunction.ADD_LOCATION_AREA, 
00145                             LocationFunction.EDIT_EXISTING_AREA]:
00146             button = QPushButton(button_text, self.widget)
00147             button.clicked[bool].connect(partial(self.startAreaEdit, button_text))
00148             button.setCheckable(True)
00149             self.subfunction_layout.addWidget(button)
00150             self.edit_area_button[button_text] = button
00151         self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(False)
00152         self.subfunction_layout.addStretch(1)
00153 
00154         # ActivateMouseHooks.
00155         self.image.mousePressEvent = self.mousePressEvent
00156         self.image.mouseMoveEvent = self.mouseMoveEvent
00157         self.image.mouseReleaseEvent = self.mouseReleaseEvent
00158 
00159         self.updateOverlay()
00160 
00161     def getLocationNameFromPoint(self, point):
00162         for location in self.locations:
00163             if self.locations[location].containsPoint(point, Qt.OddEvenFill):
00164                 return location
00165         return None
00166 
00167     def startAreaEdit(self, edit_type):
00168 
00169         if self.editing_properties:
00170             self.endPropertyEdit()
00171 
00172         self.editing_area = True
00173 
00174         if edit_type == LocationFunction.ADD_LOCATION_AREA:
00175             self.edit_existing_location = None
00176         # else edit_existing_location was set to the correct location by startPropertyEdit()
00177 
00178         # Make sure all active selections have been cleared.
00179         self.clearAreaSelection()
00180 
00181         # If we're going to edit an existing area, stop drawing it and copy it to the active selection.
00182         if self.edit_existing_location is not None:
00183             self.draw_location[self.edit_existing_location] = False 
00184             self.current_selection = QPolygon(self.locations[self.edit_existing_location])
00185             self.edit_existing_location = self.edit_existing_location
00186 
00187         # Setup the buttons in the configuration toolbar, and disable the original buttons to edit an area.
00188         clearLayoutAndFixHeight(self.configuration_layout)
00189         for button_text in ["Done", "Cancel"]:
00190             button = QPushButton(button_text, self.widget)
00191             button.clicked[bool].connect(partial(self.endAreaEdit, button_text))
00192             self.configuration_layout.addWidget(button)
00193         self.configuration_layout.addStretch(1)
00194 
00195         self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(False)
00196         self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(False)
00197 
00198         self.updateOverlay()
00199 
00200     def clearAreaSelection(self):
00201 
00202         # Make sure all selections are clear.
00203         self.new_selection_start_point = None
00204         self.new_selection_end_point = None
00205 
00206         # QPolygons to track current location.
00207         self.current_selection = None
00208         self.new_selection = None
00209         self.subtract_new_selection = None
00210 
00211     def endAreaEdit(self, button_text):
00212 
00213         edit_properties_location = None
00214 
00215         if (button_text == "Done") and (self.current_selection is not None) and (not self.current_selection.isEmpty()):
00216 
00217             # If the current location being added completely wipes out an old location, make sure you remove it.
00218             for location in self.locations.keys():
00219                 if location != self.edit_existing_location:
00220                     self.locations[location] = self.locations[location].subtracted(self.current_selection) 
00221                     if self.locations[location].isEmpty():
00222                         self.removeLocation(location)
00223 
00224             if self.edit_existing_location == None:
00225                 # We're adding a new location. Generate a new location name and color.
00226                 (self.edit_existing_location, new_location_color) = self.getUniqueNameAndColor()
00227                 self.location_colors[self.edit_existing_location] = new_location_color
00228             self.locations[self.edit_existing_location] = self.current_selection
00229             self.draw_location[self.edit_existing_location] = True
00230             edit_properties_location = self.edit_existing_location
00231 
00232             # Since a location was added or edited, set file modification to true.
00233             self.is_modified = True
00234         else:
00235             # Cancel was pressed, draw the original location if we were editing as before.
00236             if self.edit_existing_location is not None:
00237                 self.draw_location[self.edit_existing_location] = True
00238 
00239         self.editing_area = False
00240         self.edit_existing_location = None
00241         self.clearAreaSelection()
00242 
00243         # Update the entire image overlay.
00244         self.updateOverlay()
00245 
00246         self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(True)
00247         self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setChecked(False)
00248         self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setChecked(False)
00249         clearLayoutAndFixHeight(self.configuration_layout)
00250 
00251         if edit_properties_location is not None:
00252             self.edit_properties_location = edit_properties_location
00253             self.startPropertyEdit()
00254 
00255     def startPropertyEdit(self):
00256 
00257         self.editing_properties = True
00258         self.edit_existing_location = self.edit_properties_location
00259 
00260         self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(True)
00261         self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(True)
00262 
00263         # Construct the configuration layout.
00264         clearLayoutAndFixHeight(self.configuration_layout)
00265 
00266         self.update_name_label = QLabel("Location (" + self.edit_properties_location + ")      New Name: ", self.widget)
00267         self.configuration_layout.addWidget(self.update_name_label)
00268 
00269         self.update_name_textedit = QLineEdit(self.widget)
00270         self.update_name_textedit.setText(self.edit_properties_location)
00271         self.update_name_textedit.textEdited.connect(self.locationNameTextEdited)
00272         self.configuration_layout.addWidget(self.update_name_textedit)
00273 
00274         self.update_name_button = QPushButton("Update location Name", self.widget)
00275         self.update_name_button.clicked[bool].connect(self.updateLocationName)
00276         self.update_name_button.setEnabled(False)
00277         self.configuration_layout.addWidget(self.update_name_button)
00278 
00279         self.remove_location_button = QPushButton("Remove Location", self.widget)
00280         self.remove_location_button.clicked[bool].connect(self.removeCurrentLocation)
00281         self.configuration_layout.addWidget(self.remove_location_button)
00282 
00283         self.configuration_layout.addStretch(1)
00284 
00285         self.updateOverlay()
00286 
00287     def endPropertyEdit(self):
00288 
00289         self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(True)
00290         self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(False)
00291 
00292         clearLayoutAndFixHeight(self.configuration_layout)
00293 
00294         self.update_name_label = None
00295         self.update_name_textedit = None
00296         self.update_name_button = None
00297 
00298         self.editing_properties = False
00299 
00300         self.edit_properties_location = None
00301 
00302         self.updateOverlay()
00303 
00304     def locationNameTextEdited(self, text):
00305         if str(text) != self.edit_properties_location:
00306             self.update_name_button.setEnabled(True)
00307         else:
00308             self.update_name_button.setEnabled(False)
00309 
00310     def updateLocationName(self):
00311         old_loc_name = self.edit_properties_location
00312         new_loc_name = str(self.update_name_textedit.text())
00313 
00314         if new_loc_name in self.locations:
00315             # This means that two locations need to be merged
00316             self.locations[new_loc_name] = self.locations[new_loc_name].united(self.locations.pop(old_loc_name))
00317         else:
00318             # This is a simple rename task.
00319             self.locations[new_loc_name] = self.locations.pop(old_loc_name)
00320             self.location_colors[new_loc_name] = self.location_colors.pop(old_loc_name)
00321             self.draw_location[new_loc_name] = self.draw_location.pop(old_loc_name)
00322 
00323         # Since a location name was modified, set file modification to true.
00324         self.is_modified = True
00325 
00326         # Restart property edit with the updated name.
00327         self.endPropertyEdit()
00328         self.edit_properties_location = new_loc_name
00329         self.startPropertyEdit()
00330 
00331     def removeCurrentLocation(self):
00332         old_loc_name = self.edit_properties_location
00333         self.removeLocation(old_loc_name)
00334         self.endPropertyEdit()
00335         self.updateOverlay()
00336 
00337         # Since a location was removed, set file modification to true.
00338         self.is_modified = True
00339 
00340     def removeLocation(self, loc_name):
00341         if loc_name in self.locations:
00342             self.locations.pop(loc_name)
00343         if loc_name in self.location_colors:
00344             self.location_colors.pop(loc_name)
00345         if loc_name in self.draw_location:
00346             self.draw_location.pop(loc_name)
00347 
00348     def isModified(self):
00349         return self.is_modified
00350 
00351     def mousePressEvent(self, event):
00352         if self.editing_area:
00353             self.subtract_new_selection = event.button() == Qt.RightButton
00354             self.new_selection_start_point = event.pos()
00355             self.new_selection_end_point = event.pos()
00356             self.new_selection = None
00357         else:
00358             loc = self.getLocationNameFromPoint(event.pos()) 
00359             if loc is not None:
00360                 self.edit_properties_location = loc
00361                 self.startPropertyEdit()
00362             else:
00363                 self.endPropertyEdit()
00364 
00365     def mouseReleaseEvent(self, event):
00366         if self.editing_area:
00367             self.mouseMoveEvent(event)
00368             if self.new_selection is not None:
00369                 if self.current_selection is None and self.subtract_new_selection == False:
00370                     self.current_selection = self.new_selection
00371                 if self.subtract_new_selection:
00372                     self.current_selection = self.current_selection.subtracted(self.new_selection)
00373                 else:
00374                     self.current_selection = self.current_selection.united(self.new_selection)
00375             self.new_selection = None
00376             self.subtract_new_selection = None
00377 
00378     def mouseMoveEvent(self, event):
00379 
00380         if self.editing_area:
00381 
00382             # First make sure we update the region corresponding to the old mark.
00383             old_overlay_update_rect = self.get_rectangular_polygon(self.new_selection_start_point, self.new_selection_end_point)
00384 
00385             # Draw new mark, taking some care to reduce the size of the polygon's bottom right corner by (1,1).
00386             self.new_selection_end_point = event.pos()
00387             self.new_selection = self.get_rectangular_polygon(self.new_selection_start_point, self.new_selection_end_point)
00388             self.new_selection = self.new_selection.boundingRect()
00389             self.new_selection.setHeight(self.new_selection.height() - 1)
00390             self.new_selection.setWidth(self.new_selection.width() - 1)
00391             self.new_selection = QPolygon(self.new_selection, True)
00392 
00393             # Next determine the region that needs to be update because of the new mark.
00394             new_overlay_update_rect = self.get_rectangular_polygon(self.new_selection_start_point, self.new_selection_end_point)
00395 
00396             overlay_update_region = (old_overlay_update_rect + new_overlay_update_rect).boundingRect()
00397             self.updateOverlay(overlay_update_region)
00398 
00399     def updateOverlay(self, rect = None):
00400         # Redraw the overlay image from scratch using the location image and current location.
00401 
00402         self.image.overlay_image.fill(Qt.transparent)
00403         painter = QPainter(self.image.overlay_image)
00404         painter.setBackgroundMode(Qt.TransparentMode)
00405         painter.setCompositionMode(QPainter.CompositionMode_Source)
00406 
00407         for location in self.locations:
00408             if self.draw_location[location]:
00409                 color = self.location_colors[location]
00410                 if self.edit_properties_location == location and self.editing_properties:
00411                     color = self.edit_area_selection_color
00412                 lineColor = QColor(color)
00413                 lineColor.setAlpha(255)
00414                 brushColor = QColor(color)
00415                 brushColor.setAlpha(128)
00416                 painter.setPen(lineColor)
00417                 painter.setBrush(brushColor)
00418                 painter.drawPolygon(self.locations[location])
00419 
00420         if (self.current_selection is not None) or (self.new_selection is not None):
00421             lineColor = QColor(self.edit_area_selection_color)
00422             lineColor.setAlpha(255)
00423             brushColor = QColor(self.edit_area_selection_color)
00424             brushColor.setAlpha(128)
00425             painter.setPen(lineColor)
00426             painter.setBrush(brushColor)
00427             if self.new_selection is not None:
00428                 # Create a temporary polygon as the new selection is being drawn.
00429                 if self.current_selection is not None:
00430                     current_selection = QPolygon(self.current_selection)
00431                     if self.subtract_new_selection:
00432                         current_selection = current_selection.subtracted(self.new_selection)
00433                     else:
00434                         current_selection = current_selection.united(self.new_selection)
00435                     painter.drawPolygon(current_selection)
00436                 elif self.subtract_new_selection == False:
00437                     painter.drawPolygon(self.new_selection)
00438             else:
00439                 painter.drawPolygon(self.current_selection)
00440         painter.end()
00441 
00442         if rect is None:
00443             self.image.update()
00444         else:
00445             self.image.update(rect)
00446 
00447     def getUniqueNameAndColor(self):
00448         """
00449         Use golden ratio to generate unique colors.
00450         http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/
00451         """
00452         name = "new_loc" + str(self.unique_loc_counter)
00453         h = int(359 * (self.unique_loc_counter * 0.618033988749895))
00454         h = h % 359 
00455         self.unique_loc_counter += 1
00456         return name, QColor.fromHsv(h, 255, 255)
00457 
00458     def get_rectangular_polygon(self, pt1, pt2):
00459         return QPolygon([pt1, QPoint(pt1.x(), pt2.y()), pt2, QPoint(pt2.x(), pt1.y())])
00460 


bwi_planning_common
Author(s): Piyush Khandelwal
autogenerated on Thu Jun 6 2019 17:57:32