How to transform PointCloud2 with TF?

edit

This is a simple code to transform scan point cloud to base link. Use a separate script for class laser_to_points2d: for ease of use.

from math import sin, cos
import tf
import numpy as np
from sensor_msgs.msg import PointCloud
from geometry_msgs.msg import Point32

 class laser_to_points2d:
    def __init__(self,):
        self.base_link_point2d=PointCloud()
        self.base_link_point2d.header.frame_id = "base_link" 

    def update(self,scandata,angle_min,angle_max,angle_increment):
        self.base_link_point2d.points=[]
        self.base_link_point2d.channels=[]

        for i in xrange(len(scandata)):     
            angle=angle_min+i*angle_increment
            if not math.isnan(scandata[i]):
                r=scandata[i]
            else:r=0.0 

            px=float(r)*cos(angle)
            py=float(r)*sin(angle)
            self.base_link_point2d.points.append(Point32(px,py, 0.0))
            self.base_link_point2d.channels.append(0)# add some information if needed

You need to listen to the transformation for Odom to base_link and subscribe the scan topic

import rospy
import roslib
from sensor_msgs.msg import LaserScan
import tf
from file_name_your_ class _laser_to_points2d import laser_to_points2d

def scanCb(msg):
    py,px=[],[]
    scandata=msg.ranges
    angle_min=msg.angle_min
    angle_max=msg.angle_max
    angle_increment=msg.angle_increment
    range_min=msg.range_min
    range_max=msg.range_max
    points2d.update(scandata,angle_min,angle_max,angle_increment)

if __name__ == "__main__":

    rospy.init_node('main', anonymous=True) #make node  
    rospy.Subscriber("/scan", LaserScan, scanCb,queue_size = 1) 
    rate = rospy.Rate(5) # 5hz         
    points2d=laser_to_points2d()
    listener = tf.TransformListener()           
    while not rospy.is_shutdown():

        listener.waitForTransform("/base_link", "/odom", rospy.Time(0),rospy.Duration(1.0))     
        p=listener.transformPointCloud("odom",points2d.base_link_point2d)   
        rate.sleep()

You can transform using the tf2_sensor_msgs (both C++ and Python interface, since at least Indigo):

#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>

const sensor_msgs::PointCloud2 cloud_in, cloud_out;
const geometry_msgs::TransformStamped transform;
// TODO load everything
tf2::doTransform (cloud_in, cloud_out, transform);

Or in Python:

from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
cloud_out = do_transform_cloud(cloud_in, transform)

Please note that all of these transform functions only transform the x,y,z channel. If your pointcloud contains viewpoints or normals, you'd have to transform them yourself.

You can use do_transform_cloud from tf2 to get this done just have to feed in the incoming cloud with the transform. This is using Python. And you'll have to convert the tuple from lookupTransform to a TransformStamped to be fed into the function. Follow this other thread for execution

link text

Cheers