// /home/fengxuewei/C++_myself/tutorial_ws/src/learning_tf/src/turtle_tf_broadcaster.cpp#include<ros/ros.h>// to help make the task of publishing transforms easier.#include<tf/transform_broadcaster.h>#include<turtlesim/Pose.h>
std::string turtle_name;voidposeCallback(const turtlesim::PoseConstPtr& msg){// we create a TransformBroadcaster object that we'll use later to send the transformations over the wire.static tf::TransformBroadcaster br;// we create a Transform object, and copy the information from the 2D turtle pose into the 3D transform.
tf::Transform transform;// copy the information from the 2D turtle pose into the 3D transform
transform.setOrigin( tf::Vector3(msg->x, msg->y,0.0));
tf::Quaternion q;
q.setRPY(0,0, msg->theta);
transform.setRotation(q);// Sending a transform with a TransformBroadcaster requires four arguments.// First, we pass in the transform itself.// Now we need to give the transform being published a timestamp, we'll just stamp it with the current time, ros::Time::now().// Then, we need to pass the name of the parent frame of the link we're creating, in this case "world"// Finally, we need to pass the name of the child frame of the link we're creating, in this case this is the name of the turtle itself.
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),"world", turtle_name));}intmain(int argc,char** argv){
ros::init(argc, argv,"my_tf_broadcaster");if(argc !=2){ROS_ERROR("need turtle name as argument");return-1;};
turtle_name = argv[1];
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10,&poseCallback);
ros::spin();return0;};
#include<ros/ros.h>// to help make the task of receiving transforms easier.#include<tf/transform_listener.h>// #include <turtlesim/Velocity.h> is replaced by <geometry_msgs/Twist.h>#include<geometry_msgs/Twist.h>#include<turtlesim/Spawn.h>intmain(int argc,char** argv){
ros::init(argc, argv,"my_tf_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);
tf::TransformListener listener;
ros::Rate rate(10.0);while(node.ok()){
tf::StampedTransform transform;try{
listener.lookupTransform("/turtle2","/turtle1",
ros::Time(0), transform);}catch(tf::TransformException &ex){ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();continue;}
geometry_msgs::Twist vel_msg;
vel_msg.angular.z =4.0*atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x =0.5*sqrt(pow(transform.getOrigin().x(),2)+pow(transform.getOrigin().y(),2));
turtle_vel.publish(vel_msg);
rate.sleep();}return0;}