class NavView(QGraphicsView):
map_changed = Signal()
path_changed = Signal(str)
polygon_changed = Signal(str)
def __init__(self, map_topic='/map',
paths=['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint'], tf=None, parent=None):
super(NavView, self).__init__()
self._parent = parent
self._pose_mode = False
self._goal_mode = False
self.last_path = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = 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()
if tf is None:
self._tf = tf.TransformListener()
else:
self._tf = tf
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)
try:
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=100)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=100)
except TypeError:
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped)
self.setScene(self._scene)
def add_dragdrop(self, item):
# Add drag and drop functionality to all the items in the view
def c(x, e):
self.dragEnterEvent(e)
def d(x, e):
self.dropEvent(e)
item.setAcceptDrops(True)
item.dragEnterEvent = c
item.dropEvent = d
def dragEnterEvent(self, e):
if self._parent:
self._parent.dragEnterEvent(e)
def dropEvent(self, e):
if self._parent:
self._parent.dropEvent(e)
def wheelEvent(self, event):
event.ignore()
if event.delta() > 0:
self.scale(1.15, 1.15)
else:
self.scale(0.85, 0.85)
def map_cb(self, msg):
self.resolution = msg.info.resolution
self.w = msg.info.width
self.h = msg.info.height
a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
a = a.reshape((self.h, self.w))
if self.w % 4:
e = numpy.empty((self.h, 4 - self.w % 4), dtype=a.dtype, order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.w, self.h, QImage.Format_Indexed8)
for i in reversed(range(101)):
image.setColor(100 - i, qRgb(i* 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
self._map = image
self.setSceneRect(0, 0, self.w, self.h)
self.map_changed.emit()
def add_path(self, name):
path = PathInfo(name)
def c(msg):
if not self._map:
return
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:
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 self._map:
return
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 pose_mode(self):
if not self._pose_mode:
self._pose_mode = True
self._goal_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._pose_mode:
self._pose_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def goal_mode(self):
if not self._goal_mode:
self._goal_mode = True
self._pose_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._goal_mode:
self._goal_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def draw_position(self, e, mirror=True):
p = self.mapToScene(e.x(), e.y())
v = (p.x() - self.drag_start[0], p.y() - self.drag_start[1])
mag = sqrt(pow(v[0], 2) + pow(v[1], 2))
u = (v[0]/mag, v[1]/mag)
res = (u[0]*20, u[1]*20)
path = self._scene.addLine(self.drag_start[0], self.drag_start[1],
self.drag_start[0] + res[0], self.drag_start[1] + res[1])
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
self.last_path = path
if mirror:
# Mirror point over x axis
x = ((self.w / 2) - self.drag_start[0]) + (self.w /2)
else:
x = self.drag_start[0]
map_p = [x * self.resolution, self.drag_start[1] * self.resolution]
angle = atan(u[1] / u[0])
quat = quaternion_from_euler(0, 0, degrees(angle))
return map_p, quat
def mousePressEvent(self, e):
if self._goal_mode or self._pose_mode:
p = self.mapToScene(e.x(), e.y())
self.drag_start = (p.x(), p.y())
else:
super(NavView, self).mousePressEvent(e)
def mouseReleaseEvent(self, e):
if self._goal_mode:
self._goal_mode = False
map_p, quat = self.draw_position(e)
msg = PoseStamped()
msg.header.frame_id = '/map'
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = map_p[0]
msg.pose.position.y = map_p[1]
msg.pose.orientation.w = quat[0]
msg.pose.orientation.z = quat[3]
self._goal_pub.publish(msg)
elif self._pose_mode:
self._pose_mode = False
map_p, quat = self.draw_position(e)
msg = PoseWithCovarianceStamped()
msg.header.frame_id = '/map'
msg.header.stamp = rospy.Time.now()
#TODO: Is it ok to just ignore the covariance matrix here?
msg.pose.pose.orientation.w = quat[0]
msg.pose.pose.orientation.z = quat[3]
msg.pose.pose.position.x = map_p[0]
msg.pose.pose.position.y = map_p[1]
self._pose_pub.publish(msg)
# Clean up the path
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
#def mouseMoveEvent(self, e):
# if e.buttons() == Qt.LeftButton and (self._pose_mode or self._goal_mode):
# map_p, quat = self.draw_position(e)
def close(self):
if self.map_sub:
self.map_sub.unregister()
for p in self._paths.itervalues():
if p.sub:
p.sub.unregister()
for p in self._polygons.itervalues():
if p.sub:
p.sub.unregister()
super(NavView, self).close()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
# Everything must be mirrored
self._mirror(self._map_item)
# Add drag and drop functionality
self.add_dragdrop(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 save_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be saved
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be restored
pass
class NavView(QGraphicsView):
map_changed = Signal()
path_changed = Signal(str)
polygon_changed = Signal(str)
def __init__(self,
map_topic='/map',
paths=None,
polygons=None,
tf_listener=None,
parent=None):
super(NavView, self).__init__()
if paths is None:
paths = [
'/move_base/SBPLLatticePlanner/plan',
'/move_base/TrajectoryPlannerROS/local_plan'
]
if polygons is None:
polygons = ['/move_base/local_costmap/robot_footprint']
if tf_listener is None:
tf_listener = tf.TransformListener()
self._parent = parent
self._pose_mode = False
self._goal_mode = False
self.last_path = None
self.drag_start = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
# ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self._map_hash = None
self._map_item = None
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = None
self.frame_id = ""
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_listener
self.map_sub = rospy.Subscriber(map_topic, OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
try:
self._pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped,
queue_size=100)
self._goal_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=100)
except TypeError:
self._pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped)
self._goal_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped)
self.setScene(self._scene)
def add_dragdrop(self, item):
# Add drag and drop functionality to all the items in the view
def c(x, e):
self.dragEnterEvent(e)
def d(x, e):
self.dropEvent(e)
item.setAcceptDrops(True)
item.dragEnterEvent = c
item.dropEvent = d
def dragEnterEvent(self, e):
if self._parent:
self._parent.dragEnterEvent(e)
def dropEvent(self, e):
if self._parent:
self._parent.dropEvent(e)
def wheelEvent(self, event):
event.ignore()
try:
delta = event.angleDelta().y()
except AttributeError:
delta = event.delta()
if delta > 0:
self.scale(1.15, 1.15)
else:
self.scale(0.85, 0.85)
def map_cb(self, msg):
map_hash = hash(msg.data)
if map_hash == self._map_hash:
rospy.logdebug("Skipping map cb, because the map is the same")
return
self._map_hash = map_hash
self.map_resolution = msg.info.resolution
self.map_width = msg.info.width
self.map_height = msg.info.height
self.map_origin = msg.info.origin
self.frame_id = msg.header.frame_id
a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
a = a.reshape((self.map_height, self.map_width))
if self.map_width % 4:
e = numpy.empty((self.map_height, 4 - self.map_width % 4),
dtype=a.dtype,
order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.map_width,
self.map_height, QImage.Format_Indexed8)
for i in reversed(range(101)):
image.setColor(100 - i, qRgb(i * 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
self._map = image
self.setSceneRect(0, 0, self.map_width, self.map_height)
self.map_changed.emit()
def add_path(self, name):
path = PathInfo(name)
def cb(msg):
if not self._map:
return
pp = QPainterPath()
# Transform everything in to the map frame
if not (msg.header.frame_id == self.frame_id
or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id,
self.frame_id, rospy.Time(),
rospy.Duration(10))
data = [
self._tf.transformPose(self.frame_id, pose)
for pose in msg.poses
]
except tf.Exception:
rospy.logerr("TF Error")
data = []
else:
data = msg.poses
if len(data) > 0:
start = data[0].pose.position
pp.moveTo(*self.point_map_to_qt((start.x, start.y)))
for pose in data:
pt = pose.pose.position
pp.lineTo(*self.point_map_to_qt((pt.x, pt.y)))
path.path = pp
self.path_changed.emit(name)
path.color = random.choice(self._colors)
self._colors.remove(path.color)
path.cb = cb
path.sub = rospy.Subscriber(path.name, Path, path.cb)
self._paths[name] = path
def add_polygon(self, name):
poly = PathInfo(name)
def cb(msg):
if not self._map:
return
if not (msg.header.frame_id == self.frame_id
or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id,
self.frame_id, 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 = []
for pt in points_stamped:
point = self._tf.transformPoint(self.frame_id,
pt).point
trans_pts.append((point.x, point.y))
except tf.Exception:
rospy.logerr("TF Error")
trans_pts = []
else:
trans_pts = [(pt.x, pt.y) for pt in msg.polygon.points]
if len(trans_pts) > 0:
trans_pts.append(trans_pts[0])
pts = [QPointF(*self.point_map_to_qt(pt)) for pt in trans_pts]
poly.path = QPolygonF(pts)
self.polygon_changed.emit(name)
poly.color = random.choice(self._colors)
self._colors.remove(poly.color)
poly.cb = cb
poly.sub = rospy.Subscriber(poly.name, PolygonStamped, poly.cb)
self._polygons[name] = poly
def pose_mode(self):
if not self._pose_mode:
self._pose_mode = True
self._goal_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._pose_mode:
self._pose_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def goal_mode(self):
if not self._goal_mode:
self._goal_mode = True
self._pose_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._goal_mode:
self._goal_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def draw_position(self, e):
p = self.mapToScene(e.x(), e.y())
v = (p.x() - self.drag_start[0], p.y() - self.drag_start[1])
mag = sqrt(pow(v[0], 2) + pow(v[1], 2))
v = (v[0] / mag, v[1] / mag) # Normalize diff vector
u = (-v[1], v[0]) # Project diff vector to mirrored map
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
res = (v[0] * 25, v[1] * 25)
if self._pose_mode:
pen = QPen(QColor("red"))
elif self._goal_mode:
pen = QPen(QColor("green"))
self.last_path = self._scene.addLine(self.drag_start[0],
self.drag_start[1],
self.drag_start[0] + res[0],
self.drag_start[1] + res[1], pen)
map_p = self.point_qt_to_map(self.drag_start)
angle = atan2(u[0], u[1])
quat = quaternion_from_euler(0, 0, angle)
self.drag_start = None
return map_p, quat
def mousePressEvent(self, e):
if self._goal_mode or self._pose_mode:
p = self.mapToScene(e.x(), e.y())
self.drag_start = (p.x(), p.y())
else:
super(NavView, self).mousePressEvent(e)
def mouseReleaseEvent(self, e):
if self._goal_mode:
map_p, quat = self.draw_position(e)
self.goal_mode(
) # Disable goal_mode and enable dragging/scrolling again
msg = PoseStamped()
msg.header.frame_id = self.frame_id
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = map_p[0]
msg.pose.position.y = map_p[1]
msg.pose.orientation.z = quat[2]
msg.pose.orientation.w = quat[3]
self._goal_pub.publish(msg)
elif self._pose_mode:
map_p, quat = self.draw_position(e)
self.pose_mode(
) # Disable pose_mode and enable dragging/scrolling again
msg = PoseWithCovarianceStamped()
msg.header.frame_id = self.frame_id
msg.header.stamp = rospy.Time.now()
# ToDo: Is it ok to just ignore the covariance matrix here?
msg.pose.pose.orientation.z = quat[2]
msg.pose.pose.orientation.w = quat[3]
msg.pose.pose.position.x = map_p[0]
msg.pose.pose.position.y = map_p[1]
self._pose_pub.publish(msg)
def close(self):
if self.map_sub:
self.map_sub.unregister()
for p in self._paths.values():
if p.sub:
p.sub.unregister()
for p in self._polygons.values():
if p.sub:
p.sub.unregister()
super(NavView, self).close()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
# Everything must be mirrored
self._mirror(self._map_item)
# Add drag and drop functionality
self.add_dragdrop(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)))
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)))
if old_item:
self._scene.removeItem(old_item)
def _mirror(self, item):
"""
Mirror any QItem to have correct orientation
:param item:
:return:
"""
item.setTransform(QTransform().scale(1, -1).translate(
0, -self.map_height))
def point_qt_to_map(self, point):
"""
Convert point from Qt to map coordinates
:param point: tuple or list
:return: map point
"""
# Mirror point over y axis
x = point[0]
y = self.map_height - point[1]
# Orientation might need to be taken into account
return [
x * self.map_resolution + self.map_origin.position.x,
y * self.map_resolution + self.map_origin.position.y
]
def point_map_to_qt(self, point):
"""
Convert point from map to qt coordinates
:param point: tuple or list
:return: map point
"""
# Orientation might need to be taken into account
x = (point[0] - self.map_origin.position.x) / self.map_resolution
y = (point[1] - self.map_origin.position.y) / self.map_resolution
# Mirror point over y axis
return [x, self.map_height - y]
class NavView(QGraphicsView):
map_changed = Signal()
path_changed = Signal(str)
polygon_changed = Signal(str)
def __init__(self, map_topic='/map',
paths=['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint'], tf=None, parent=None):
super(NavView, self).__init__()
self._parent = parent
self._pose_mode = False
self._goal_mode = False
self.last_path = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = 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()
if tf is None:
self._tf = tf.TransformListener()
else:
self._tf = tf
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)
try:
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=100)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=100)
except TypeError:
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped)
self.setScene(self._scene)
def add_dragdrop(self, item):
# Add drag and drop functionality to all the items in the view
def c(x, e):
self.dragEnterEvent(e)
def d(x, e):
self.dropEvent(e)
item.setAcceptDrops(True)
item.dragEnterEvent = c
item.dropEvent = d
def dragEnterEvent(self, e):
if self._parent:
self._parent.dragEnterEvent(e)
def dropEvent(self, e):
if self._parent:
self._parent.dropEvent(e)
def wheelEvent(self, event):
event.ignore()
try:
delta = event.angleDelta().y()
except AttributeError:
delta = event.delta()
if delta > 0:
self.scale(1.15, 1.15)
else:
self.scale(0.85, 0.85)
def map_cb(self, msg):
self.resolution = msg.info.resolution
self.w = msg.info.width
self.h = msg.info.height
a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
a = a.reshape((self.h, self.w))
if self.w % 4:
e = numpy.empty((self.h, 4 - self.w % 4), dtype=a.dtype, order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.w, self.h, QImage.Format_Indexed8)
for i in reversed(range(101)):
image.setColor(100 - i, qRgb(i* 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
self._map = image
self.setSceneRect(0, 0, self.w, self.h)
self.map_changed.emit()
def add_path(self, name):
path = PathInfo(name)
def c(msg):
if not self._map:
return
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:
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 self._map:
return
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 pose_mode(self):
if not self._pose_mode:
self._pose_mode = True
self._goal_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._pose_mode:
self._pose_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def goal_mode(self):
if not self._goal_mode:
self._goal_mode = True
self._pose_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._goal_mode:
self._goal_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def draw_position(self, e, mirror=True):
p = self.mapToScene(e.x(), e.y())
v = (p.x() - self.drag_start[0], p.y() - self.drag_start[1])
mag = sqrt(pow(v[0], 2) + pow(v[1], 2))
u = (v[0]/mag, v[1]/mag)
res = (u[0]*20, u[1]*20)
path = self._scene.addLine(self.drag_start[0], self.drag_start[1],
self.drag_start[0] + res[0], self.drag_start[1] + res[1])
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
self.last_path = path
if mirror:
# Mirror point over x axis
x = ((self.w / 2) - self.drag_start[0]) + (self.w /2)
else:
x = self.drag_start[0]
map_p = [x * self.resolution, self.drag_start[1] * self.resolution]
angle = atan(u[1] / u[0])
quat = quaternion_from_euler(0, 0, degrees(angle))
return map_p, quat
def mousePressEvent(self, e):
if self._goal_mode or self._pose_mode:
p = self.mapToScene(e.x(), e.y())
self.drag_start = (p.x(), p.y())
else:
super(NavView, self).mousePressEvent(e)
def mouseReleaseEvent(self, e):
if self._goal_mode:
self._goal_mode = False
map_p, quat = self.draw_position(e)
msg = PoseStamped()
msg.header.frame_id = '/map'
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = map_p[0]
msg.pose.position.y = map_p[1]
msg.pose.orientation.w = quat[0]
msg.pose.orientation.z = quat[3]
self._goal_pub.publish(msg)
elif self._pose_mode:
self._pose_mode = False
map_p, quat = self.draw_position(e)
msg = PoseWithCovarianceStamped()
msg.header.frame_id = '/map'
msg.header.stamp = rospy.Time.now()
#TODO: Is it ok to just ignore the covariance matrix here?
msg.pose.pose.orientation.w = quat[0]
msg.pose.pose.orientation.z = quat[3]
msg.pose.pose.position.x = map_p[0]
msg.pose.pose.position.y = map_p[1]
self._pose_pub.publish(msg)
# Clean up the path
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
#def mouseMoveEvent(self, e):
# if e.buttons() == Qt.LeftButton and (self._pose_mode or self._goal_mode):
# map_p, quat = self.draw_position(e)
def close(self):
if self.map_sub:
self.map_sub.unregister()
for p in self._paths.itervalues():
if p.sub:
p.sub.unregister()
for p in self._polygons.itervalues():
if p.sub:
p.sub.unregister()
super(NavView, self).close()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
# Everything must be mirrored
self._mirror(self._map_item)
# Add drag and drop functionality
self.add_dragdrop(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 save_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be saved
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be restored
pass