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

可以使用MoveIt提供的Python API将点云转换为场景(Scene)。以下是一个简单的示例代码：

import rospy
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import PlanningScene, ObjectColor
from moveit_python import PlanningSceneInterface
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2

def pointcloud_to_scene_callback(cloud_msg):
    # Convert point cloud message to a list of points
    point_list = []
    for point in pc2.read_points(cloud_msg, skip_nans=True):
        point_list.append(point)

    # Create a PlanningScene object
    scene = PlanningScene()

    # Set the ID of the PlanningScene object
    scene.id = "my_scene"

    # Set the frame ID of the PlanningScene object
    scene.robot_state.attached_collision_objects[0].object.header.frame_id = "base_link"

    # Create an object in the PlanningScene
    object_pose = PoseStamped()
    object_pose.header.frame_id = "base_link"
    object_pose.pose.position.x = 0.5
    object_pose.pose.position.y = 0.0
    object_pose.pose.position.z = 0.5
    object_pose.pose.orientation.w = 1.0
    object_color = ObjectColor()
    object_color.id = "my_object"
    object_color.color.r = 1.0
    object_color.color.g = 0.0
    object_color.color.b = 0.0
    object_color.color.a = 1.0
    scene.world.collision_objects.append(PlanningSceneInterface.make_box("my_object", object_pose, size=(0.1, 0.1, 0.1)))
    scene.object_colors.append(object_color)

    # Publish the PlanningScene
    planning_scene_publisher.publish(scene)

if __name__ == "__main__":
    rospy.init_node("pointcloud_to_scene_node")

    # Create a PlanningSceneInterface object
    planning_scene_interface = PlanningSceneInterface()

    # Create a PlanningScenePublisher object
    planning_scene_publisher = rospy.Publisher("/planning_scene", PlanningScene, queue_size=1)

    # Subscribe to the point cloud topic
    pointcloud_subscriber = rospy.Subscriber("/camera/depth/color/points", PointCloud2, pointcloud_to_scene_callback)

    rospy.spin()

这个示例代码中，我们定义了一个名为`pointcloud_to_scene_callback`的回调函数，它将传感器点云消息转换为场景(Scene)。在这个函数中，我们首先将点云消息转换为一个点列表，然后创建一个PlanningScene对象，设置其ID和Frame ID，并创建一个对象在场景中。最后，我们将PlanningScene发布到`/planning_scene`话题中。

这个示例代码中还使用了`PlanningSceneInterface`类，它是MoveIt提供的Python API，可以用于在PlanningScene中添加或删除物体。