00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 from __future__ import division
00011 import os
00012 import math
00013 import copy
00014
00015 from python_qt_binding import loadUi
00016 from python_qt_binding.QtCore import Qt, SIGNAL, pyqtSlot, QPointF
00017 from python_qt_binding.QtGui import QWidget, QPen, QBrush, QColor, QPolygonF, QMatrix, QFont, QGraphicsView, QDialog, QPushButton, QVBoxLayout, QHBoxLayout
00018
00019 from qt_gui.plugin import Plugin
00020 import rospkg
00021 import rospy
00022 import tf
00023 from tf.transformations import quaternion_from_euler
00024
00025 import rocon_qt_library.utils as utils
00026 from rocon_qt_library.interfaces import MapAnnotationInterface
00027 from rocon_qt_library.views import QMapView
00028 from world_canvas_client import AnnotationCollection
00029 from visualization_msgs.msg import Marker
00030
00031
00032
00033
00034 class QMapAnnotation(QWidget):
00035
00036 def __init__(self, parent=None):
00037 super(QMapAnnotation, self).__init__()
00038 self._parent = parent
00039 self.destroyed.connect(self.close)
00040 self._load_ui()
00041
00042 self._scene = self.map_view._scene
00043 self._viz_markers_pose = None
00044 self._viz_marker_items = {}
00045 self._viz_marker_items["annotations"] = []
00046 self._viz_marker_items["new_annotations"] = []
00047 self._viz_marker_polygon = QPolygonF([QPointF(-4, 4), QPointF(-4, -4), QPointF(12, 0)])
00048
00049 self._callback = {}
00050 self._callback['save_annotation'] = None
00051 self._callback['load_world'] = None
00052 self._callback['add_annotation'] = None
00053 self._wc = None
00054 self._init_events()
00055 self._drawing_objects = {}
00056
00057 self._init_variables_for_drawing()
00058 self._init_variableS_for_list()
00059
00060 def _init_variableS_for_list(self):
00061 self._annotation_name_list = []
00062 self._new_annotation_name_list = []
00063 self._map_name_list = []
00064
00065 def _init_variables_for_drawing(self):
00066 self.annotating = False
00067 self.annotation = {}
00068 self.annotation_item = None
00069 self.point_x = 0
00070 self.point_y = 0
00071 self.map_resolution = 1
00072
00073 self.anno_type = None
00074
00075 self._selected_map = None
00076
00077
00078 def _load_ui(self):
00079 rospack = rospkg.RosPack()
00080 ui_file = os.path.join(rospack.get_path('rocon_qt_library'), 'ui', 'map_annotation.ui')
00081 loadUi(ui_file, self, {'QMapView': QMapView})
00082
00083 world_name = rospy.get_param('~default_world_name',None)
00084 self._enable_buttons(False)
00085
00086 def _init_events(self):
00087 self.on_map_received = self.map_view.map_cb
00088
00089 self.map_view._scene.mousePressEvent = self._mousePressEvent
00090 self.map_view._scene.mouseReleaseEvent = self._mouseReleaseEvent
00091 self.map_view._scene.mouseMoveEvent = self._mouseMoveEvent
00092
00093 self.ar_marker_cbox.stateChanged.connect(self._check_ar_marker_cbox)
00094 self.location_cbox.stateChanged.connect(self._check_location_cbox)
00095 self.add_annotation_btn.clicked.connect(self._add_annotation)
00096 self.remove_annotation_btn.clicked.connect(self._remove_annotation)
00097 self.save_annotation_btn.clicked.connect(self._save_annotation)
00098 self.load_world_btn.clicked.connect(self._load_world)
00099 self.map_select_combobox.currentIndexChanged.connect(self._select_map_item_clicked)
00100 self.annotations_list_widget.itemClicked.connect(self._annotation_list_item_clicked)
00101
00102 self.connect(self, SIGNAL("show_message"), utils.show_message)
00103 self.connect(self, SIGNAL("draw_scene"), self.draw_scene)
00104 self.connect(self, SIGNAL("update_annotation_list"), self._update_annotation_list)
00105 self.connect(self, SIGNAL("update_map_selector"), self._update_map_selector)
00106
00107 def init_map_annotation_interface(self, scene_update_slot, wc_namespace):
00108 self._map_annotation_interface = MapAnnotationInterface(scene_update_slot=self.update_scene, wc_namespace=wc_namespace)
00109 self._callback['list_world'] = self._map_annotation_interface.get_list_world
00110 self._callback['load_world'] = self._map_annotation_interface.load_world
00111 self._callback['load_map'] = self._map_annotation_interface.load_map
00112 self._callback['add_annotation'] = self._map_annotation_interface.add_annotation
00113 self._callback['remove_annotation'] = self._map_annotation_interface.remove_annotation
00114 self._callback['save_annotation'] = self._map_annotation_interface.save_annotations
00115
00116 self.load_world_list()
00117
00118 def load_world_list(self):
00119 success, message, self._world_list = self._callback['list_world']()
00120
00121 if success:
00122 self._update_world_list()
00123 else:
00124 self.emit(SIGNAL("show_message"), self, "Failed", message)
00125
00126 def _update_world_list(self):
00127 self.list_world_combobox.clear()
00128
00129 for name in self._world_list:
00130 self.list_world_combobox.addItem(name)
00131
00132 def _load_world(self):
00133
00134 world_name = str(self.list_world_combobox.currentText())
00135
00136 success, message, self._map_name_list, self._annotation_name_list = self._callback['load_world'](world_name)
00137
00138 if success:
00139 self._world_name = world_name
00140 self._selected_map = self._map_name_list[0]
00141 self.emit(SIGNAL("update_map_selector"))
00142 self._new_annotation_name_list = []
00143 self.emit(SIGNAL("update_annotation_list"))
00144 self._enable_buttons(True)
00145 else:
00146 self.emit(SIGNAL("show_message"), self, "Failed", message)
00147
00148
00149 def _enable_buttons(self, enable):
00150 self.add_annotation_btn.setEnabled(enable)
00151 self.remove_annotation_btn.setEnabled(enable)
00152 self.save_annotation_btn.setEnabled(enable)
00153
00154 def _update_map_selector(self):
00155 self.map_select_combobox.clear()
00156
00157 for name in self._map_name_list:
00158 self.map_select_combobox.addItem(name)
00159
00160 if self._selected_map:
00161 idx = self.map_select_combobox.findText(self._selected_map)
00162 self.map_select_combobox.setCurrentIndex(idx)
00163
00164 def _update_annotation_list(self):
00165 self.annotations_list_widget.clear()
00166
00167 for n in self._annotation_name_list:
00168 self.annotations_list_widget.addItem(n)
00169
00170 for n in self._new_annotation_name_list:
00171 self.annotations_list_widget.addItem('(new) '+n)
00172
00173 def _add_annotation(self):
00174 if self.annotation:
00175 self._get_annotating_info()
00176 anno = copy.deepcopy(self.annotation)
00177
00178 if anno['name'] == '':
00179 self.emit(SIGNAL('show_message'), self, "Failed", "Name is empty!")
00180 elif self.anno_type == 'ar_track_alvar_msgs/AlvarMarker' and not anno['name'].isdigit():
00181 self.emit(SIGNAL('show_message'), self, "Failed", "Marker ID should be int!")
00182 else:
00183 self._new_annotation_name_list = self._callback['add_annotation'](anno, self.anno_type)
00184 self.emit(SIGNAL("update_annotation_list"))
00185 self._clear_on_annotation()
00186 self.ar_marker_cbox.setCheckState(Qt.Unchecked)
00187 self.location_cbox.setCheckState(Qt.Unchecked)
00188 self._clear_edit_annotation_box()
00189
00190 def _remove_annotation(self):
00191 if self._selected_annotation:
00192 ann_list, names = self._callback['remove_annotation'](self._selected_annotation)
00193
00194 if ann_list == 'annotations':
00195 self._annotation_name_list = names
00196 elif ann_list == 'new_annotations':
00197 self._new_annotation_name_list = names
00198 self.emit(SIGNAL("update_annotation_list"))
00199
00200 def _save_annotation(self):
00201 success, message = self._callback['save_annotation']()
00202
00203 if success:
00204 success, message, self._map_name_list, self._annotation_name_list = self._callback['load_world'](self._world_name)
00205
00206 if success:
00207 self.emit(SIGNAL("update_map_selector"))
00208 self._new_annotation_name_list = []
00209 self.emit(SIGNAL("update_annotation_list"))
00210
00211 self._enable_buttons(True)
00212 else:
00213 self.emit(SIGNAL("show_message"), self, "Failed", message)
00214 else:
00215 self.emit(SIGNAL("show_message"), self, "Failed", message)
00216
00217 def _set_annotating_info(self, anno_type='', name='', x=0, y=0, yaw=0, radius=1, roll=None, pitch=None, height=None):
00218 resolution = self._drawing_objects['map'].info.resolution
00219 origin = self._drawing_objects['map'].info.origin
00220 w = self._drawing_objects['map'].info.width
00221 h = self._drawing_objects['map'].info.height
00222
00223 self.annotation['type'] = anno_type
00224 self.annotation['x'] = (-x + w) * resolution + origin.position.x
00225 self.annotation['y'] = y * resolution + origin.position.y
00226 self.annotation['yaw'] = yaw
00227 self.annotation['radius'] = 0.2
00228
00229 if height != None:
00230 self.annotation['height'] = height
00231 if roll != None:
00232 self.annotation['roll'] = roll
00233 if pitch != None:
00234 self.annotation['pitch'] = pitch
00235
00236 radius = self.annotation['radius']
00237 self.annotation['scale'] = (radius, radius, radius)
00238 self.annotation['name'] = name
00239
00240 def _clear_edit_annotation_box(self):
00241 self.name_txt.clear()
00242 self.x_txt.clear()
00243 self.y_txt.clear()
00244 self.height_txt.clear()
00245 self.radius_txt.clear()
00246 self.roll_txt.clear()
00247 self.pitch_txt.clear()
00248 self.yaw_txt.clear()
00249
00250 def _update_edit_annotation_box(self):
00251 self.name_txt.setText(str(self.annotation['name']))
00252 self.x_txt.setText(str(round(self.annotation['x'], 3)))
00253 self.y_txt.setText(str(round(self.annotation['y'], 3)))
00254 self.height_txt.setText(str(round(self.annotation['height'], 3)))
00255 self.radius_txt.setText(str(round(self.annotation['radius'], 3)))
00256 self.roll_txt.setText(str(round(self.annotation['roll'])))
00257 self.pitch_txt.setText(str(round(self.annotation['pitch'], 3)))
00258 self.yaw_txt.setText(str(round(self.annotation['yaw'], 3)))
00259
00260 def _set_edit_annotation_box(self, name="", x=0, y=0, height=0, radius=0, roll=0, pitch=0, yaw=0):
00261 self.name_txt.setText(str(name))
00262 self.x_txt.setText(str(round(x, 3)))
00263 self.y_txt.setText(str(round(y, 3)))
00264 self.height_txt.setText(str(round(height, 3)))
00265 self.radius_txt.setText(str(round(radius, 3)))
00266 self.roll_txt.setText(str(round(roll)))
00267 self.pitch_txt.setText(str(round(pitch, 3)))
00268 self.yaw_txt.setText(str(round(yaw, 3)))
00269
00270 def _get_annotating_info(self):
00271 self.annotation['name'] = str(self.name_txt.toPlainText().strip())
00272
00273 self.annotation['x'] = float(self.x_txt.toPlainText())
00274 self.annotation['y'] = float(self.y_txt.toPlainText())
00275 self.annotation['height'] = float(self.height_txt.toPlainText())
00276
00277 self.annotation['yaw'] = float(self.yaw_txt.toPlainText())
00278 self.annotation['roll'] = float(self.roll_txt.toPlainText())
00279 self.annotation['pitch'] = float(self.pitch_txt.toPlainText())
00280
00281 radius = float(self.radius_txt.toPlainText())
00282 self.annotation['scale'] = (radius, radius, radius)
00283 self.annotation['radius'] = radius
00284
00285 @pyqtSlot(str, dict)
00286 def update_scene(self, object_name, data):
00287 if object_name == 'map':
00288 self._drawing_objects['map'] = data['map']
00289 elif object_name == 'annotations':
00290 self._drawing_objects['annotations'] = data['annotations']
00291 elif object_name == 'new_annotations':
00292 self._drawing_objects['new_annotations'] = data['new_annotations']
00293 elif object_name == 'on_annotation':
00294 self._drawing_objects['on_annotation'] = data
00295 self.emit(SIGNAL("draw_scene"), object_name)
00296
00297 @pyqtSlot(str)
00298 def draw_scene(self, key):
00299 if key == 'map':
00300 self.draw_map(self._drawing_objects['map'])
00301 if key == 'annotations' or key == 'new_annotations':
00302 self.draw_viz_markers(self._drawing_objects[key], key)
00303 if key == 'on_annotation':
00304 self._draw_annotation(self._drawing_objects['on_annotation'])
00305
00306 def _draw_annotation(self, data):
00307 old_item = None
00308 if self.annotation_item:
00309 old_item = self.annotation_item
00310 annotation_item = None
00311
00312 if not data == {}:
00313 resolution = self._drawing_objects['map'].info.resolution
00314 origin = self._drawing_objects['map'].info.origin
00315 w = self._drawing_objects['map'].info.width
00316 h = self._drawing_objects['map'].info.height
00317
00318 x = -((data['x'] - origin.position.x) / resolution - w)
00319 y = (data['y'] - origin.position.y) / resolution
00320 if data['type'] == "yocs_msgs/Waypoint":
00321
00322 viz_marker_pose = QMatrix().rotate(-data['yaw']+180).map(self._viz_marker_polygon).translated(x, y)
00323 annotation_item = self._scene.addPolygon(viz_marker_pose, pen=QPen(QColor(0, 0, 255)), brush=QBrush(QColor(0, 0, 255, 125)))
00324 elif data['type'] == "ar_track_alvar_msgs/AlvarMarker":
00325 viz_marker_pose = QMatrix().rotate(-(data['yaw']-90)+180).map(self._viz_marker_polygon).translated(x, y)
00326 annotation_item = self._scene.addPolygon(viz_marker_pose, pen=QPen(QColor(0, 0, 255)), brush=QBrush(QColor(0, 0, 255, 125)))
00327 annotation_item.setZValue(2)
00328 if old_item:
00329 self._scene.removeItem(old_item)
00330 self.annotation_item = annotation_item
00331
00332 def draw_map(self, data):
00333 self.map_view.map_cb(data)
00334
00335 @pyqtSlot(list)
00336 def draw_viz_markers(self, data, key):
00337 old_items = []
00338 if len(self._viz_marker_items[key]):
00339 for item in self._viz_marker_items[key]:
00340 old_items.append(item)
00341 self._viz_marker_items[key] = []
00342 for viz_marker in data:
00343 if viz_marker['type'] is Marker.TEXT_VIEW_FACING:
00344
00345 viz_marker_text = viz_marker['text']
00346 viz_marker_pose_item = self._scene.addSimpleText(viz_marker_text, font=QFont())
00347
00348 viz_marker_pose_item.translate(-viz_marker['x'], viz_marker['y'])
00349 elif viz_marker['type'] is Marker.CYLINDER:
00350
00351
00352
00353
00354 viz_marker_pose = QMatrix().scale(1,-1).rotate(viz_marker['yaw']+180).map(self._viz_marker_polygon).translated(-viz_marker['x'], viz_marker['y'])
00355 viz_marker_pose_item = self._scene.addPolygon(viz_marker_pose, pen=QPen(QColor(0, 255, 0)), brush=QBrush(QColor(0, 255, 0)))
00356 elif viz_marker['type'] is Marker.ARROW:
00357
00358
00359 viz_marker_pose = QMatrix().scale(1,-1).rotate((viz_marker['yaw']-90)+180).map(self._viz_marker_polygon).translated(-viz_marker['x'], viz_marker['y'])
00360 viz_marker_pose_item = self._scene.addPolygon(viz_marker_pose, pen=QPen(QColor(255, 0, 0)), brush=QBrush(QColor(255, 0, 0)))
00361 else:
00362 rospy.logerr("Unknown Marker type : %s"%(viz_marker['type']))
00363 viz_marker_pose_item.setZValue(1)
00364 self._viz_marker_items[key].append(viz_marker_pose_item)
00365 if len(old_items):
00366 for item in old_items:
00367 self._scene.removeItem(item)
00368
00369
00370 def _mirror(self, item):
00371 item.scale(-1, 1)
00372
00373
00374
00375
00376 def _mouseReleaseEvent(self, e):
00377 if self.annotating:
00378 self.annotating = False
00379
00380
00381
00382 if self.point_x == e.scenePos().x() and self.point_y == e.scenePos().y():
00383 self.annotation = {}
00384 self.update_scene("on_annotation", self.annotation)
00385
00386 def _mousePressEvent(self, e):
00387 if not self.annotating:
00388 if self.anno_type:
00389 self.annotating = True
00390 self.annotation = {}
00391 self.point_x = e.scenePos().x()
00392 self.point_y = e.scenePos().y()
00393
00394 height = 0.36
00395 radius = 1
00396 roll = 90.0 if self.anno_type == 'ar_track_alvar_msgs/AlvarMarker' else 0.0
00397 self._set_annotating_info(anno_type=self.anno_type, x=self.point_x, y=self.point_y, yaw=0.0, radius=radius, roll=roll, pitch=0.0, height=height)
00398 self._update_edit_annotation_box()
00399
00400 self.update_scene("on_annotation", self.annotation)
00401
00402 def _mouseMoveEvent(self, e):
00403 if self.annotating:
00404 dx = e.scenePos().x() - self.point_x
00405 dy = e.scenePos().y() - self.point_y
00406 dist = math.hypot(dx, dy)
00407 yaw = -math.degrees(math.atan2(dy, dx))
00408 if self.anno_type == 'ar_track_alvar_msgs/AlvarMarker':
00409 yaw += 90
00410 self._set_annotating_info(anno_type=self.anno_type, x=self.point_x, y=self.point_y, yaw=yaw, radius=dist, roll=None, pitch=None, height=None)
00411 self._update_edit_annotation_box()
00412 self.update_scene("on_annotation", self.annotation)
00413
00414
00415
00416
00417 def _annotation_list_item_clicked(self, item):
00418
00419 self._selected_annotation = str(item.text())
00420 if self._selected_annotation.startswith('(new)'):
00421 self._selected_annotation = self._selected_annotation[6:]
00422 annotation_info = self._map_annotation_interface.get_annotation_info(self._selected_annotation)
00423
00424 if annotation_info:
00425 self._set_edit_annotation_box()
00426
00427 self._set_edit_annotation_box(name=annotation_info[1],
00428 x=annotation_info[2],
00429 y=annotation_info[3],
00430 yaw=annotation_info[4],
00431 radius=annotation_info[5],
00432 roll=annotation_info[6],
00433 pitch=annotation_info[7],
00434 height=annotation_info[8])
00435
00436
00437
00438
00439 def _select_map_item_clicked(self, item_index):
00440
00441 if item_index >= 0 and self.map_select_combobox.count() > 1:
00442 self._selected_map = self.map_select_combobox.itemText(item_index)
00443 print(str("in item clicked : " + self._selected_map))
00444 success, message = self._callback['load_map'](self._selected_map)
00445
00446 if not success:
00447 self.emit(SIGNAL("show_message"), self, "Failed", message)
00448
00449
00450
00451
00452
00453
00454 def _check_ar_marker_cbox(self, data):
00455 if data == Qt.Checked:
00456 self.location_cbox.setCheckState(Qt.Unchecked)
00457 self.map_view.setDragMode(QGraphicsView.NoDrag)
00458 self.anno_type = "ar_track_alvar_msgs/AlvarMarker"
00459 else:
00460 self.map_view.setDragMode(QGraphicsView.ScrollHandDrag)
00461 if self.location_cbox.checkState() == Qt.Unchecked:
00462 self.anno_type = None
00463 self._clear_on_annotation()
00464 self._deselect_list_annotation()
00465
00466 def _check_location_cbox(self, data):
00467 if data == Qt.Checked:
00468 self.ar_marker_cbox.setCheckState(Qt.Unchecked)
00469 self.map_view.setDragMode(QGraphicsView.NoDrag)
00470 self.anno_type = "yocs_msgs/Waypoint"
00471 else:
00472 self.map_view.setDragMode(QGraphicsView.ScrollHandDrag)
00473
00474 if self.ar_marker_cbox.checkState() == Qt.Unchecked:
00475 self.anno_type = None
00476 self._clear_on_annotation()
00477 self._deselect_list_annotation()
00478
00479 def _clear_on_annotation(self):
00480 if self.annotation_item:
00481 self._scene.removeItem(self.annotation_item)
00482 self.annotation_item = None
00483 self.annotation = None
00484 self.annotation = {}
00485 self._clear_edit_annotation_box()
00486
00487 def _deselect_list_annotation(self):
00488 listwidget = self.annotations_list_widget
00489 self._selected_annotation = None
00490
00491 for i in range(listwidget.count()):
00492 item = listwidget.item(i)
00493 listwidget.setItemSelected(item, False)
00494
00495
00496
00497
00498 def save_settings(self, plugin_settings, instance_settings):
00499
00500 pass
00501
00502 def restore_settings(self, plugin_settings, instance_settings):
00503
00504 pass