tf/tf2 Broadcaster and Listener in C++
tf Broadcaster and Listener
tf broadcaster node written in C++ can be found here:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 |
#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <turtlesim/Pose.h> std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr& msg){ static tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); } int main(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(); return 0; }; |
In the code above, the node is subscribing to the pose of robot given by “turtle_name” parameter. Upon receiving the pose topic, it will run “poseCallback” callback function which basically broadcast the transformation between the robot frame and the world frame.
tf listener node in C++ can be found here:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 |
#include <ros/ros.h> #include <tf/transform_listener.h> #include <turtlesim/Velocity.h> #include <turtlesim/Spawn.h> int main(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<turtlesim::Velocity>("turtle2/command_velocity", 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(); } turtlesim::Velocity vel_msg; vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x()); vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)); turtle_vel.publish(vel_msg); rate.sleep(); } return 0; }; |
The following are the most important parts:
|
1 |
tf::TransformListener listener; |
This is declaring a variable representing “tf.TransformListener()”.
|
1 2 3 4 5 6 7 8 9 10 |
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(); } |
This is to perform “tf::TransformListener.lookupTransform”, which basically listen the transformation, between “turtle1” frame and “turtle2” frame, which are returned as “transform” (having data type tf::StampedTransform). The “transform” values are then fed into some mathematics to provide the required linear and angular velocity commands to move the robot.
tf2 Broadcaster and Listener
The tf2 ways to achieve the same things as described above (using tf) are below.
tf2 broadcaster node in C++ can be found here:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 |
#include <ros/ros.h> #include <tf2/LinearMath/Quaternion.h> #include <tf2_ros/transform_broadcaster.h> #include <geometry_msgs/TransformStamped.h> #include <turtlesim/Pose.h> std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr& msg){ static tf2_ros::TransformBroadcaster br; geometry_msgs::TransformStamped transformStamped; transformStamped.header.stamp = ros::Time::now(); transformStamped.header.frame_id = "world"; transformStamped.child_frame_id = turtle_name; transformStamped.transform.translation.x = msg->x; transformStamped.transform.translation.y = msg->y; transformStamped.transform.translation.z = 0.0; tf2::Quaternion q; q.setRPY(0, 0, msg->theta); transformStamped.transform.rotation.x = q.x(); transformStamped.transform.rotation.y = q.y(); transformStamped.transform.rotation.z = q.z(); transformStamped.transform.rotation.w = q.w(); br.sendTransform(transformStamped); } int main(int argc, char** argv){ ros::init(argc, argv, "my_tf2_broadcaster"); ros::NodeHandle private_node("~"); if (! private_node.hasParam("turtle")) { if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}; turtle_name = argv[1]; } else { private_node.getParam("turtle", turtle_name); } ros::NodeHandle node; ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); ros::spin(); return 0; }; |
tf2 listener node in C++ can be found here:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 |
#include <ros/ros.h> #include <tf2_ros/transform_listener.h> #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.serviceClient<turtlesim::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; tf2_ros::TransformListener tfListener(tfBuffer); ros::Rate rate(10.0); while (node.ok()){ geometry_msgs::TransformStamped transformStamped; try{ transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(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; }; |