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

将多个点云数据转化为moveit中的场景（scene）数据，可以通过以下Python非类函数实现：

import rospy
import moveit_msgs.msg as moveit_msg
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import numpy as np

def pointcloud_to_scene(cloud_list):
    # 初始化moveit_msgs CollisionObject类型的场景
    scene_msg = moveit_msg.PlanningScene()
    scene_msg.is_diff = True

    # 循环遍历点云列表，将每个点云转化为moveit_msgs CollisionObject类型的障碍物
    for idx, cloud in enumerate(cloud_list):
        # 创建一个moveit_msgs CollisionObject类型的障碍物，类型为MESH
        object_msg = moveit_msg.CollisionObject()
        object_msg.id = "cloud_obj_" + str(idx)
        object_msg.header.frame_id = "camera_depth_optical_frame" # 根据具体情况修改
        object_msg.operation = object_msg.ADD

        # 从点云中获取点的坐标数据
        points = []
        for p in pc2.read_points(cloud, skip_nans=True):
            points.append([p[0], p[1], p[2]])
        points = np.array(points)

        # 创建一个moveit_msgs Mesh类型的形状
        shape_msg = moveit_msg.SolidPrimitive()
        shape_msg.type = shape_msg.BOX
        shape_msg.dimensions = [0.1, 0.1, 0.1] # 根据具体情况修改

        # 将Mesh形状和点云数据合并为一个障碍物
        mesh_msg = moveit_msg.Mesh()
        mesh_msg.vertices = [moveit_msg.Point(x=p[0], y=p[1], z=p[2]) for p in points]
        mesh_msg.triangles = []
        shape_msg.meshes.append(mesh_msg)
        object_msg.primitives.append(shape_msg)

        # 将障碍物添加到场景中
        scene_msg.world.collision_objects.append(object_msg)

    return scene_msg

以上代码中，我们通过`PointCloud2`类型的消息获取点云数据，并将其转化为`moveit_msgs`中的`Mesh`类型的形状数据，最后将转化好的形状和障碍物信息添加到场景中。需要注意的是，代码中的`header.frame_id`和`shape_msg.dimensions`需要根据具体情况进行修改。