def to_ros_pose(self):
p = PoseStamped()
p.header.stamp = rospy.Time.from_sec(self.stamp)
p.header.frame_id = self.ros_frame_id
p.child_frame_id = ""
tran = p.pose.position
tran.x, tran.y, tran.z = self.position.as_numpy
rot = p.pose.orientation
rot.x, rot.y, rot.z, rot.w = self.quaternion.as_numpy
return p
def pq_to_pose_stamped(p, q, source_frame, target_frame, stamp=None):
""" convert position, quaternion to geometry_msgs/PoseStamped
Args:
p (np.array): position array of [x, y, z]
q (np.array): quaternion array of [x, y, z, w]
source_frame (string): name of tf source frame
target_frame (string): name of tf target frame
Returns:
pose_stamped (geometry_msgs/PoseStamped): ROS geometric message to be converted of given p and q
"""
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = source_frame
if stamp is None: stamp = rospy.Time.now()
pose_stamped.header.stamp = stamp
pose_stamped.child_frame_id = target_frame
pose_stamped.pose = pq_to_pose(p, q)
return pose_stamped
