rviz selects point cloud data and publishes it as a point cloud topic

First, write a python program that publishes point cloud data, named cloud_pub.py.

#! /usr/bin/env python3.8
# encoding: utf-8
import open3d as o3d
import numpy as np
import rospy
from sensor_msgs.msg import PointCloud2, PointField
from sensor_msgs import point_cloud2
from std_msgs.msg import Header

# 类
class PointRead(object):
    def __init__(self):
        self.pcloud = rospy.Publisher('/read_pointcloud', PointCloud2, queue_size=1)
        data = o3d.io.read_point_cloud("./src/data/point_cloud/pointcloud_1.pcd")
        zpath = np.array(data.points)
        points_pcd = o3d.geometry.PointCloud()# 传入3d点云
        points_pcd.points = o3d.utility.Vector3dVector(zpath)
        # publish
        header = Header()
        header.frame_id = "map" 
        header.stamp = rospy.Time.now()
        fields = [PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1)
        ]
        rate=rospy.Rate(1)
        pc = point_cloud2.create_cloud(header, fields, points_pcd.points)  
        while not rospy.is_shutdown():      
            self.pcloud.publish(pc)
            rate.sleep()

# 主函数
if __name__ == "__main__": 
    rospy.init_node("select_pc")
    # 发布点云话题
    PointRead()
    rospy.spin()

Run roscore.

roscore

Run Publish Point Cloud.

rosrun <package> cloud_pub.py

Run rviz.

rviz

add point cloud topic, get your own point cloud.

 Use the "Select" tool in `rviz` to select point cloud data and publish the selected data to another topic, which can be achieved by writing a ROS node. The following code is how to select point cloud data in `rviz` and publish it to another topic:

import rospy

from sensor_msgs.msg import PointCloud2

from visualization_msgs.msg import InteractiveMarkerFeedback

import sensor_msgs.point_cloud2 as pc2


def marker_feedback_callback(msg):

    if msg.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:

        # 获取点击的点的位置信息

        x = msg.pose.position.x

        y = msg.pose.position.y

        z = msg.pose.position.z


        # 创建点云数据

        fields = [pc2.PointField(name='x', offset=0, datatype=pc2.PointField.FLOAT32, count=1),

                  pc2.PointField(name='y', offset=4, datatype=pc2.PointField.FLOAT32, count=1),

                  pc2.PointField(name='z', offset=8, datatype=pc2.PointField.FLOAT32, count=1)]

        header = rospy.Header(frame_id='base_link')  # 假设点云相对于base_link坐标系

        cloud = pc2.create_cloud_xyz32(header, [[x, y, z]])


        # 发布点云数据到另一个话题

        pub.publish(cloud)


if __name__ == '__main__':

    # 初始化ROS节点

    rospy.init_node('point_cloud_selector', anonymous=True)


    # 创建一个发布器，用于发布所选点云数据

    pub = rospy.Publisher('selected_point_cloud_topic', PointCloud2, queue_size=10)


    # 创建一个订阅器，用于接收InteractiveMarkerFeedback消息

    rospy.Subscriber('/interactive_marker_feedback', InteractiveMarkerFeedback, marker_feedback_callback)


    # 循环等待回调

    rospy.spin()

In the above example, we first import the required ROS message types and libraries. Then, in the `marker_feedback_callback` callback function, we check the event type of the `InteractiveMarkerFeedback` message, and perform the selection operation when the mouse is pressed. We get the position information of the selected point from `msg.pose.position`, and use the `create_cloud_xyz32` function to create a point cloud data containing the point. Then, we publish the point cloud data to the topic `selected_point_cloud_topic` via the publisher `pub`.

To run the above code, make sure your ROS environment is properly set up, and save the code as an executable. Then, execute the following command in the terminal to start the ROS node:

rosrun <package> interacpy

Replace <package> and `` with the package name. After execution, the node will receive the message of the `/interactive_marker_feedback` topic in `rviz`, and publish the selected point cloud data to the `selected_point_cloud_topic` topic according to the selected point.

In `rviz`, you can add a `PointCloud2` display to subscribe to the `selected_point_cloud_topic` topic to view the selected point cloud data.

Note that before using the "Select" tool, make sure the interactive markup tool is enabled in `rviz`.

To enable the interactive markup tool in `rviz`, you can follow these steps:

1. Open `rviz` and load your configuration file (if any).

2. In the `rviz` toolbar, click the "Add" button (the plus icon) to add a new visualizer.

3. In the dialog box that pops up, select "InteractiveMarkers" and click "OK".

4. Now, you will see an "Interact" button in the toolbar of `rviz`.

5. Click the "Interact" button to enable the interactive markup tool.

6. Now you can interact with the interactive markers in `rviz`. You can click, drag or rotate the marker to trigger actions related to the marker.

 Finally add the selected point cloud topic.

The selected part of the point cloud can be obtained.