Ask Your Question

# Revision history [back]

I'm not sure what exactly you need, but I guess this snippet to rotate geometry_msg poses from this World Robot Summit project might help:

// RPY rotations are applied in the frame of the pose.
void rotatePoseByRPY(const double roll, const double pitch, const double yaw, geometry_msgs::Pose& inpose)
{
tf::Quaternion q;
tf::Quaternion qrotate = tf::createQuaternionFromRPY(roll, pitch, yaw);

tf::quaternionMsgToTF(inpose.orientation, q);

q = q * qrotate;

tf::quaternionTFToMsg(q, inpose.orientation);
}


In Python:

import tf.transformations
import geometry_msgs.msg

def rotate_pose_by_rpy(in_pose, roll, pitch, yaw):
"""
Apply an RPY rotation to a pose in its parent coordinate system.
"""
try:
if in_pose.header: # = in_pose is a PoseStamped instead of a Pose.
in_pose.pose = rotate_pose_by_rpy(in_pose.pose, roll, pitch, yaw)
return in_pose
except:
pass
q_in = [in_pose.orientation.x, in_pose.orientation.y, in_pose.orientation.z, in_pose.orientation.w]
q_rot = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
q_rotated = tf.transformations.quaternion_multiply(q_in, q_rot)

rotated_pose = copy.deepcopy(in_pose)
rotated_pose.orientation = geometry_msgs.msg.Quaternion(*q_rotated)
return rotated_pose


There are other helpful functions to work with quaternions in tf and tf2, but they're not very easy to find. Feel free to add references or examples to the tutorials where you believe they might be helpful.