ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Subscribing and publishing to the same topic (/map) in a single python ros node

asked 2022-10-27 14:01:22 -0500

hunterlineage1 gravatar image

updated 2022-10-27 14:08:18 -0500

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()
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-10-28 18:56:56 -0500

wyatt gravatar image

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

edit flag offensive delete link more

Comments

Thank you for your help. Suppose I publish this /map2 topic, will I be able to view it in RViz?

hunterlineage1 gravatar image hunterlineage1  ( 2022-10-28 21:50:02 -0500 )edit
1

@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.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-10-29 10:43:41 -0500 )edit

Thank you!

hunterlineage1 gravatar image hunterlineage1  ( 2022-10-29 15:42:50 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2022-10-27 14:01:22 -0500

Seen: 500 times

Last updated: Oct 28 '22