ros noetic moveit 将点云话题通过open3d进行三角化，并将三角后的点云转换为环境scene的Python函数

可以使用以下Python函数来将点云话题通过Open3D进行三角化，并将三角后的点云转换为MoveIt环境scene：

import rospy
import moveit_msgs.msg as moveit_msgs
import open3d as o3d
import numpy as np

def point_cloud_to_scene(point_cloud_topic, camera_frame_id, scene_publisher):
    # Subscribe to point cloud topic
    point_cloud = rospy.wait_for_message(point_cloud_topic, sensor_msgs.msg.PointCloud2)

    # Convert point cloud to numpy array
    points = np.array(list(pc2.read_points(point_cloud, skip_nans=True)))

    # Convert numpy array to Open3D point cloud
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)

    # Triangulate point cloud
    pcd.estimate_normals()
    pcd.orient_normals_towards_camera_location()
    pcd = pcd.triangulate()

    # Convert Open3D point cloud to MoveIt scene
    mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd, alpha=0.03)
    vertices = np.asarray(mesh.vertices)
    triangles = np.asarray(mesh.triangles)
    scene_msg = moveit_msgs.PlanningScene()
    scene_msg.is_diff = True
    object_msg = moveit_msgs.PlanningSceneWorld()
    object_msg.collision_objects.append(moveit_msgs.CollisionObject())
    object_msg.collision_objects[0].id = "point_cloud"
    object_msg.collision_objects[0].header.frame_id = camera_frame_id
    object_msg.collision_objects[0].meshes.append(moveit_msgs.Mesh())
    object_msg.collision_objects[0].meshes[0].vertices = [moveit_msgs.Vertex(x=vertices[i][0], y=vertices[i][1], z=vertices[i][2]) for i in range(vertices.shape[0])]
    object_msg.collision_objects[0].meshes[0].triangles = [moveit_msgs.Triangle(mesh.vertices[i][0], mesh.vertices[i][1], mesh.vertices[i][2]) for i in range(triangles.shape[0])]
    scene_msg.world.collision_objects = object_msg.collision_objects
    scene_publisher.publish(scene_msg)

这个函数需要三个参数：

- `point_cloud_topic`：点云话题的名称。
- `camera_frame_id`：相机坐标系的名称。
- `scene_publisher`：MoveIt环境scene的发布者。

使用此函数时，您需要将其作为单独的ROS节点运行，并订阅相应的点云话题。请注意，此函数使用Open3D进行三角化，并将结果转换为MoveIt环境scene。因此，您需要安装Open3D库。