Skip to content

Instantly share code, notes, and snippets.

Created February 5, 2019 22:34
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
What would you like to do?
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "fix_tf_cpp");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0);
while (node.ok()){
transform.setOrigin( tf::Vector3(5.0, 2.0, 0.0) );
// transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); // another way to do the angles
transform.setRotation(tf::createQuaternionFromRPY(0., 0., 0.));
// the signature order in C++ is always being parent then child
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "child_cpp"));
geometry_msgs::TransformStamped transformStamped;
transformStamped = tfBuffer.lookupTransform("world", "child_cpp",
catch (tf2::TransformException &ex) {
// the received data is a geometry_msgs/Transform, we need to convert it to tf::Transform.
tf::transformMsgToTF(transformStamped.transform, transform);
// inverse the tf and create a frame
transform = transform.inverse();
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "child_cpp", "world_inv_cpp"));
return 0;
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment