tf2_sensor_msgs in ROS Galactic: ModuleNotFoundError

edit

Hi all,

I figured out how to manually transform my points on by one. This way tf2_sensor_msgs is not needed. Maybe this will help someone.

Note that in C++, pcl_ros::transformPointCloud() can be used to achieve the same thing with less code and in a small fraction of the time... I just couldn't figure out how to call pcl_ros::transformPointCloud from Python.

import numpy as np
import rclpy
from rclpy.node import Node
from std_msgs.msg import Header
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2
from geometry_msgs.msg import Point
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
import tf_transformations


def transform_point(trans, pt):
    # https://answers.ros.org/question/249433/tf2_ros-buffer-transform-pointstamped/
    quat = [
        trans.transform.rotation.x,
        trans.transform.rotation.y,
        trans.transform.rotation.z,
        trans.transform.rotation.w
    ]
    mat = tf_transformations.quaternion_matrix(quat)
    pt_np = [pt.x, pt.y, pt.z, 1.0]
    pt_in_map_np = np.dot(mat, pt_np)

    pt_in_map = Point()
    pt_in_map.x = pt_in_map_np[0] + trans.transform.translation.x
    pt_in_map.y = pt_in_map_np[1] + trans.transform.translation.y
    pt_in_map.z = pt_in_map_np[2] + trans.transform.translation.z

    return pt_in_map


class FrameListener(Node):

    def __init__(self):
        super().__init__('target_transformer')

        self.subscription = self.create_subscription(PointCloud2, 'pc2_raw', self.listener_callback, 10)
        self.publisher = self.create_publisher(PointCloud2, 'pc2_transformed', 10)

        self.declare_parameter('target_frame', 'base_link')
        self.target_frame = self.get_parameter('target_frame').get_parameter_value().string_value

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)


    def listener_callback(self, msg):

        timestamp = msg.header.stamp
        source_frame = msg.header.frame_id

        # Look up for the transformation between target_frame and source_frame
        # frames at a specific time
        try:
            trans = self.tf_buffer.lookup_transform(
                self.target_frame,
                source_frame,
                timestamp)
        except TransformException as ex:
            self.get_logger().info(f'Could not transform {source_frame} to {self.target_frame}: {ex}')
            pass

        new_points = []

        # walk through list of points and transform each point one by one
        for x, y, z in pc2.read_points(msg, field_names=('x', 'y', 'z')):

            pt = Point()
            pt.x, pt.y, pt.z = x, y, z

            new_pt = transform_point(trans, pt)
            new_points.append((new_pt.x, new_pt.y, new_pt.z))

        # stick transformed point into new PointCloud2 message for easy visualization
        # in the actual application, the points would be transformed and used locally
        header = Header()
        header.stamp = msg.header.stamp
        header.frame_id = self.target_frame
        pc_relative = pc2.create_cloud_xyz32(header=header, points=new_points)
        self.publisher.publish(pc_relative)


def main():
    rclpy.init()
    node = FrameListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()