# Determining pose from ar_track_alvar message in ROS

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
id: 3
confidence: 0
pose:
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?

• Where exactly is your problem? The pose-member of the message is a geometry_msgs/Pose that contains the information you need. How does your callback look like? Oct 10 '15 at 20:54
• What language are you using? C or Python?
– Ben
Oct 12 '15 at 17:14

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
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.

• Thanks!! I tried your solution but I get an error that pose is not a member of msg. Am I doing somehting drastically wrong here? I have updated the post to add my code and the error. Oct 13 '15 at 23:04
• You need to use the -> operator when accessing an element of a pointer. So it should be: msg->pose.pose.position.x
– Ben
Oct 15 '15 at 17:45

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/