ros moveit中订阅点云话题将多个点云转环境scene的Python函数

在ROS MoveIt中，将多个点云转换为环境场景（scene）的Python函数可以如下所示：

import rospy
from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import Pose, Point, Quaternion
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import numpy as np

def create_scene_from_pointcloud(pointcloud_topic):
    # Initialize ROS node
    rospy.init_node('create_scene_from_pointcloud')

    # Create a publisher to publish the scene
    scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10)

    # Wait for the PlanningSceneWorld message to become available
    planning_scene_world = None
    while planning_scene_world is None and not rospy.is_shutdown():
        try:
            planning_scene_world = rospy.wait_for_message('/move_group/monitored_planning_scene_world', PlanningSceneWorld, timeout=1)
        except:
            rospy.logwarn('Waiting for PlanningSceneWorld message...')

    # Create an empty scene
    new_scene = PlanningScene()
    new_scene.is_diff = True
    new_scene.world = planning_scene_world

    # Subscribe to the point cloud topic
    rospy.Subscriber(pointcloud_topic, PointCloud2, pointcloud_callback)

    # Spin until shutdown
    rospy.spin()

def pointcloud_callback(pointcloud_msg):
    # Convert the point cloud message to a numpy array
    points = np.array(list(point_cloud2.read_points(pointcloud_msg)))

    # Create a SolidPrimitive for each point in the point cloud
    solid_primitives = []
    for point in points:
        solid_primitive = SolidPrimitive()
        solid_primitive.type = SolidPrimitive.SPHERE
        solid_primitive.dimensions = [0.01]  # Diameter of the sphere
        pose = Pose()
        pose.position = Point(*point)
        pose.orientation = Quaternion(0, 0, 0, 1)
        solid_primitive.pose = pose
        solid_primitives.append(solid_primitive)

    # Add the SolidPrimitives to the scene
    new_scene.world.collision_objects = []
    new_scene.world.collision_objects.extend(solid_primitives)
    new_scene.world.collision_objects[-1].operation = new_scene.world.collision_objects[-1].ADD
    new_scene.is_diff = True

    # Publish the updated scene
    scene_pub.publish(new_scene)

此函数订阅从`pointcloud_topic`话题发布的点云消息，并将每个点转换为一个具有固定半径的球体（使用SolidPrimitive），将这些球体添加到环境场景中，然后发布新的场景。注意，这个函数是无限循环的，直到ROS节点被关闭。