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

可以使用以下Python函数将点云话题转换为MoveIt!的场景对象：

import rospy
import open3d as o3d
import numpy as np
from sensor_msgs.msg import PointCloud2
from moveit_msgs.msg import PlanningScene, CollisionObject
from shape_msgs.msg import SolidPrimitive, Plane, Mesh
from geometry_msgs.msg import Pose, Point, Quaternion

def point_cloud_to_scene(data):
    # Convert PointCloud2 to numpy array
    pc = ros_numpy.numpify(data)
    points = np.zeros((pc.shape[0], 3))
    points[:, 0] = pc['x']
    points[:, 1] = pc['y']
    points[:, 2] = pc['z']

    # Triangulate using Open3D
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
    pcd.orient_normals_consistent_tangent_plane(30)
    pcd = pcd.remove_non_manifold_edges()
    mesh, _ = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd, depth=8)
    mesh.compute_vertex_normals()

    # Convert mesh to CollisionObject
    obj = CollisionObject()
    obj.header.frame_id = data.header.frame_id
    obj.id = "point_cloud"
    obj.operation = CollisionObject.ADD
    mesh_msg = Mesh()
    mesh_msg.vertices = [Point(p[0], p[1], p[2]) for p in mesh.vertices]
    mesh_msg.triangles = [mesh.triangles[i].astype(int) for i in range(len(mesh.triangles))]
    obj.meshes = [mesh_msg]
    obj.mesh_poses = [Pose(position=Point(0, 0, 0), orientation=Quaternion(0, 0, 0, 1))]
    obj.meshes[0].triangulation_type = Mesh.TRIANGLE_LIST
    obj.meshes[0].vertex_normals = [Point(n[0], n[1], n[2]) for n in mesh.vertex_normals]

    # Create PlanningScene and add object
    scene = PlanningScene()
    scene.world.collision_objects.append(obj)
    scene.is_diff = True

    return scene

这个函数将输入的 `PointCloud2` 消息转换为 `open3d.geometry.PointCloud` 对象，并使用 Poisson 重建算法将其三角化为 `open3d.geometry.TriangleMesh`。然后，将 `TriangleMesh` 转换为 `moveit_msgs/CollisionObject` 消息，并将其添加到 MoveIt! 场景中。最后，返回包含新场景的 `PlanningScene` 消息。