0
$\begingroup$

Rosanswers logo

I am trying to process a pointcloud2 being generated by the /map topic, which I play from a rosbag file containing the topics /tf, /map, /ssl_slam/trajectory topics. By process, I mean I want to remove some points in the pointcloud2 which do not satisfy a certain RGB range. I was able to find the points in the pointcloud2 that do not satisfy my RGB range condition and I tried publishing them to a different topic which I named '/map2'.

However, when I try to publish the new pointcloud2 to /map2, and try to visualize it in RViz, I get the following error: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty. I checked my RViz displays and the original pointcloud2, coming from the /map topic, gets published and visualized correctly, but the /map2 topic's pointcloud2 does not appear in my window and I get the following error when I click on the PointCloud2 Status: Transform [sender=unknown_publisher] For frame []: Frame [] does not exist

My goal: (1) Subscribe to /map topic to obtain original pointcloud2 data -> (2) 'Process' the pointcloud2, i.e. by removing points which do not satisfy a certain condition; -> (3) publish this pointcloud2 data to another topic /map2 -> (4) be able to visualize the pointcloud2's from /map and /map2 topics in rviz.

How do I make my processed pointcloud2, the one that I am publishing to /map2 topic, have the same tf as the original pointcloud2 from the /map topic, so that I can visualize both pointcloud2's in rviz? I looked at how to Transform Pointcloud2 with tf and Invalid argument passed to canTransform argument ... tf2 frame_ids cannot be empty and I am struggling. Please please help, here is my code.

import pdb
import rospy
from roslib import message
import math
import numpy as np
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header
from ros_numpy.point_cloud2 import pointcloud2_to_array, array_to_pointcloud2, split_rgb_field, merge_rgb_fields

lastpcl_data = ""
started = False
pclpublisher = rospy.Publisher('/map2', PointCloud2, latch=True, queue_size=1000)

def pcl_callback(pcl2data):
    print("New message received", "\n")
    global started, lastpcl_data
    pcl_data = pcl2data # <class 'sensor_msgs.msg._PointCloud2.PointCloud2'>
    pcl2_array = pointcloud2_to_array(pcl_data)
    pcl2_array = split_rgb_field(pcl2_array)
    print('pcl2 array shape BEFORE color separation: ', pcl2_array.shape)

    maskint1 = (pcl2_array['r'] <= 190)
    maskint2 = (pcl2_array['g'] <= 190)
    maskint3 = (pcl2_array['b'] >= 140)
    pcl2_array = pcl2_array[np.logical_not(
        np.logical_and(maskint1,maskint2,maskint3))]
    
    pcl2_array = merge_rgb_fields(pcl2_array)
    print('pcl2 array shape AFTER color separation: ', pcl2_array.shape)
    lastpcl_data = array_to_pointcloud2(pcl2_array) # <class 'sensor_msgs.msg._PointCloud2.PointCloud2'>
    print('CHECKPOINT ****1')
    if (not started):
        started = True

def timer_callback(event):
    global started, pclpublisher, lastpcl_data
    if (started):
        pclpublisher.publish(lastpcl_data)
        print("Last message published", "\n")

def pcl_processor():
    rospy.init_node('pclprocessor')
    rospy.Subscriber('/map', PointCloud2, pcl_callback)
    timer = rospy.Timer(rospy.Duration(0.5), timer_callback)
    rospy.spin()
    timer.shutdown()

if __name__ == '__main__':
    print("Running")
    pcl_processor()

Originally posted by hunterlineage1 on ROS Answers with karma: 24 on 2022-10-27

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

you probably need to fill the header field on the lastpcl_data PointCloud2 message. As the array_to_pointcloud docs state you can pass the stamp and frame id to the function, you can use the same data in the messages on the map topic:

lastpcl_data = array_to_pointcloud2(pcl2_array, stamp=pcl2data.header.stamp, frame_id=pcl2data.header.frame_id) 

Originally posted by Pepis with karma: 130 on 2022-10-27

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by hunterlineage1 on 2022-10-28:
@Pepis Thank you so much for your help -- it worked instantly. Hats off to you!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.