00001 #include <ros/ros.h>
00002 #include <tf/transform_listener.h>
00003 #include <turtlesim/Velocity.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<turtlesim::Velocity>("turtle2/command_velocity", 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("/turtle2", "/turtle1",
00027 ros::Time(0), transform);
00028 }
00029 catch (tf::TransformException ex){
00030 ROS_ERROR("%s",ex.what());
00031 ros::Duration(1.0).sleep();
00032 }
00033
00034 turtlesim::Velocity vel_msg;
00035 vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
00036 transform.getOrigin().x());
00037 vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
00038 pow(transform.getOrigin().y(), 2));
00039 turtle_vel.publish(vel_msg);
00040
00041 rate.sleep();
00042 }
00043 return 0;
00044 };
