Rosanswers logo

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
        self.pclpublisher = rospy.Publisher('/map', PointCloud2, latch=True, queue_size=1000)

        rospy.Subscriber('/map', PointCloud2, self.pcl_callback)

def pcl_callback(self, pcl2data):
    self.pcl_data = pcl2data
    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():
        print("CHECKPOINT: after publishing self.lastpcldata")

if __name__ == '__main__':

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

Post score: 0


1 Answer 1


Rosanswers logo

I think you should just be able to do the method you mentioned and produce a new publisher by instantiating it the same way you made the first map publisher,

        self.new_pcl_publisher = rospy.Publisher('/map2', PointCloud2, latch=True, queue_size=1000)

And from there you can publish to this one and subscribe to it

Originally posted by wyatt with karma: 26 on 2022-10-28

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by hunterlineage1 on 2022-10-28:
Thank you for your help. Suppose I publish this /map2 topic, will I be able to view it in RViz?

Comment by Mike Scheutzow on 2022-10-29:
@hunterlineage1 Yes, you can view messages from the new topic in rviz. In the left panel of rviz, expand the plugin that is displaying the pointcloud (or create one), then assign the ros topic you want that particular plugin to display from the pull-down list.

Comment by hunterlineage1 on 2022-10-29:
Thank you!


Your Answer

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