Instantly share code, notes, and snippets.

Embed
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;
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;
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment