tf_tester.cpp

#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;

    try{
      transformStamped = tfBuffer.lookupTransform("world", "child_cpp",
                               ros::Time(0));
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    // 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"));

    
    rate.sleep();
  }
  return 0;
};