ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

It would be nice to have a good example page/repo that shows transforming PointCloud2 with tf2... or even just a good answer- here for example (I'll test and improve the code here once I get a chance).

There is a doTransform example in #q12890 https://answers.ros.org/question/12890/transforming-pointcloud2/ in comment form:

import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
import tf2_ros

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
# TODO wait for transform up to certain timeout
transform = tf_buffer.lookup_transform("camera_link", "map", rospy.Time())  # TODO use the time from the pointcloud
pcd2_camera = do_transform_cloud(pcd2, transform)

That example isn't using the correct timestamp in the lookup however, and ought to wait for the transform to become available at that time also.

The answer also shows using doTransform() in C++, though it would be worth fleshing out the example code to show proper looking up of the transform and exception handling.

pcl_ros has a tf1 transformPointCloud function, it would be nice if that could be updated to use tf2. It would be fewer lines of code and less error prone with timestamping than the do transform methods.

It would be nice to have a good example page/repo that shows transforming PointCloud2 with tf2... or even just a good answer- here for example (I'll test and improve the code here once I get a chance).

Python

There is a doTransform example in #q12890 https://answers.ros.org/question/12890/transforming-pointcloud2/ in comment form:form.

import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
import tf2_ros

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
# TODO wait for transform up to certain timeout
transform = tf_buffer.lookup_transform("camera_link", "map", rospy.Time())  # TODO use the time from the pointcloud
pointcloud, have a timeout
pcd2_camera = do_transform_cloud(pcd2, transform)

That example isn't using the correct timestamp in the lookup however, and ought to wait for the transform to become available at that time also.

The answer also shows using doTransform() in C++, though it would be worth fleshing also. I'll update https://github.com/lucasw/transform_point_cloud and this once I have a chance.

C++

Fleshed out the example code to show proper looking up of the transform and exception handling.

C++ example

#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
...

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
...

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  geometry_msgs::TransformStamped transform;\
  try
  {
    transform = tf_buffer_.lookupTransform(target_frame_, msg->header.frame_id,
      msg->header.stamp, ros::Duration(timeout_));
    sensor_msgs::PointCloud2 cloud_out;
    tf2::doTransform(*msg, cloud_out, transform);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }
  ...

pcl_ros

pcl_ros has a tf1 transformPointCloud function, it would be nice if that could be updated to use tf2. It would be fewer lines of code and less error prone with timestamping than the do transform methods.

Python

There is a doTransform example in #q12890 https://answers.ros.org/question/12890/transforming-pointcloud2/ in comment form.

import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
import tf2_ros

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
transform = tf_buffer.lookup_transform("camera_link", "map", rospy.Time())  # TODO use the time from the pointcloud, have a timeout
pcd2_camera = do_transform_cloud(pcd2, transform)

That example isn't using the correct timestamp in the lookup however, and ought to wait for the transform to become available at that time also. I'll update https://github.com/lucasw/transform_point_cloud and this once I have a chance.

C++

Fleshed [Fleshed out C++ exampleexample] (https://github.com/lucasw/transform_point_cloud/blob/master/src/transform_point_cloud.cpp)

roslaunch transform_point_cloud demo.launch

The code:

#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
...

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
...

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  geometry_msgs::TransformStamped transform;\
  try
  {
    transform = tf_buffer_.lookupTransform(target_frame_, msg->header.frame_id,
      msg->header.stamp, ros::Duration(timeout_));
    sensor_msgs::PointCloud2 cloud_out;
    tf2::doTransform(*msg, cloud_out, transform);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }
  ...

pcl_ros

pcl_ros has a tf1 transformPointCloud function, it would be nice if that could be updated to use tf2. It would be fewer lines of code and less error prone with timestamping than the do transform methods.

Python

There is a doTransform example in #q12890 https://answers.ros.org/question/12890/transforming-pointcloud2/ in comment form.

import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
import tf2_ros

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
transform = tf_buffer.lookup_transform("camera_link", "map", rospy.Time())  # TODO use the time from the pointcloud, have a timeout
pcd2_camera = do_transform_cloud(pcd2, transform)

That example isn't using the correct timestamp in the lookup however, and ought to wait for the transform to become available at that time also. I'll update https://github.com/lucasw/transform_point_cloud and this once I have a chance.

C++

[Fleshed Fleshed out C++ example] (https://github.com/lucasw/transform_point_cloud/blob/master/src/transform_point_cloud.cpp)example

roslaunch transform_point_cloud demo.launch

The code:

#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
...

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
...

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  geometry_msgs::TransformStamped transform;\
  try
  {
    transform = tf_buffer_.lookupTransform(target_frame_, msg->header.frame_id,
      msg->header.stamp, ros::Duration(timeout_));
    sensor_msgs::PointCloud2 cloud_out;
    tf2::doTransform(*msg, cloud_out, transform);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }
  ...

pcl_ros

pcl_ros has a tf1 transformPointCloud function, it would be nice if that could be updated to use tf2. It would be fewer lines of code and less error prone with timestamping than the do transform methods.

Python

There is a doTransform example in #q12890 https://answers.ros.org/question/12890/transforming-pointcloud2/ in comment form.form, I've fleshed it out here:

import sensor_msgs.point_cloud2 as pc2
import tf2_ros
import tf2_py as tf2
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
import tf2_ros

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
transform ...

     try:
        trans = tf_buffer.lookup_transform("camera_link", "map", rospy.Time())  # TODO use the time from the pointcloud, have a timeout
pcd2_camera tf_buffer.lookup_transform(target_frame, msg.header.frame_id,
                                                msg.header.stamp,
                                                rospy.Duration(timeout))
    except tf2.LookupException as ex:
        rospy.logwarn(str(lookup_time.to_sec()))
        rospy.logwarn(ex)
        return
    except tf2.ExtrapolationException as ex:
        rospy.logwarn(str(lookup_time.to_sec()))
        rospy.logwarn(ex)
        return
    cloud_out = do_transform_cloud(pcd2, transform)
do_transform_cloud(msg, trans)

That example isn't using the correct timestamp in the lookup however, and ought to wait for the transform to become available at that time also. I'll update https://github.com/lucasw/transform_point_cloud and this once I have a chance.

C++

Fleshed out C++ example

roslaunch transform_point_cloud demo.launch

The code:

#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
...

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
...

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  geometry_msgs::TransformStamped transform;\
  try
  {
    transform = tf_buffer_.lookupTransform(target_frame_, msg->header.frame_id,
      msg->header.stamp, ros::Duration(timeout_));
    sensor_msgs::PointCloud2 cloud_out;
    tf2::doTransform(*msg, cloud_out, transform);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }
  ...

pcl_ros

pcl_ros has a tf1 transformPointCloud function, it would be nice if that could be updated to use tf2. It would be fewer lines of code and less error prone with timestamping than the do transform methods.

Python

There is a doTransform example in #q12890 https://answers.ros.org/question/12890/transforming-pointcloud2/ in comment form, I've fleshed it out here:

import sensor_msgs.point_cloud2 as pc2
import tf2_ros
import tf2_py as tf2
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
...

     try:
        trans = tf_buffer.lookup_transform(target_frame, msg.header.frame_id,
                                                msg.header.stamp,
                                                rospy.Duration(timeout))
    except tf2.LookupException as ex:
        rospy.logwarn(str(lookup_time.to_sec()))
        rospy.logwarn(ex)
        return
    except tf2.ExtrapolationException as ex:
        rospy.logwarn(str(lookup_time.to_sec()))
        rospy.logwarn(ex)
        return
    cloud_out = do_transform_cloud(msg, trans)

That example isn't using the correct timestamp in the lookup however, and ought to wait for the transform to become available at that time also. I'll update https://github.com/lucasw/transform_point_cloud and this once I have a chance.

C++

Fleshed out C++ example

roslaunch transform_point_cloud demo.launch

The code:

#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
...

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
...

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  geometry_msgs::TransformStamped transform;\
  try
  {
    transform = tf_buffer_.lookupTransform(target_frame_, msg->header.frame_id,
      msg->header.stamp, ros::Duration(timeout_));
    sensor_msgs::PointCloud2 cloud_out;
    tf2::doTransform(*msg, cloud_out, transform);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }
  ...

pcl_ros

pcl_ros has a tf1 transformPointCloud function, it would be nice if that could be updated to use tf2. It would be fewer lines of code and less error prone with timestamping than the do transform methods.

Python

There is a doTransform example in #q12890 https://answers.ros.org/question/12890/transforming-pointcloud2/ in comment form, I've fleshed it out here:

import sensor_msgs.point_cloud2 as pc2
import tf2_ros
import tf2_py as tf2
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
...

     try:
        trans = tf_buffer.lookup_transform(target_frame, msg.header.frame_id,
                                            msg.header.stamp,
                                            rospy.Duration(timeout))
    except tf2.LookupException as ex:
        rospy.logwarn(str(lookup_time.to_sec()))
        rospy.logwarn(ex)
        return
    except tf2.ExtrapolationException as ex:
        rospy.logwarn(str(lookup_time.to_sec()))
        rospy.logwarn(ex)
        return
    cloud_out = do_transform_cloud(msg, trans)

C++

Fleshed out C++ example

roslaunch transform_point_cloud demo.launch

The code:

#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
...

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
...

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  geometry_msgs::TransformStamped transform;\
  try
  {
    transform = tf_buffer_.lookupTransform(target_frame_, msg->header.frame_id,
      msg->header.stamp, ros::Duration(timeout_));
    sensor_msgs::PointCloud2 cloud_out;
    tf2::doTransform(*msg, cloud_out, transform);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }
  ...

pcl_ros

pcl_ros has a tf1 transformPointCloud function, it would be nice if that could be updated to use tf2. It would be fewer lines of code and less error prone with timestamping than the do transform methods.

ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2