Skip to content

Instantly share code, notes, and snippets.

Show Gist options
  • Save vim5818/6cb4fb8465d311495fcdb2fe2d18c2e7 to your computer and use it in GitHub Desktop.
Save vim5818/6cb4fb8465d311495fcdb2fe2d18c2e7 to your computer and use it in GitHub Desktop.
ROS quaternion to rotation matrix OR rotation matrix to roll-pitch-yaw OR roll-pitch-yaw to quaternion
#include <iostream>
#include <tf/tf.h>
int main(){
/**< Declaration of quaternion */
tf::Quaternion q;
q.setW(1);
q.setX(0);
q.setY(0);
q.setZ(0);
tf::Quaternion q2(0, 0, 0, 1); // x, y, z, w in order
/**< quaternion -> rotation Matrix */
tf::Matrix3x3 m(q);
tf::Matrix3x3 m2;
m2.setRotation(q);
/**< rotation Matrix - > quaternion */
m.getRotation(q);
/**< rotation Matrix -> rpy */
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
/**< rpy -> quaternion */
tf::Quaternion q3;
q3.setRPY(roll, pitch, yaw);
q3.normalize();
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment