ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
ok worked this out - the x_vec, y_vec, z_vec form the columns of the rotation matrix so the solution is simply:
tf2::Matrix3x3 rot_mat;
tf2::Quaternion q;
rot_mat.setValue(x_vec.x, y_vec.x, z_vec.x,
x_vec.y, y_vec.y, z_vec.y,
x_vec.z, y_vec.z, z_vec.z);
rot_mat.getRotation(q);