Getting roll, pitch and yaw from Quaternion orientation as returned by AR marker pose

In my previous post I used the AR_track_alvar package to find AR markers from the video stream.

Now, suppose you want to use the information in the marker to find out its orientation. The topic returns a pose object which contains pose.orientation atttibute. This attibute is of type quaternion revealing four values x,y,z and w.

For my code, it was easier to deal with 'normal' (Euler) angles giving rotations around the 3D axis. Or in other words: roll, pitch and yaw.

Source: wikipedia

With the AR tag the rotation axis are aligned as follows indicated the the image.

Converting between quaternions and roll, pitch, yaw is easily done via the function getRPY(). The code fragment below illustrates how. The sample listens to the ar_pose_marker topic and if a marker is found, converts the coordinates to roll, pitch and yaw and prints them to the console.

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <ar_track_alvar_msgs/AlvarMarkers.h>

void cb(ar_track_alvar_msgs::AlvarMarkers req) {
    if (!req.markers.empty()) {
      tf::Quaternion q(req.markers[0].pose.pose.orientation.x, req.markers[0].pose.pose.orientation.y, req.markers[0].pose.pose.orientation.z, req.markers[0].pose.pose.orientation.w);
      tf::Matrix3x3 m(q);
      double roll, pitch, yaw;
      m.getRPY(roll, pitch, yaw);
      ROS_INFO("roll, pitch, yaw=%1.2f  %1.2f  %1.2f", roll, pitch, yaw);
      // roll  --> rotate around vertical axis
      // pitch --> rotate around horizontal axis
      // yaw   --> rotate around depth axis
    } // if
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "arlistener");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("ar_pose_marker", 1, cb);
  ros::spin();
  return 0;

}

Note: this code assumes -for simplicity- that there is maximum one marker in the screen at the same time.