A robotic system typically has many 3D coordinate frames that change over time, such as a world frame, base frame, gripper frame, head frame, etc. tf keeps track of all these frames over time, and allows you to ask questions like:
Where was the head frame relative to the world frame, 5s ago? What is the pose of the object in my gripper relative to my base? What is the current pose of the base frame in the map frame?
There is no center server of transform information.
This is the code :
#include <ros/ros.h> #include <tf2_ros/transform_listener.h> //The tf2 packge provides an implementation of a TransformListener to help make the task of receiving transforms easier. #include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/Twist.h> #include <turtlesim/Spawn.h>
int main(int argc, char** argv){ ros::init(argc, argv, "my_tf2_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn"); ros::ServiceClient spawner = node.serviceClientturtlesim::Spawn("spawn"); turtlesim::Spawn turtle; turtle.request.x = 4; turtle.request.y = 2; turtle.request.theta = 0; turtle.request.name = "turtle2"; spawner.call(turtle);
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf2_ros::Buffer tfBuffer; //Here,we create a TransformListener object. tf2_ros::TransformListener tfListener(tfBuffer);//Once the Listenser is created ,it starts receiving tf2 transformations over the wire, and buffer them for up to 10s.
ros::Rate rate(10.0); while (node.ok()){ geometry_msgs::TransformStamped transformStamped; try{ transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0)); /*Here, the real work is done. The arguments of lookupTransform() is ,we want the transform to target frame("turtle2") * *from source frame("trutle1"), the time at which we want to transform by providing ros::Time(0) ,and the Duration *before timeout, which is optional , default=ros::Duration(0.0). */ } catch (tf2::TransformException &ex) { ROS_WARN("%s",ex.what()); ros::Duration(1.0).sleep(); continue; }
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x); vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2)); turtle_vel.publish(vel_msg);
rate.sleep(); } return 0; };
For many tasks it is easier to think inside a local frame. tf2 allows you to define a local frame for each sensor ,link, etc in your system. And tf2 will take care of all the extra frame transforms that are introduced.
tf2 builds up a tree structure of frames, which means it does not allow a closed loop in the frame structure. So a frame only has one single parent, but it can have several children. If we want to add a frame to tf2, one of the tree existing frames needs to be the parent frame and the new one would be a child frame.
This is the code:
#include <ros/ros.h> #include <tf2_ros/transform_broadcaster.h> #include <tf2/LinearMath/Quaternion.h>
int main(int argc, char** argv){ ros::init(argc, argv, "my_tf2_broadcaster"); ros::NodeHandle node;
tf2_ros::TransformBroadcaster tfb; geometry_msgs::TransformStamped transformStamped;
transformStamped.header.frame_id = "turtle1"; transformStamped.child_frame_id = "carrot1"; transformStamped.transform.translation.x = 0.0; transformStamped.transform.translation.y = 2.0;//The carrot1 frame is 2 meters offset from the turtle1 frame transformStamped.transform.translation.z = 0.0; tf2::Quaternion q; q.setRPY(0, 0, 0);//To get quaternion value by Euler angle transformStamped.transform.rotation.x = q.x(); transformStamped.transform.rotation.y = q.y(); transformStamped.transform.rotation.z = q.z(); transformStamped.transform.rotation.w = q.w();
ros::Rate rate(10.0); while (node.ok()){ transformStamped.header.stamp = ros::Time::now(); tfb.sendTransform(transformStamped); rate.sleep(); printf("sending\n"); }