Publishing transformed PointCloud topic

edit

Hello ROS Community!

I am new to this space and ROS is kicking my but! Therefore I hope to seek some wisdom from experienced ROS wizards.

The setup:

• Ubuntu 18.04
• ROS version melodic

So in short I want to do something that seems simple. I would like to:

1. Subscribe to a topic (with msg type PointCloud)
2. Grab the transform from topic_frame to world_frame
3. Use the transform on the subscribed topic and publish that into a new topic.

Now lets give an example of how I thought this would be done. I will write in python but if C++ is better for this job than feel free to suggest solutions in C++!

#! /usr/bin/env python
import rospy
import math
import tf
import sensor_msgs

# Local variable
source_frame_pos = sensor_msgs.msg.PointCloud()

# Subscriber callback
def callback(msg):
   source_frame_pos = sensor_msgs.msg.PointCloud(msg)

# Main function
if __name__ =  '__main__':

   rospy.init_node('node_name')
   listener = tf.TransformListener()

   # Frames, hardcoded for testing purpuse
   source_frame = '/vehicle'
   target_frame = '/world'

   # Publisher node and subscriber node
   pub_trans = rospy.Publisher(source_frame + '/transformed', sensor_msgs.msg.PointCloud, queue_size=1)
   rospy.Subscriber("pointcloud_topic", sensor_msgs.msg.PointCloud, callback)
   rate = rospy.Rate(10.0)
   ctrl_c = False

   # Using alternative shutdown method (to rospy.is_shutdown())
   def shutdownhook():
      global ctrl_c
      print "Shutdown node_name."
      pos = sensor_msgs.msg.PointCloud()
      pub_transformed.publish(pos)
      ctrl_c = True

   # Calling the shutdown hook if rospy says shutdown
   rospy.on_shutdown(shutdownhook)

   # Main while loop for the publisher
   while not ctrl_c:
      try:
         (trans, rot) = listener.lookupTransform(source_frame, target_frame, rospy.Time(0))
      except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         continue

      # Use the transform on the topic
      pos_transformed = some_local_transform_function(trans, rot, source_frame_pos)
      pub_trans.publish(pos_transformed)

      # Spin the thread again at rate 10hz
      rate.sleep()

So this is what I thought at first would be the way to go. But apparently this is not the case. I suspect there is some rookie mistake i have done when it comes to how subscribers and publishers are not suppose to share the same thread or something like that. The code runs but do not do what I intend it do to. Judging by the "what I want to do list" how would you guys tackle this problem? Open to all solutions, C++, python and/or packages that does this!