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

你可以使用以下Python函数来实现将多个点云转换为MoveIt场景中的环境：

import rospy
from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import Pose, Point
from sensor_msgs.msg import PointCloud2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
import tf2_ros
import tf2_geometry_msgs

def point_cloud_to_planning_scene(cloud_topic, frame_id):
    # Initialize ROS node
    rospy.init_node('point_cloud_to_planning_scene')

    # Initialize TF2 buffer and listener
    tf_buffer = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener(tf_buffer)

    # Create PlanningScene publisher
    planning_scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene, queue_size=10)

    # Create PlanningScene message
    planning_scene = PlanningScene()
    planning_scene.world = PlanningSceneWorld()
    planning_scene.is_diff = True

    def point_cloud_callback(cloud):
        # Transform point cloud to frame_id
        try:
            transform = tf_buffer.lookup_transform(frame_id, cloud.header.frame_id, rospy.Time())
            cloud_transformed = do_transform_cloud(cloud, transform)
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            rospy.logerr('Failed to transform point cloud')
            return

        # Create CollisionObject from point cloud
        solid_primitives = []
        poses = []
        for point in cloud_transformed.points:
            # Create SolidPrimitive for each point
            solid_primitive = SolidPrimitive()
            solid_primitive.type = SolidPrimitive.SPHERE
            solid_primitive.dimensions = [0.05]  # Diameter of the sphere
            solid_primitives.append(solid_primitive)

            # Create Pose for each point
            pose = Pose()
            pose.position = Point(point.x, point.y, point.z)
            poses.append(pose)

        # Create CollisionObject for all SolidPrimitives and their poses
        collision_object = CollisionObject()
        collision_object.id = cloud.header.frame_id
        collision_object.operation = CollisionObject.ADD
        collision_object.primitives = solid_primitives
        collision_object.primitive_poses = poses

        # Add CollisionObject to PlanningSceneWorld
        planning_scene.world.collision_objects.append(collision_object)

        # Publish updated PlanningScene
        planning_scene_pub.publish(planning_scene)

    # Subscribe to point cloud topic
    rospy.Subscriber(cloud_topic, PointCloud2, point_cloud_callback)

    # Spin ROS node
    rospy.spin()

这个函数的参数是点云话题的名称和期望的帧ID。在函数内部，我们初始化了ROS节点、TF2缓存和监听器，并创建了PlanningScene发布者和PlanningScene消息。然后我们定义了一个回调函数，该回调函数将点云转换为CollisionObject并将其添加到PlanningSceneWorld中。

回调函数中的第一步是将点云转换为期望的帧ID。我们使用`tf2_ros`库来执行这个转换。然后我们遍历点云中的每个点，并为每个点创建一个SolidPrimitive和它的姿态。最后，我们将所有的SolidPrimitive和姿态组合成一个CollisionObject，并将其添加到PlanningSceneWorld中。

最后，我们订阅指定的点云话题，并在回调函数中处理每个点云消息。