00001 #include <cstdio>
00002 #include "sensor_msgs/LaserScan.h"
00003 #include "ros/node.h"
00004 #include "tf/transform_listener.h"
00005
00006 class Test
00007 {
00008 public:
00009 Test()
00010 {
00011 node_ = new ros::Node("test");
00012 tf_ = new tf::TransformListener(*node_, true,
00013 ros::Duration(10));
00014 tf_->setExtrapolationLimit( ros::Duration().fromSec(0.2));
00015
00016 node_->subscribe("base_scan", scan_, &Test::laser_cb, this, -1);
00017 }
00018 ~Test()
00019 {
00020 delete tf_;
00021 delete node_;
00022 }
00023
00024 void laser_cb()
00025 {
00026
00027 tf::Stamped<tf::Pose> ident;
00028 tf::Stamped<tf::Transform> odom_pose;
00029 ident.setIdentity();
00030 ident.frame_id_ = "base";
00031 ident.stamp_ = scan_.header.stamp;
00032 try
00033 {
00034 this->tf_->transformPose("odom", ident, odom_pose);
00035 }
00036 catch(tf::TransformException e)
00037 {
00038 ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
00039 return;
00040 }
00041 double yaw,pitch,roll;
00042 tf::Matrix3x3 mat = odom_pose.getBasis();
00043 mat.getEulerZYX(yaw, pitch, roll);
00044
00045 printf("%f: %.6f %.6f %.6f\n",
00046 scan_.header.stamp.toSec(),
00047 odom_pose.getOrigin().x(),
00048 odom_pose.getOrigin().y(),
00049 yaw);
00050 }
00051 void spin() { node_->spin(); }
00052
00053 private:
00054 ros::Node* node_;
00055 tf::TransformListener* tf_;
00056 sensor_msgs::LaserScan scan_;
00057 };
00058
00059 int
00060 main(int argc, char** argv)
00061 {
00062 ros::init(argc, argv);
00063
00064 Test t;
00065 t.spin();
00066
00067
00068 return 0;
00069 }
00070
