ros2中常用的三维构图工具

ROS 2中常用的三维构图工具是OctoMap。下面是一个简单的OctoMap构图示例的代码：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from octomap_msgs.msg import Octomap
from octomap_msgs.srv import GetOctomap

class OctoMapper(Node):
    def __init__(self):
        super().__init__('octo_mapper')
        self.subscription = self.create_subscription(
            PointCloud2,
            'point_cloud_topic',
            self.callback,
            10)
        self.publisher = self.create_publisher(Octomap, 'octomap_topic', 10)
        self.octomap_client = self.create_client(GetOctomap, 'octomap_binary')

    def callback(self, msg):
        # Convert PointCloud2 to PointCloud
        # 假设点云消息的坐标系为 "map"，需要根据实际情况修改
        point_cloud = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True, uvs=[])

        # Initialize OctoMap
        octree = octomap.OcTree(0.1)

        # Insert points into the OctoMap
        for point in point_cloud:
            octree.update_node(point, True)

        # Create Octomap message
        octomap_msg = Octomap()
        octomap_msg.header.frame_id = "map"
        octomap_msg.binary = True
        octomap_msg.data = octree.write_binary()

        # Publish Octomap message
        self.publisher.publish(octomap_msg)

        # Request Octomap service
        request = GetOctomap.Request()
        request.header.frame_id = "map"
        future = self.octomap_client.call_async(request)

def main(args=None):
    rclpy.init(args=args)
    node = OctoMapper()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

该代码订阅了一个名为 point_cloud_topic 的点云话题，并将点云转换为OctoMap格式，然后发布到名为 octomap_topic 的话题中，并通过OctoMap的服务 octomap_binary 请求二进制OctoMap数据。