本文将指导如何在ROS2 Humble和Gazebo11环境下使用Cartographer进行三维SLAM建图，包括三维点云地图和八叉树地图的创建。

系统要求

• Ubuntu 22.04
• ROS2 Humble
• Gazebo 11
• Cartographer ROS2

安装步骤

1. 安装ROS2 Humble

按照官方指南安装ROS2 Humble：

sudo apt update
sudo apt install ros-humble-desktop

2. 安装Gazebo11

sudo apt install gazebo11 libgazebo11-dev

3. 安装Cartographer ROS2

安装依赖

sudo apt update
sudo apt install -y python3-wstool python3-rosdep ninja-build stow

创建工作空间

mkdir -p ~/cartographer_ws/src
cd ~/cartographer_ws/src

克隆Cartographer相关代码

git clone https://github.com/ros2/cartographer.git -b ros2
git clone https://github.com/ros2/cartographer_ros.git -b ros2

安装依赖

rosdep update
rosdep install --from-paths . --ignore-src --rosdistro humble -y

构建和安装

cd ~/cartographer_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

配置三维SLAM

1. 创建配置文件

在cartographer_ros/configuration_files目录下创建新的配置文件my_robot_3d.lua：

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = map_builder,
  trajectory_builder = trajectory_builder,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

map_builder.use_trajectory_builder_3d = true
map_builder.num_background_threads = 4

trajectory_builder_3d = trajectory_builder.trajectory_builder_3d
trajectory_builder_3d.submaps.num_range_data = 100
trajectory_builder_3d.min_range = 1.
trajectory_builder_3d.max_range = 100.
trajectory_builder_3d.motion_filter.max_angle_radians = math.rad(0.5)

return options

2. 创建启动文件

创建新的启动文件my_robot_3d.launch.py：

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='cartographer_ros',
            executable='cartographer_node',
            name='cartographer_node',
            output='screen',
            parameters=[{'use_sim_time': True}],
            arguments=[
                '-configuration_directory', 
                os.path.join(get_package_share_directory('cartographer_ros'), 'configuration_files'),
                '-configuration_basename', 'my_robot_3d.lua'
            ],
            remappings=[
                ('points2', '/camera/depth/points'),
            ]
        ),
        Node(
            package='cartographer_ros',
            executable='occupancy_grid_node',
            name='occupancy_grid_node',
            output='screen',
            parameters=[{'use_sim_time': True}],
            arguments=['-resolution', '0.05', '-publish_period_sec', '1.0']
        ),
    ])

在Gazebo中运行仿真

1. 启动Gazebo仿真环境

source /opt/ros/humble/setup.bash
gazebo --verbose /opt/ros/humble/share/gazebo_plugins/worlds/gazebo_ros_depth_camera_demo.world

2. 启动Cartographer

source ~/cartographer_ws/install/setup.bash
ros2 launch cartographer_ros my_robot_3d.launch.py

3. 启动RViz查看地图

ros2 run rviz2 rviz2 -d ~/cartographer_ws/src/cartographer_ros/cartographer_ros/configuration_files/demo_3d.rviz

生成八叉树地图

1. 安装octomap包

sudo apt install ros-humble-octomap-server

2. 创建八叉树地图生成节点

创建octomap_generator.py：

#!/usr/bin/env python3

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
import octomap

class OctomapGenerator(Node):
    def __init__(self):
        super().__init__('octomap_generator')
        self.subscription = self.create_subscription(
            PointCloud2,
            '/map',
            self.pointcloud_callback,
            10)
        self.octomap_pub = self.create_publisher(Octomap, '/octomap', 10)
        
        # 创建八叉树地图，分辨率为0.05米
        self.octree = octomap.OcTree(0.05)
        
    def pointcloud_callback(self, msg):
        # 将PointCloud2转换为八叉树地图
        # 这里需要实现PointCloud2到octomap的转换
        # 实际实现会更复杂，需要处理点云数据格式
        
        # 发布八叉树地图
        octomap_msg = Octomap()
        octomap_msg.header.frame_id = "map"
        octomap_msg.binary = True
        # 将octree序列化到消息中
        # 实际实现需要将octree转换为消息格式
        
        self.octomap_pub.publish(octomap_msg)
        
def main(args=None):
    rclpy.init(args=args)
    octomap_generator = OctomapGenerator()
    rclpy.spin(octomap_generator)
    octomap_generator.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

保存地图

1. 保存三维点云地图

ros2 run cartographer_ros cartographer_pbstream_to_ros_map \
  -map_filestem=/tmp/my_map \
  -pbstream_filename=/tmp/my_map.pbstream \
  -resolution=0.05

2. 保存八叉树地图

ros2 service call /octomap_binary get_octomap
# 然后使用适当的工具保存接收到的八叉树地图

注意事项

1. 确保Gazebo中的传感器配置正确，特别是深度相机的参数
2. 根据实际机器人模型调整Cartographer的配置文件
3. 八叉树地图生成需要额外的点云处理，可能需要使用PCL库
4. 对于大型环境，可能需要调整Cartographer的参数以提高性能和精度

故障排除

1. 如果遇到依赖问题，确保所有必要的ROS2包都已安装
2. 如果Gazebo无法启动，检查Gazebo安装和ROS2集成
3. 如果Cartographer无法接收数据，检查话题名称和 remappings

通过以上步骤，能够在ROS2 Humble和Gazebo11环境下使用Cartographer进行三维SLAM建图，并生成三维点云地图和八叉树地图。