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