tftest.cpp
Go to the documentation of this file.
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       // Get the robot's pose 
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 


gmapping
Author(s): Brian Gerkey
autogenerated on Fri Aug 28 2015 13:09:21