def __init__(self, map_topic = '/map',
paths = ['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons= ['/move_base/local_costmap/robot_footprint']):
super(QWidget, self).__init__()
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(1)
self._map = None
self._map_orig = None
self._map_item = None
self.w = 0
self.h = 0
self._paths = {}
self._polygons = {}
self.path_changed.connect(self._update_path)
self.polygon_changed.connect(self._update_polygon)
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self._scene = QGraphicsScene()
self._tf = tf.TransformListener()
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
self.setScene(self._scene)
class NavView(QGraphicsView):
map_changed = pyqtSignal()
path_changed = pyqtSignal(str)
polygon_changed = pyqtSignal(str)
def __init__(self, map_topic = '/map',
paths = ['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons= ['/move_base/local_costmap/robot_footprint']):
super(QWidget, self).__init__()
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(1)
self._map = None
self._map_orig = None
self._map_item = None
self.w = 0
self.h = 0
self._paths = {}
self._polygons = {}
self.path_changed.connect(self._update_path)
self.polygon_changed.connect(self._update_polygon)
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self._scene = QGraphicsScene()
self._tf = tf.TransformListener()
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
self.setScene(self._scene)
def wheelEvent(self, event):
event.ignore()
if event.delta() > 0:
self.zoom_in(2)
else:
self.zoom_out(2)
def map_cb(self, msg):
self.resolution = msg.info.resolution
self.w = msg.info.width
self.h = msg.info.height
data = []
# Get correct colors
for c in msg.data:
if c == 100:
data.append((0, 0, 0))
elif c == 0:
data.append((255, 255, 255))
else:
data.append((127, 127, 127))
im = Image.new('RGB', (self.w, self.h))
im.putdata(data)
self._map = im
self._map_orig = self._map
self.map_changed.emit()
def zoom_in(self, amount):
self.w = self.w * amount
self.h = self.h * amount
self._map = self._map_orig.resize((self.w, self.h))
self.resolution = self.resolution / amount
self.map_changed.emit()
def zoom_out(self, amount):
self.w = self.w / amount
self.h = self.h / amount
self._map = self._map_orig.resize((self.w, self.h))
self.resolution = self.resolution * amount
self.map_changed.emit()
def add_path(self, name):
path = PathInfo(name)
def c(msg):
pp = QPainterPath()
# Transform everything in to the map frame
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
data = [self._tf.transformPose('/map', pose) for pose in msg.poses]
except tf.Exception as e:
rospy.logerr("TF Error")
data = []
else:
data = msg.poses
if len(data) > 0:
start = data[0].pose.position
pp.moveTo(start.x / self.resolution, start.y / self.resolution)
for pose in data:
pt = pose.pose.position
pp.lineTo(pt.x / self.resolution, pt.y / self.resolution)
path.path = pp
self.path_changed.emit(name)
path.color = random.choice(self._colors)
self._colors.remove(path.color)
path.cb = c
path.sub = rospy.Subscriber(path.name, Path, path.cb)
self._paths[name] = path
def add_polygon(self, name):
poly = PathInfo(name)
def c(msg):
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
points_stamped = []
for pt in msg.polygon.points:
ps = PointStamped()
ps.header.frame_id = msg.header.frame_id
ps.point.x = pt.x
ps.point.y = pt.y
points_stamped.append(ps)
trans_pts = [self._tf.transformPoint('/map', pt).point for pt in points_stamped]
except tf.Exception:
rospy.logerr("TF Error")
trans_pts = []
else:
trans_pts = [pt for pt in msg.polygon.points]
if len(trans_pts) > 0:
pts = [QPointF(pt.x / self.resolution, pt.y / self.resolution) for pt in trans_pts]
close = trans_pts[0]
pts.append(QPointF(close.x / self.resolution, close.y / self.resolution))
poly.path = QPolygonF(pts)
self.polygon_changed.emit(name)
poly.color = random.choice(self._colors)
self._colors.remove(poly.color)
poly.cb = c
poly.sub = rospy.Subscriber(poly.name, PolygonStamped, poly.cb)
self._polygons[name] = poly
def close(self):
if self.map_sub:
self.map_sub.unregister()
for p in self._paths:
if p.sub:
p.sub.unregister()
for p in self._polygons:
if p.sub:
p.sub.unregister()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
self.pix = ImageQt(self._map)
self.setSceneRect(0,0, self.w, self.h)
self._map_item = self._scene.addPixmap(QPixmap.fromImage(self.pix))
# Everything must be mirrored
self._mirror(self._map_item)
self.centerOn(self._map_item)
self.show()
def _update_path(self, name):
old_item = None
if name in self._paths.keys():
old_item = self._paths[name].item
self._paths[name].item = self._scene.addPath(self._paths[name].path, pen = QPen(QColor(*self._paths[name].color)))
# Everything must be mirrored
self._mirror(self._paths[name].item)
if old_item:
self._scene.removeItem(old_item)
def _update_polygon(self, name):
old_item = None
if name in self._polygons.keys():
old_item = self._polygons[name].item
self._polygons[name].item = self._scene.addPolygon(self._polygons[name].path, pen = QPen(QColor(*self._polygons[name].color)))
# Everything must be mirrored
self._mirror(self._polygons[name].item)
if old_item:
self._scene.removeItem(old_item)
def _mirror(self, item):
item.scale(-1,1)
item.translate(-self.w, 0)
def __init__(self, context):
super(HipViewerPlugin, self).__init__(context)
self.setObjectName('HipViewer')
self._current_dotcode = None
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'hip_viewer.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('HipViewerUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._widget.graphics_view.setScene(self._scene)
self._widget.graph_type_combo_box.insertItem(0, self.tr('infinite'),
-1)
self._widget.graph_type_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.graph_type_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.graph_type_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.graph_type_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.graph_type_combo_box.setCurrentIndex(0)
self._widget.refresh_graph_push_button.setIcon(
QIcon.fromTheme('view-refresh'))
self._widget.highlight_connections_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(
QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(
QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(
self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
self.connect(self, SIGNAL('update_planning_graph'),
self.update_planning_graph_synchronous)
# start the planner in a separate thread
planning_node = HipViewerNode(ex_callback=self.update_planning_graph)
planning_node.start()
