Determining pose from ar_track_alvar message in ROS

Question

I am using the ar_track_alvar package in Indigo to detect AR Tags and determine their respective poses. I am able to run the tracker successfully as I can visualize the markers in RViz. I give the following command to print the pose values

rostopic echo /ar_pose_marker

and I get the following output indicating that the poses are determined.

header: 
  seq: 0
  stamp: 
    secs: 1444430928
    nsecs: 28760322
  frame_id: /head_camera
id: 3
confidence: 0
pose: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs: 0
    frame_id: ''
  pose: 
    position: 
      x: 0.196624979223
      y: -0.238047436646
      z: 1.16247606451
    orientation: 
      x: 0.970435431848
      y: 0.00196992162831
      z: -0.126455066154
      w: -0.205573121457

Now I want to use these poses in another ROS node and hence I need to subscribe to the appropriate ROS message('ar_pose_marker"). But I am unable to get enough information on the web on the header files and functions to use in order to extract data from the published message. It would be great if somebody can point to a reference implementation or documentation on handling these messages. It might be useful to note that ar_track_alvar is just a ROS wrapper and hence people who have used ALVAR outside of ROSmay also give their inputs.

UPDATE:

I tried to write code for the above task as suggested by @Ben in the comments but I get an error. The code is as follows

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

void printPose(const ar_track_alvar_msgs::AlvarMarker::ConstPtr& msg)
{   
    tf::Pose marker_pose_in_camera_;    

    marker_pose_in_camera_.setOrigin(tf::Vector3(msg.pose.pose.position.x,
                             msg.pose.pose.position.y,
                             msg.pose.pose.position.z));

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pose_subscriber");

    ros::NodeHandle nh;

    ros::Subscriber pose_sub = nh.subscribe("ar_pose_marker", 1000, printPose);

    ros::spin();

    return 0;

}

And I get the following error

/home/karthik/ws_ros/src/auto_land/src/pose_subscriber.cpp: In function ‘void printPose(const ConstPtr&)’:
/home/karthik/ws_ros/src/auto_land/src/pose_subscriber.cpp:17:53: error: ‘const ConstPtr’ has no member named ‘pose’
    marker_pose_in_camera_.setOrigin(tf::Vector3(msg.pose.pose));
                                                     ^
make[2]: *** [auto_land/CMakeFiles/pose_subscriber.dir/src/pose_subscriber.cpp.o] Error 1
make[1]: *** [auto_land/CMakeFiles/pose_subscriber.dir/all] Error 2
make: *** [all] Error 2

Any suggestions?

Answer 1

If you do a rostopic type /ar_pose_marker you should see something like:

ar_track_alvar_msgs/AlvarMarker

Then if you do a rosmsg show ar_track_alvar_msgs/AlvarMarker you should see something like:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 id
uint32 confidence
geometry_msgs/PoseStamped pose
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

So now you can subscribe to the /ar_pose_marker topic which is of type ar_track_alvar_msgs/AlvarMarker. (Follow the cpp or python tutorials for the syntax). Then in your callback, you can get the pose out of the message by simply doing msg.pose.pose.

Keep in mind that this is the pose in message format. It doesn't support the normal mathematical operations that you would expect. For that, you must convert it to another type such as an Eigen matrix, Numpy matrix, or KDL Frame. tf_conversions has some code for that. Or you can use some external library like tfx. Or just do it yourself.

Answer 2

may be this would be useful to you,

#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;

}

Thanks to http://ros-robotics.blogspot.de/