#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