Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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.