how to construct a vector from quaternion

Messages like Pose or PoseStamped have a orientation which is a quaternion. I would like to construct a vector3 of length 1 facing in the direction of the orientation from the quaternion. Is there a helper function in tf to acomplish this?

Thanks!

edit retag close merge delete

Sort by » oldest newest most voted When using the TF library, you can use the method tf::quatRotate to rotate your vector by the rotation represented by the quaternion. E.g. the following code rotates your vector by 180 degrees around the x axis:

tf::Quaternion rotation(1, 0, 0, 0);
tf::Vector3 vector(0, 1, 0);
tf::Vector3 rotated_vector = tf::quatRotate(rotation, vector);

Don't forget to include tf/tf.h.

more

so for instance if i set a goal in rviz and use your code (and replace the values of rotation with the goal orientation) and construct a new point like so: goal.position + rotatedVector; it will result in a point 1 meter forward in the goal direction/orientation?

Yes. I think so. Alternatively, you can convert your goal to a tf::Stamped<tf::Pose> and multiply it with another tf::Pose with origin 1 0 0 and rotation 0 0 0 1. Same maths but less code. Also see for convertion functions: http://ros.org/doc/fuerte/api/tf/html/c++/transform__datatypes_8h.html

1

it works. thanks alot! Just one note in electric quatRotate isnt in tf namespace it is in btQuaternion.h.