Revision history [back]

You seem to be asking how to manipulate quaternions to change orientations/rotations of poses. The easiest way to do this in ROS is to use the TF quaternion object (or the TF2 version).

You can create a TF quaternion from the geometry_msgs orientation, multiply two rotations, and then extract the geometry_msgs Quaternion.

Assuming that your PoseStamped object is called p, in C++ use:

tf::Quaternion q_old(p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w);
tf::Quaternion q_new, q_rotate;
q_rotate.setRPY(0, 0, M_PI/2)l;
q_new = q_old*q_rotate;
tf::quaternionTFToMsg(q_new, p.pose.orientation);

In Python, import tf.transformations and do:

q_old = [p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w]
q_rotate = tf.transformations.quaternion_from_euler(0, 0, pi/2)
q_new = tf.transformations.quaternion_multiply(q_old, q_rotate)
p.pose.orientation = geometry_msgs.msg.Quaternion(*q_new)

You seem to be asking how to manipulate quaternions to change orientations/rotations of poses. The easiest way to do this in ROS is to use the TF quaternion object (or the TF2 version).

You can create a TF quaternion from the geometry_msgs orientation, multiply two rotations, and then extract the geometry_msgs Quaternion.

Assuming that your PoseStamped object is called p, in C++ use:

tf::Quaternion q_old(p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w);
tf::Quaternion q_new, q_rotate;
q_rotate.setRPY(0, 0, M_PI/2)l;
M_PI);
q_new = q_old*q_rotate;
tf::quaternionTFToMsg(q_new, p.pose.orientation);

In Python, import tf.transformations and do:

q_old = [p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w]
q_rotate = tf.transformations.quaternion_from_euler(0, 0, pi/2)
pi)
q_new = tf.transformations.quaternion_multiply(q_old, q_rotate)
p.pose.orientation = geometry_msgs.msg.Quaternion(*q_new)