Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <tf/transform_broadcaster.h>
00003 #include <turtlesim/Pose.h>
00004
00005 std::string turtle_name;
00006
00007
00008
00009 void poseCallback(const turtlesim::PoseConstPtr& msg){
00010 static tf::TransformBroadcaster br;
00011 tf::Transform transform;
00012 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
00013 tf::Quaternion q;
00014 q.setRPY(0, 0, msg->theta);
00015 transform.setRotation(q);
00016 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
00017 }
00018
00019 int main(int argc, char** argv){
00020 ros::init(argc, argv, "my_tf_broadcaster");
00021 if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
00022 turtle_name = argv[1];
00023
00024 ros::NodeHandle node;
00025 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
00026
00027 ros::spin();
00028 return 0;
00029 };
00030