-1
$\begingroup$

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?

$\endgroup$
2
  • $\begingroup$ 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? $\endgroup$
    – FooTheBar
    Oct 10 '15 at 20:54
  • $\begingroup$ What language are you using? C or Python? $\endgroup$
    – Ben
    Oct 12 '15 at 17:14
1
$\begingroup$

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.

$\endgroup$
2
  • $\begingroup$ 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. $\endgroup$
    – Nagsaver
    Oct 13 '15 at 23:04
  • $\begingroup$ You need to use the -> operator when accessing an element of a pointer. So it should be: msg->pose.pose.position.x $\endgroup$
    – Ben
    Oct 15 '15 at 17:45
0
$\begingroup$

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/

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.