Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <tf/transform_listener.h>
00003 #include <geometry_msgs/Twist.h>
00004 #include <turtlesim/Spawn.h>
00005
00006 int main(int argc, char** argv){
00007 ros::init(argc, argv, "my_tf_listener");
00008
00009 ros::NodeHandle node;
00010
00011 ros::service::waitForService("spawn");
00012 ros::ServiceClient add_turtle =
00013 node.serviceClient<turtlesim::Spawn>("spawn");
00014 turtlesim::Spawn srv;
00015 add_turtle.call(srv);
00016
00017 ros::Publisher turtle_vel =
00018 node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
00019
00020 tf::TransformListener listener;
00021
00022 ros::Rate rate(10.0);
00023 while (node.ok()){
00024 tf::StampedTransform transform;
00025 try{
00026 listener.lookupTransform("/turtle3", "/turtle1",
00027 ros::Time::now(), transform);
00028 }
00029 catch (tf::TransformException &ex) {
00030 ROS_ERROR("%s",ex.what());
00031 ros::Duration(1.0).sleep();
00032 continue;
00033 }
00034
00035 geometry_msgs::Twist vel_msg;
00036 vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
00037 transform.getOrigin().x());
00038 vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
00039 pow(transform.getOrigin().y(), 2));
00040 turtle_vel.publish(vel_msg);
00041
00042 rate.sleep();
00043 }
00044 return 0;
00045 };