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)
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)
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:
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)
