def markerCB(self, msg):
try:
self.marker_lock.acquire()
if not self.initialize_poses:
return
self.initial_poses = {}
for marker in msg.markers:
if marker.name.startswith("EE:goal_"):
# resolve tf
if marker.header.frame_id != self.frame_id:
ps = PoseStamped(header=marker.header, pose=marker.pose)
try:
transformed_pose = self.tf_listener.transformPose(self.frame_id, ps)
self.initial_poses[marker.name[3:]] = transformed_pose.pose
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e):
rospy.logerr("tf error when resolving tf: %s" % e)
else:
self.initial_poses[marker.name[3:]] = marker.pose #tf should be resolved
finally:
self.marker_lock.release()
评论列表
文章目录