My goal is to process a PointCloud2 and visualize it in RViz. I have rosbag data collected. This rosbag file includes /map, /tf, /ssl_slam/trajectory topics. The /map topic is in the form of sensor_msgs/PointCloud2 messages. In the single ros node I want to: 1) Read the incoming pointcloud2 messages (from /map topic) and perform point cloud processing, 2) publish back to the same /map topic. Essentially I am trying to modify data on a topic and republish it on the same topic (/map).
I looked at several pages explaining how to construct a publisher and subscriber in the same ros node, but I understand that if subscribe and publish to the same topic, you will get an endless loop because the callback function triggers itself by publishing. I also understand that I need to create a different topic for the outgoing data and map this outgoing data to the different topic (i.e. not the same /map topic), but I am clueless on how to go about doing this. I need to be able to visualize the pointcloud2 messages from the different topic (which I have not created yet) in RViz. This will be my 'processed' pointcloud. Please help. Here is what I have currently:
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
class PCLprocessor(object):
def __init__(self):
self.lastpcl_data = 0
rospy.init_node('pclprocessor')
self.pclpublisher = rospy.Publisher('/map', PointCloud2, latch=True, queue_size=1000)
rospy.Subscriber('/map', PointCloud2, self.pcl_callback)
rospy.spin()
def pcl_callback(self, pcl2data):
self.pcl_data = pcl2data
pdb.set_trace()
self.pcl2_array = pointcloud2_to_array(self.pcl_data)
self.pcl2_array = split_rgb_field(self.pcl2_array)
print('pcl2 array shape BEFORE color separation: ', self.pcl2_array.shape)
self.maskint1 = (self.pcl2_array['r'] <= 190)
self.maskint2 = (self.pcl2_array['g'] <= 190)
self.maskint3 = (self.pcl2_array['b'] >= 140)
self.pcl2_array = self.pcl2_array[np.logical_not(
np.logical_and(self.maskint1, self.maskint2, self.maskint3))]
self.pcl2_array = merge_rgb_fields(self.pcl2_array)
print('pcl2 array shape AFTER color separation: ', self.pcl2_array.shape)
self.lastpcl_data = array_to_pointcloud2(self.pcl2_array)
print('CHECKPOINT ****1')
def run(self):
print("CHECKPOINT: def run(self)")
r = rospy.Rate(10)
while not rospy.is_shutdown():
self.pclpublisher.publish(self.lastpcl_data)
print("CHECKPOINT: after publishing self.lastpcldata")
r.sleep(0.5)
if __name__ == '__main__':
PCLprocessor()
Originally posted by hunterlineage1 on ROS Answers with karma: 24 on 2022-10-27
Post score: 0