nav_view.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2012, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 import rospy
34 import tf
35 from tf.transformations import quaternion_from_euler
36 import rostopic
37 
38 import numpy
39 import random
40 from math import sqrt, atan2
41 
42 from nav_msgs.msg import OccupancyGrid, Path
43 from geometry_msgs.msg import PolygonStamped, PointStamped, PoseWithCovarianceStamped, PoseStamped
44 
45 from python_qt_binding.QtCore import Signal, Slot, QPointF, qWarning, Qt
46 from python_qt_binding.QtGui import QPixmap, QImage, QPainterPath, QPen, QPolygonF, QColor, qRgb, QTransform
47 from python_qt_binding.QtWidgets import QWidget, QGraphicsView, QGraphicsScene, QVBoxLayout, QHBoxLayout, QPushButton, \
48  QInputDialog
49 
50 from rqt_py_common.topic_helpers import get_field_type
51 
52 from .list_dialog import ListDialog
53 
54 
55 def accepted_topic(topic):
56  msg_types = [OccupancyGrid, Path, PolygonStamped, PointStamped]
57  msg_type, array = get_field_type(topic)
58 
59  if not array and msg_type in msg_types:
60  return True
61  else:
62  return False
63 
64 
65 class PathInfo(object):
66  def __init__(self, name=None):
67  self.color = None
68  self.sub = None
69  self.cb = None
70  self.path = None
71  self.item = None
72  self.name = name
73 
74 
75 class NavViewWidget(QWidget):
76 
77  def __init__(self, map_topic='/map', paths=None, polygons=None):
78  super(NavViewWidget, self).__init__()
79  if paths is None:
80  paths = ['/move_base/NavFn/plan', '/move_base/TrajectoryPlannerROS/local_plan']
81  if polygons is None:
82  polygons = ['/move_base/local_costmap/robot_footprint']
83 
84  self._layout = QVBoxLayout()
85  self._button_layout = QHBoxLayout()
86 
87  self.setAcceptDrops(True)
88  self.setWindowTitle('Navigation Viewer')
89 
90  self.paths = paths
91  self.polygons = polygons
92  self.map_topic = map_topic
94 
95  self._set_pose = QPushButton('Set Pose')
96  self._set_goal = QPushButton('Set Goal')
97 
98  self._button_layout.addWidget(self._set_pose)
99  self._button_layout.addWidget(self._set_goal)
100 
101  self._layout.addLayout(self._button_layout)
102 
103  self._nav_view = None
104 
105  self.setLayout(self._layout)
106 
107  def new_nav_view(self):
108  if self._nav_view:
109  self._nav_view.close()
110  self._nav_view = NavView(self.map_topic, self.paths, self.polygons, tf_listener=self._tf, parent=self)
111  self._set_pose.clicked.connect(self._nav_view.pose_mode)
112  self._set_goal.clicked.connect(self._nav_view.goal_mode)
113  self._layout.addWidget(self._nav_view)
114 
115  def dragEnterEvent(self, e):
116  if not e.mimeData().hasText():
117  if not hasattr(e.source(), 'selectedItems') or len(e.source().selectedItems()) == 0:
118  qWarning('NavView.dragEnterEvent(): not hasattr(event.source(), selectedItems) or '
119  'len(event.source().selectedItems()) == 0')
120  return
121  item = e.source().selectedItems()[0]
122  topic_name = item.data(0, Qt.UserRole)
123  if topic_name is None:
124  qWarning('NavView.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
125  return
126 
127  else:
128  topic_name = str(e.mimeData().text())
129 
130  if accepted_topic(topic_name):
131  e.acceptProposedAction()
132 
133  def dropEvent(self, e):
134  if e.mimeData().hasText():
135  topic_name = str(e.mimeData().text())
136  else:
137  dropped_item = e.source().selectedItems()[0]
138  topic_name = str(dropped_item.data(0, Qt.UserRole))
139 
140  topic_type, array = get_field_type(topic_name)
141  if not array:
142  if topic_type is OccupancyGrid:
143  self.map_topic = topic_name
144 
145  # Swap out the nav view for one with the new topics
146  self.new_nav_view()
147  elif topic_type is Path:
148  self.paths.append(topic_name)
149  self._nav_view.add_path(topic_name)
150  elif topic_type is PolygonStamped:
151  self.polygons.append(topic_name)
152  self._nav_view.add_polygon(topic_name)
153 
154  def save_settings(self, plugin_settings, instance_settings):
155  instance_settings.set_value("map_topic", self.map_topic)
156  instance_settings.set_value("paths", self.paths)
157  instance_settings.set_value("polygons", self.polygons)
158 
159  def restore_settings(self, plugin_settings, instance_settings):
160  try:
161  self.map_topic = instance_settings.value("map_topic", "/map")
162  except Exception:
163  pass
164 
165  try:
166  self.paths = instance_settings.value("paths", [])
167  except Exception:
168  pass
169 
170  try:
171  self.polygons = instance_settings.value("polygons", [])
172  except Exception:
173  pass
174 
175  self.new_nav_view()
176 
178  """
179  Callback when the configuration button is clicked
180  """
181  changed = False
182  map_topics = sorted(rostopic.find_by_type('nav_msgs/OccupancyGrid'))
183  try:
184  index = map_topics.index(self.map_topic)
185  except ValueError:
186  index = 0
187  map_topic, ok = QInputDialog.getItem(self, "Select map topic name", "Topic name",
188  map_topics, index)
189  if ok:
190  if map_topic != self.map_topic:
191  changed = True
192  self.map_topic = map_topic
193 
194  # Paths
195  path_topics = sorted(rostopic.find_by_type('nav_msgs/Path'))
196  path_topics = [(topic, topic in self.paths) for topic in path_topics]
197  dialog = ListDialog("Select path topic(s)", path_topics, self)
198  paths, ok = dialog.exec_()
199 
200  if ok:
201  if not paths:
202  changed = True
203  diff = set(paths).symmetric_difference(set(self.paths))
204  if diff:
205  self.paths = paths
206  changed = True
207 
208  # Polygons
209  polygon_topics = sorted(rostopic.find_by_type('geometry_msgs/PolygonStamped'))
210  polygon_topics = [(topic, topic in self.polygons) for topic in polygon_topics]
211  dialog = ListDialog("Select polygon topic(s)", polygon_topics, self)
212  polygons, ok = dialog.exec_()
213 
214  if ok:
215  if not polygons:
216  changed = True
217  diff = set(polygons).symmetric_difference(set(self.polygons))
218  if diff:
219  self.polygons = polygons
220  changed = True
221 
222  if changed:
223  rospy.logdebug("New configuration is different, creating a new nav_view")
224  self.new_nav_view()
225 
226 
227 class NavView(QGraphicsView):
228  map_changed = Signal()
229  path_changed = Signal(str)
230  polygon_changed = Signal(str)
231 
232  def __init__(self, map_topic='/map', paths=None, polygons=None, tf_listener=None, parent=None):
233  super(NavView, self).__init__()
234  if paths is None:
235  paths = ['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan']
236  if polygons is None:
237  polygons = ['/move_base/local_costmap/robot_footprint']
238  if tf_listener is None:
239  tf_listener = tf.TransformListener()
240 
241  self._parent = parent
242 
243  self._pose_mode = False
244  self._goal_mode = False
245  self.last_path = None
246  self.drag_start = None
247 
248  self.map_changed.connect(self._update)
249  self.destroyed.connect(self.close)
250 
251  # ScrollHandDrag
252  self.setDragMode(QGraphicsView.ScrollHandDrag)
253 
254  self._map = None
255  self._map_hash = None
256  self._map_item = None
257 
258  self.map_width = 0
259  self.map_height = 0
260  self.map_resolution = 0
261  self.map_origin = None
262  self.frame_id = ""
263 
264  self._paths = {}
265  self._polygons = {}
266  self.path_changed.connect(self._update_path)
267  self.polygon_changed.connect(self._update_polygon)
268 
269  self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10),
270  (196, 30, 250)]
271 
272  self._scene = QGraphicsScene()
273 
274  self._tf = tf_listener
275  self.map_sub = rospy.Subscriber(map_topic, OccupancyGrid, self.map_cb)
276 
277  for path in paths:
278  self.add_path(path)
279 
280  for poly in polygons:
281  self.add_polygon(poly)
282 
283  try:
284  self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=100)
285  self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=100)
286  except TypeError:
287  self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
288  self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped)
289 
290  self.setScene(self._scene)
291 
292  def add_dragdrop(self, item):
293  # Add drag and drop functionality to all the items in the view
294  def c(x, e):
295  self.dragEnterEvent(e)
296 
297  def d(x, e):
298  self.dropEvent(e)
299 
300  item.setAcceptDrops(True)
301  item.dragEnterEvent = c
302  item.dropEvent = d
303 
304  def dragEnterEvent(self, e):
305  if self._parent:
306  self._parent.dragEnterEvent(e)
307 
308  def dropEvent(self, e):
309  if self._parent:
310  self._parent.dropEvent(e)
311 
312  def wheelEvent(self, event):
313  event.ignore()
314  try:
315  delta = event.angleDelta().y()
316  except AttributeError:
317  delta = event.delta()
318  if delta > 0:
319  self.scale(1.15, 1.15)
320  else:
321  self.scale(0.85, 0.85)
322 
323  def map_cb(self, msg):
324  map_hash = hash(msg.data)
325  if map_hash == self._map_hash:
326  rospy.logdebug("Skipping map cb, because the map is the same")
327  return
328 
329  self._map_hash = map_hash
330 
331  self.map_resolution = msg.info.resolution
332  self.map_width = msg.info.width
333  self.map_height = msg.info.height
334  self.map_origin = msg.info.origin
335  self.frame_id = msg.header.frame_id
336 
337  a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
338  a = a.reshape((self.map_height, self.map_width))
339  if self.map_width % 4:
340  e = numpy.empty((self.map_height, 4 - self.map_width % 4), dtype=a.dtype, order='C')
341  a = numpy.append(a, e, axis=1)
342  image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.map_width, self.map_height, QImage.Format_Indexed8)
343 
344  for i in reversed(range(101)):
345  image.setColor(100 - i, qRgb(i * 2.55, i * 2.55, i * 2.55))
346  image.setColor(101, qRgb(255, 0, 0)) # not used indices
347  image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
348  self._map = image
349  self.setSceneRect(0, 0, self.map_width, self.map_height)
350  self.map_changed.emit()
351 
352  def add_path(self, name):
353  path = PathInfo(name)
354 
355  def cb(msg):
356  if not self._map:
357  return
358 
359  pp = QPainterPath()
360 
361  # Transform everything in to the map frame
362  if not (msg.header.frame_id == self.frame_id or msg.header.frame_id == ''):
363  try:
364  self._tf.waitForTransform(msg.header.frame_id, self.frame_id, rospy.Time(), rospy.Duration(10))
365  data = [self._tf.transformPose(self.frame_id, pose) for pose in msg.poses]
366  except tf.Exception:
367  rospy.logerr("TF Error")
368  data = []
369  else:
370  data = msg.poses
371 
372  if len(data) > 0:
373  start = data[0].pose.position
374  pp.moveTo(*self.point_map_to_qt((start.x, start.y)))
375 
376  for pose in data:
377  pt = pose.pose.position
378  pp.lineTo(*self.point_map_to_qt((pt.x, pt.y)))
379 
380  path.path = pp
381  self.path_changed.emit(name)
382 
383  path.color = random.choice(self._colors)
384  self._colors.remove(path.color)
385 
386  path.cb = cb
387  path.sub = rospy.Subscriber(path.name, Path, path.cb)
388 
389  self._paths[name] = path
390 
391  def add_polygon(self, name):
392  poly = PathInfo(name)
393 
394  def cb(msg):
395  if not self._map:
396  return
397 
398  if not (msg.header.frame_id == self.frame_id or msg.header.frame_id == ''):
399  try:
400  self._tf.waitForTransform(msg.header.frame_id, self.frame_id, rospy.Time(), rospy.Duration(10))
401  points_stamped = []
402  for pt in msg.polygon.points:
403  ps = PointStamped()
404  ps.header.frame_id = msg.header.frame_id
405  ps.point.x = pt.x
406  ps.point.y = pt.y
407 
408  points_stamped.append(ps)
409 
410  trans_pts = []
411  for pt in points_stamped:
412  point = self._tf.transformPoint(self.frame_id, pt).point
413  trans_pts.append((point.x, point.y))
414  except tf.Exception:
415  rospy.logerr("TF Error")
416  trans_pts = []
417  else:
418  trans_pts = [(pt.x, pt.y) for pt in msg.polygon.points]
419 
420  if len(trans_pts) > 0:
421  trans_pts.append(trans_pts[0])
422  pts = [QPointF(*self.point_map_to_qt(pt)) for pt in trans_pts]
423  poly.path = QPolygonF(pts)
424 
425  self.polygon_changed.emit(name)
426 
427  poly.color = random.choice(self._colors)
428  self._colors.remove(poly.color)
429 
430  poly.cb = cb
431  poly.sub = rospy.Subscriber(poly.name, PolygonStamped, poly.cb)
432 
433  self._polygons[name] = poly
434 
435  def pose_mode(self):
436  if not self._pose_mode:
437  self._pose_mode = True
438  self._goal_mode = False
439  self.setDragMode(QGraphicsView.NoDrag)
440  elif self._pose_mode:
441  self._pose_mode = False
442  self.setDragMode(QGraphicsView.ScrollHandDrag)
443 
444  def goal_mode(self):
445  if not self._goal_mode:
446  self._goal_mode = True
447  self._pose_mode = False
448  self.setDragMode(QGraphicsView.NoDrag)
449  elif self._goal_mode:
450  self._goal_mode = False
451  self.setDragMode(QGraphicsView.ScrollHandDrag)
452 
453  def draw_position(self, e):
454  p = self.mapToScene(e.x(), e.y())
455  v = (p.x() - self.drag_start[0], p.y() - self.drag_start[1])
456  mag = sqrt(pow(v[0], 2) + pow(v[1], 2))
457  v = (v[0]/mag, v[1]/mag) # Normalize diff vector
458  u = (-v[1], v[0]) # Project diff vector to mirrored map
459 
460  if self.last_path:
461  self._scene.removeItem(self.last_path)
462  self.last_path = None
463 
464  res = (v[0]*25, v[1]*25)
465 
466  if self._pose_mode:
467  pen = QPen(QColor("red"))
468  elif self._goal_mode:
469  pen = QPen(QColor("green"))
470  self.last_path = self._scene.addLine(self.drag_start[0], self.drag_start[1],
471  self.drag_start[0] + res[0], self.drag_start[1] + res[1], pen)
472 
473  map_p = self.point_qt_to_map(self.drag_start)
474 
475  angle = atan2(u[0], u[1])
476  quat = quaternion_from_euler(0, 0, angle)
477 
478  self.drag_start = None
479 
480  return map_p, quat
481 
482  def mousePressEvent(self, e):
483  if self._goal_mode or self._pose_mode:
484  p = self.mapToScene(e.x(), e.y())
485  self.drag_start = (p.x(), p.y())
486  else:
487  super(NavView, self).mousePressEvent(e)
488 
489  def mouseReleaseEvent(self, e):
490  if self._goal_mode:
491  map_p, quat = self.draw_position(e)
492  self.goal_mode() # Disable goal_mode and enable dragging/scrolling again
493 
494  msg = PoseStamped()
495  msg.header.frame_id = self.frame_id
496  msg.header.stamp = rospy.Time.now()
497 
498  msg.pose.position.x = map_p[0]
499  msg.pose.position.y = map_p[1]
500  msg.pose.orientation.z = quat[2]
501  msg.pose.orientation.w = quat[3]
502 
503  self._goal_pub.publish(msg)
504 
505  elif self._pose_mode:
506  map_p, quat = self.draw_position(e)
507  self.pose_mode() # Disable pose_mode and enable dragging/scrolling again
508 
509  msg = PoseWithCovarianceStamped()
510  msg.header.frame_id = self.frame_id
511  msg.header.stamp = rospy.Time.now()
512 
513  # ToDo: Is it ok to just ignore the covariance matrix here?
514  msg.pose.pose.orientation.z = quat[2]
515  msg.pose.pose.orientation.w = quat[3]
516  msg.pose.pose.position.x = map_p[0]
517  msg.pose.pose.position.y = map_p[1]
518 
519  self._pose_pub.publish(msg)
520 
521  def close(self):
522  if self.map_sub:
523  self.map_sub.unregister()
524  for p in self._paths.values():
525  if p.sub:
526  p.sub.unregister()
527 
528  for p in self._polygons.values():
529  if p.sub:
530  p.sub.unregister()
531 
532  super(NavView, self).close()
533 
534  def _update(self):
535  if self._map_item:
536  self._scene.removeItem(self._map_item)
537 
538  pixmap = QPixmap.fromImage(self._map)
539  self._map_item = self._scene.addPixmap(pixmap)
540 
541  # Everything must be mirrored
542  self._mirror(self._map_item)
543 
544  # Add drag and drop functionality
545  self.add_dragdrop(self._map_item)
546 
547  self.centerOn(self._map_item)
548  self.show()
549 
550  def _update_path(self, name):
551  old_item = None
552  if name in self._paths.keys():
553  old_item = self._paths[name].item
554 
555  self._paths[name].item = self._scene.addPath(self._paths[name].path,
556  pen=QPen(QColor(*self._paths[name].color)))
557 
558  if old_item:
559  self._scene.removeItem(old_item)
560 
561  def _update_polygon(self, name):
562  old_item = None
563  if name in self._polygons.keys():
564  old_item = self._polygons[name].item
565 
566  self._polygons[name].item = self._scene.addPolygon(self._polygons[name].path,
567  pen=QPen(QColor(*self._polygons[name].color)))
568 
569  if old_item:
570  self._scene.removeItem(old_item)
571 
572  def _mirror(self, item):
573  """
574  Mirror any QItem to have correct orientation
575  :param item:
576  :return:
577  """
578  item.setTransform(QTransform().scale(1, -1).translate(0, -self.map_height))
579 
580  def point_qt_to_map(self, point):
581  """
582  Convert point from Qt to map coordinates
583 
584  :param point: tuple or list
585  :return: map point
586  """
587  # Mirror point over y axis
588  x = point[0]
589  y = self.map_height - point[1]
590 
591  # Orientation might need to be taken into account
592  return [x * self.map_resolution + self.map_origin.position.x,
593  y * self.map_resolution + self.map_origin.position.y]
594 
595  def point_map_to_qt(self, point):
596  """
597  Convert point from map to qt coordinates
598 
599  :param point: tuple or list
600  :return: map point
601  """
602  # Orientation might need to be taken into account
603  x = (point[0] - self.map_origin.position.x) / self.map_resolution
604  y = (point[1] - self.map_origin.position.y) / self.map_resolution
605 
606  # Mirror point over y axis
607  return [x, self.map_height - y]
d
Definition: setup.py:6
def add_path(self, name)
Definition: nav_view.py:352
def point_qt_to_map(self, point)
Definition: nav_view.py:580
def add_polygon(self, name)
Definition: nav_view.py:391
def mousePressEvent(self, e)
Definition: nav_view.py:482
def wheelEvent(self, event)
Definition: nav_view.py:312
def restore_settings(self, plugin_settings, instance_settings)
Definition: nav_view.py:159
def add_dragdrop(self, item)
Definition: nav_view.py:292
def save_settings(self, plugin_settings, instance_settings)
Definition: nav_view.py:154
def __init__(self, map_topic='/map', paths=None, polygons=None)
Definition: nav_view.py:77
def _mirror(self, item)
Definition: nav_view.py:572
def __init__(self, name=None)
Definition: nav_view.py:66
def accepted_topic(topic)
Definition: nav_view.py:55
def _update_polygon(self, name)
Definition: nav_view.py:561
def point_map_to_qt(self, point)
Definition: nav_view.py:595
def mouseReleaseEvent(self, e)
Definition: nav_view.py:489
def __init__(self, map_topic='/map', paths=None, polygons=None, tf_listener=None, parent=None)
Definition: nav_view.py:232
def _update_path(self, name)
Definition: nav_view.py:550


rqt_nav_view
Author(s): Ze'ev Klapow
autogenerated on Wed Mar 1 2023 03:51:26