def transform_by_subjects(ps, to_frame):
'''Return a transformed PoseStamped'''
while '/' == to_frame[0]:
to_frame = to_frame[1:]
while '/' == ps.header.frame_id[0]:
ps.header.frame_id = ps.header.frame_id[1:]
if (to_frame == ps.header.frame_id):
return ps
while (not _subject_name_ok(to_frame) or not _subject_name_ok(
ps.header.frame_id)) and not rospy.is_shutdown():
print "missing subject %s %s" % (
to_frame if to_frame not in all_subjects else "",
ps.header.frame_id
if ps.header.frame_id not in all_subjects else "")
rospy.sleep(0.01)
if isinstance(ps, PoseStamped):
coord = geometry_pose_to_pose(ps.pose)
elif isinstance(ps, PointStamped):
coord = geometry_point_to_point(ps.point)
elif isinstance(ps, Vector3Stamped):
coord = geometry_point_to_point(ps.vector)
else:
return None
if to_frame == 'map':
# Destination frame is in global coordinates
forward = get_subject_transform(ps.header.frame_id)
combined = forward * coord
elif ps.header.frame_id == 'map':
# Source frame is in global coordinates
backward = get_subject_transform(to_frame)
combined = backward.inverse() * coord
else:
# General transform between two objects
backward = get_subject_transform(to_frame)
forward = get_subject_transform(ps.header.frame_id)
combined = backward.inverse() * forward * coord
if isinstance(ps, PoseStamped):
ps_out = PoseStamped()
ps_out.pose = pose_to_geometry_pose(combined)
elif isinstance(ps, PointStamped):
ps_out = PointStamped()
ps_out.point = point_to_geometry_point(combined)
elif isinstance(ps, Vector3Stamped):
ps_out = Vector3Stamped()
ps_out.vector = vector_to_geometry_vector(combined)
ps_out.header.stamp = ps.header.stamp
ps_out.header.frame_id = to_frame
return ps_out
def transform_by_subjects(ps, to_frame):
'''Return a transformed PoseStamped'''
while '/' == to_frame[0]:
to_frame = to_frame[1:]
while '/' == ps.header.frame_id[0]:
ps.header.frame_id = ps.header.frame_id[1:]
if (to_frame == ps.header.frame_id):
return ps
while (not _subject_name_ok(to_frame) or not _subject_name_ok(ps.header.frame_id)) and not rospy.is_shutdown():
print "missing subject %s %s" % (to_frame if to_frame not in all_subjects else "", ps.header.frame_id if ps.header.frame_id not in all_subjects else "")
rospy.sleep(0.01)
if isinstance(ps, PoseStamped):
coord = geometry_pose_to_pose(ps.pose)
elif isinstance(ps, PointStamped):
coord = geometry_point_to_point(ps.point)
elif isinstance(ps, Vector3Stamped):
coord = geometry_point_to_point(ps.vector)
else:
return None
if to_frame == 'map':
# Destination frame is in global coordinates
forward = get_subject_transform(ps.header.frame_id)
combined = forward * coord
elif ps.header.frame_id == 'map':
# Source frame is in global coordinates
backward = get_subject_transform(to_frame)
combined = backward.inverse() * coord
else:
# General transform between two objects
backward = get_subject_transform(to_frame)
forward = get_subject_transform(ps.header.frame_id)
combined = backward.inverse() * forward * coord
if isinstance(ps, PoseStamped):
ps_out = PoseStamped()
ps_out.pose = pose_to_geometry_pose(combined)
elif isinstance(ps, PointStamped):
ps_out = PointStamped()
ps_out.point = point_to_geometry_point(combined)
elif isinstance(ps, Vector3Stamped):
ps_out = Vector3Stamped()
ps_out.vector = vector_to_geometry_vector(combined)
ps_out.header.stamp = ps.header.stamp
ps_out.header.frame_id = to_frame
return ps_out
def CreateStampedObject(self, object):
if object._type == "geometry_msgs/Pose":
stampedObject = PoseStamped()
stampedObject.pose = object
elif object._type == "geometry_msgs/Point":
stampedObject = PointStamped()
stampedObject.point = object
elif object._type == "geometry_msgs/Vector3":
stampedObject = Vector3Stamped()
stampedObject.vector = object
else:
rospy.logwarn("msg type not found")
return
stampedObject.header.frame_id = self.frame_id
stampedObject.header.stamp = rospy.Time.now()
return stampedObject
