ROS TF2 code snippts

ros_tf2.md

Prodcasting a transform

Static TF

C++

#include "geometry_msgs/TransformStamped.h"
#include "ros/ros.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include <cstdio>

int main(int argc, char **argv) {
  ros::init(argc, argv, "learn_tf");
  if (argc != 9) {
    ROS_ERROR(
        "Invalid number of parameters\nusage: static_turtle_tf2_broadcaster "
        "parent_frame_name child_frame_name x y z roll pitch yaw");
    return -1;
  }
  static tf2_ros::StaticTransformBroadcaster static_brod;
  geometry_msgs::TransformStamped tf_stamped;
  tf_stamped.header.stamp = ros::Time::now();
  tf_stamped.header.frame_id = argv[1];
  tf_stamped.child_frame_id = argv[2];
  tf_stamped.transform.translation.x = atof(argv[3]);
  tf_stamped.transform.translation.y = atof(argv[4]);
  tf_stamped.transform.translation.z = atof(argv[5]);

  tf2::Quaternion quat;
  quat.setRPY(atof(argv[6]), atof(argv[7]), atof(argv[8]));

  tf_stamped.transform.rotation.x = quat.x();
  tf_stamped.transform.rotation.y = quat.y();
  tf_stamped.transform.rotation.z = quat.z();
  tf_stamped.transform.rotation.w = quat.w();

  static_brod.sendTransform(tf_stamped);
  ROS_INFO("Spinning until killed publishing %s to %s", argv[2], argv[1]);
  ros::spin();
  return 0;
}

ROS Launch

<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1" />
</launch>

Changing TF

C++

#include <geometry_msgs/TransformStamped.h>
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

int main(int argc, char **argv) {

  ros::init(argc, argv, "my_tf2_broadcaster");
  ros::NodeHandle private_node("~"); // Get private node parameters
  ros::NodeHandle node;

  tf2_ros::TransformBroadcaster tf_brod;
  geometry_msgs::TransformStamped transformStamped;

  while (ros::ok()) {
    // Update the transform and publish it
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "world"; // Parent frame
    transformStamped.child_frame_id = "child";
    transformStamped.transform.translation.x = 0.0;
    transformStamped.transform.translation.y = 0.0;
    transformStamped.transform.translation.z = 0.0;
    tf2::Quaternion q;
    q.setRPY(0, 0, 1.577);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

    tf_brod.sendTransform(transformStamped);
  }

  return 0;
}

TF Listener

#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>

int main(int argc, char **argv) {

  ros::init(argc, argv, "tf_listener");
  ros::NodeHandle node;
  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(
          argv[1], argv[2], ros::Time(0),
          ros::Duration(1)); // How long to block ros::Duration(1)
    } catch (tf2::TransformException &ex) {
      ROS_WARN("%s", ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    ROS_INFO(transformStamped.header.frame_id.c_str());
    ROS_INFO(transformStamped.child_frame_id.c_str());
    rate.sleep();
  }

  return 0;
}

RPY to Quat

#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"

geometry_msgs::TransformStamped tf_stamped;
tf_stamped.header.stamp = ros::Time::now();
tf_stamped.header.frame_id = argv[1];
tf_stamped.child_frame_id = argv[2];
tf_stamped.transform.translation.x = atof(argv[3]);
tf_stamped.transform.translation.y = atof(argv[4]);
tf_stamped.transform.translation.z = atof(argv[5]);

tf2::Quaternion quat;
quat.setRPY(atof(argv[6]), atof(argv[7]), atof(argv[8]));

tf_stamped.transform.rotation.x = quat.x();
tf_stamped.transform.rotation.y = quat.y();
tf_stamped.transform.rotation.z = quat.z();
tf_stamped.transform.rotation.w = quat.w();

Get the latest available TF

try{
   transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
 } catch (tf2::TransformException &ex) {
   ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what());
 }

Get TF in the current time

try{
    transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time::now());
  } catch (tf2::TransformException &ex) {
    ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what());
  }