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