Go to the documentation of this file.00001 #include "ros/ros.h"
00002 #include "tf/transform_listener.h"
00003 #include "tf/message_filter.h"
00004 #include "message_filters/subscriber.h"
00005
00006 class PoseDrawer
00007 {
00008 public:
00009 PoseDrawer() : tf_(), target_frame_("turtle1")
00010 {
00011 point_sub_.subscribe(n_, "turtle_point_stamped", 10);
00012 tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
00013 tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
00014 } ;
00015
00016 private:
00017 message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
00018 tf::TransformListener tf_;
00019 tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
00020 ros::NodeHandle n_;
00021 std::string target_frame_;
00022
00023
00024 void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr)
00025 {
00026 geometry_msgs::PointStamped point_out;
00027 try
00028 {
00029 tf_.transformPoint(target_frame_, *point_ptr, point_out);
00030
00031 printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
00032 point_out.point.x,
00033 point_out.point.y,
00034 point_out.point.z);
00035 }
00036 catch (tf::TransformException &ex)
00037 {
00038 printf ("Failure %s\n", ex.what());
00039 }
00040 };
00041
00042 };
00043
00044
00045 int main(int argc, char ** argv)
00046 {
00047 ros::init(argc, argv, "pose_drawer");
00048 PoseDrawer pd;
00049 ros::spin();
00050 };