Hello The Construct Community!
What I’m trying to do is sounding very simple but is proving to be very hard (For me as a ROS rookie at least).
The setup:
- Ubuntu 18.04
- ROS version melodic
So in short I want to do something that seems simple. I would like to:
- Subscribe to a topic (with msg type PointCloud)
- Grab the transform from topic_frame to world_frame
- Use the transform on the subscribed topic and publish that into a new topic.
Here is my current strategy to test the transform out but is not working. I constantly get the error of the transform not being available but it works just fine with “rosrun tf tf_echo source target”.
#! /usr/bin/env python
import sys
import rospy
import math
import tf
from tf import TransformerROS as TransformerROS
import tf2_ros
import sensor_msgs.msg
publisher = rospy.Publisher('source_to_target/Pointcloud', sensor_msgs.msg.PointCloud, queue_size=5)
def pos_callback(msg):
print 'Callback funtion called'
sub_msg = sensor_msgs.msg.PointCloud()
sub_msg.header = msg.header
sub_msg.channels = msg.channels
sub_msg.points = msg.points
if len(sub_msg.points) > 0:
try:
listener.waitForTransform('source', 'target', rospy.Time.now(), rospy.Duration(0.1))
pp = trans_ros.transformPointCloud('target', sub_msg)
print "Original:"
print sub_msg.points[0]
print "Transformed"
print pp.points[0]
publisher.publish(pp)
except Exception as e:
print e
if __name__ == '__main__':
rospy.init_node('Pointcloud_transformer')
print 'starting node'
listener = tf.TransformListener()
trans_ros = tf.TransformerROS()
print 'init transformlistener'
rospy.Subscriber("Source/PointCould", sensor_msgs.msg.PointCloud, pos_callback)
print 'Sub'
rate = rospy.Rate(10.0)
rospy.spin()
Now how this should be done I can’t seem to figure out. Does anyone have a good answer to how this should be done in a good way?