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
