ROS2 Cartographer纯定位导航遇到的问题

G果2025-12-01 15:35

目录

Cartographer纯定位效果

实验中发现：如果需要快速获取准确初始定位，需要对机器人进行移动；但是如果环境点云特征较为明显，等待3-4分钟，不需要移动机器人，算法也可以自动校正初始位置

pbstream 地图保存

参考文章：ros2-humble 使用cartographer建图并保存为pbstream格式

C++代码（保存pbstream 地图）

cpp 复制代码 
bool save_pbstream_map(const std::string& map_name, std::string& message) {
    if (!mapping_running_) {
        message = "建图未在运行，无法保存地图";
        return false;
    }

    std::string map_path = map_directory_ + "/" + map_name + ".pbstream";
    
    RCLCPP_INFO(get_logger(), "正在保存pbstream地图: %s", map_path.c_str());

    // 方法1：尝试使用简单的system命令
    std::string command1 = "timeout 120 ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "
                          "\"{filename: '" + map_path + "', include_unfinished_submaps: false}\"";
    
    RCLCPP_INFO(get_logger(), "尝试方法1: ROS服务调用");
    int result1 = system(command1.c_str());
    
    if (result1 == 0) {
        if (std::filesystem::exists(map_path)) {
            message = "pbstream地图保存成功（方法1）";
            return true;
        }
    }

    // 方法1失败，尝试方法2：使用Cartographer命令行工具
    RCLCPP_WARN(get_logger(), "方法1失败，尝试方法2");
    std::string command2 = "timeout 120 ros2 run cartographer_ros cartographer_pbstream_map_writer -f " + map_path;
    
    int result2 = system(command2.c_str());
    
    if (result2 == 0) {
        if (std::filesystem::exists(map_path)) {
            message = "pbstream地图保存成功（方法2）";
            return true;
        }
    }

    // 所有方法都失败
    message = "所有保存方法都失败，请检查Cartographer是否正常运行";
    RCLCPP_ERROR(get_logger(), "地图保存失败，方法1返回码: %d, 方法2返回码: %d", result1, result2);
    return false;
}

launch 启动文件

主要三个节点： nav2_bringup_node 、cartographer_node、cartographer_occupancy_grid_node

设置use_amcl=false，启动需要带use_sim_time:=false

特别注意artographer_node、cartographer_occupancy_grid_node的参数配置，这是必备的

python 复制代码 
    cartographer_node = Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time,
                     'use_amcl': use_amcl}],
        arguments=['-configuration_directory', configuration_directory,
                   '-configuration_basename', configuration_basename,
                   '-load_state_filename', pbstream_path],                   
                   )

    cartographer_occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='cartographer_occupancy_grid_node',
        name='cartographer_occupancy_grid_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time,
                     'use_amcl': use_amcl}],
        arguments=['-resolution', resolution, 
                   '-publish_period_sec', publish_period_sec],        
        )

autopatrol_cartographer.launch.py

python 复制代码 
import os
import launch
import launch_ros
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction


def generate_launch_description():
    # 获取与拼接默认路径
    autopatrol_robot_dir = get_package_share_directory('autopatrol_robot')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup_xu')
    rviz_config_dir = os.path.join( nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
    lidar_dir = get_package_share_directory('bluesea2')
    lidar_launch_dir = os.path.join(lidar_dir, 'launch')
    patrol_config_path = os.path.join( autopatrol_robot_dir, 'config', 'patrol_config.yaml')

    # 配置文件夹路径
    pkg_share = FindPackageShare(package='fishbot_cartographer').find('fishbot_cartographer')
    configuration_directory = LaunchConfiguration('configuration_directory',
                                                  default= os.path.join(pkg_share, 'config') )
    # 配置文件
    configuration_basename = LaunchConfiguration('configuration_basename', default='pure_locate.lua')
    
    # 地图的分辨率
    resolution = LaunchConfiguration('resolution', default='0.05')
    # 地图的发布周期
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='2.0')###10
    
    # 创建 Launch 配置
    use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='false')
    use_amcl = LaunchConfiguration('use_amcl', default='false')#
    pbstream_path = LaunchConfiguration(
        'load_state_filename',
        default='/home/sunrise/njau/maps/vision.pbstream'  # ← 你根据实际位置修改
    )
    nav2_param_path = launch.substitutions.LaunchConfiguration(
        'params_file', default=os.path.join(autopatrol_robot_dir, 'config', 'nav2_params_carto.yaml'))
    
   # 声明新的 Launch 参数
    use_time_de = DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
                                             description='Use simulation (Gazebo) clock if true')
    pbstream_map_de = DeclareLaunchArgument('load_state_filename', default_value=pbstream_path)
    params_de = DeclareLaunchArgument('params_file', default_value=nav2_param_path,
                                             description='Full path to param file to load')
    
    # lidar_node = IncludeLaunchDescription(
    #         PythonLaunchDescriptionSource(os.path.join(lidar_launch_dir, 'uart_lidar.launch')),
    #         )

    nav2_bringup_node = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
            # 使用 Launch 参数替换原有参数
            launch_arguments={
                'map': '',
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path}.items(),
        )
    
    # print( f"\n ============== map_yaml_path: {map_dir} ====================\n\n")

    cartographer_node = Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time,
                     'use_amcl': use_amcl}],
        arguments=['-configuration_directory', configuration_directory,
                   '-configuration_basename', configuration_basename,
                   '-load_state_filename', pbstream_path],                   
                   )

    cartographer_occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='cartographer_occupancy_grid_node',
        name='cartographer_occupancy_grid_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time,
                     'use_amcl': use_amcl}],
        arguments=['-resolution', resolution, 
                   '-publish_period_sec', publish_period_sec],        
        )


    nav2_rviz2_node = Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen')


    
    action_node_turtle_control = Node(
        package='autopatrol_robot',
        executable='patrol_node',
        # parameters=[patrol_config_path]
    )
    # action_node_patrol_client = launch_ros.actions.Node(
    #     package='autopatrol_robot',
    #     executable='speaker',
    # )

    return launch.LaunchDescription([
        use_time_de,
        pbstream_map_de,
        params_de,
        # lidar_node,
        # mipi_cam_node,
        nav2_bringup_node,
        cartographer_node,
        cartographer_occupancy_grid_node,
        
        nav2_rviz2_node,
        # action_node_turtle_control,
        # action_node_patrol_client,
    ])

启动文件是 bringup.launch.py

同时注意修改nav2配置文件 params_filed 对应的 yaml 文件中 robot_base_frame 和 frame_id 等，保证 tf 合理

cartographer 纯定位配置

激光雷达 直接链接 地图

pure_locate.lua（核心配置文件）

我遇到的问题是地图虽然在rviz2上显示了地图，但是 地图map 无法与 激光雷达frame_id 连接，nav2也失败，膨胀地图和激光点云数据都无法显示，原因是.lua配置文件缺少 TRAJECTORY_BUILDER_2D.min_range = 0.15 等配置。这里提供的 pure_locate.lua 已经测试过可根据要求使用。

lua 复制代码 
include "map_builder.lua"
include "trajectory_builder.lua"


options = {
    map_builder = MAP_BUILDER,
    trajectory_builder = TRAJECTORY_BUILDER,
    
    -- 帧配置
    map_frame = "map",
    tracking_frame = "base_scan",
    published_frame = "base_scan", 
    odom_frame = "odom",
    provide_odom_frame = false, --true,
    
    -- 传感器配置
    use_odometry = false,
    use_nav_sat = false,
    use_landmarks = false,
    num_laser_scans = 1,
    num_multi_echo_laser_scans = 0,
    num_subdivisions_per_laser_scan = 1,
    num_point_clouds = 0,
    publish_frame_projected_to_2d = true,
    -- 必要参数配置
    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.,
}

-- 配置轨迹构建器为 2D
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 50

-- 0改成0.10,比机器人半径小的都忽略
TRAJECTORY_BUILDER_2D.min_range = 0.15
-- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些
TRAJECTORY_BUILDER_2D.max_range = 16.0
-- 5改成3,传感器数据超出有效范围最大值
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 10.0
-- true改成false,不使用IMU数据，大家可以开启，然后对比下效果
TRAJECTORY_BUILDER_2D.use_imu_data = false
-- false改成true,使用实时回环检测来进行前端的扫描匹配
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 
-- 1.0改成0.1,提高对运动的敏感度
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

-- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。
POSE_GRAPH.constraint_builder.min_score = 0.65
--0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7


TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options
相关推荐
Brduino脑机接口技术答疑1 小时前
脑机接口数据处理连载（二） 数据源头解析：脑电信号的采集原理与数据特性
人工智能·python·算法·数据分析·脑机接口
Wise玩转AI1 小时前
Day 25｜智能体的“可观察性（Observability）体系”
人工智能·python·ai·chatgpt·ai智能体
p***92481 小时前
Python使用PyMySQL操作MySQL完整指南
数据库·python·mysql
kali-Myon2 小时前
NewStarCTF2025-Week5-Web
java·python·安全·web安全·php·web·ctf
DFT计算杂谈2 小时前
Abinit-10.4.7安装教程
linux·数据库·python·算法·matlab
肖永威2 小时前
VSCode调试Python命令行输入参数程序argparse模块实践
vscode·python
Chasing Aurora2 小时前
Python连接云端Linux服务器进行远程 （后端开发/深度学习）时候的注意事项
linux·开发语言·python·ubuntu·ai编程
咖啡の猫2 小时前
Python顺序结构
java·前端·python
xxxxxmy3 小时前
相向双指针—三数之和
python·算法·相向双指针
热门推荐
01GitHub 镜像站点02BongoCat - 跨平台键盘猫动画工具03UV安装并设置国内源04安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）05Linux下V2Ray安装配置指南06【保姆级教程】免费使用Gemini3的5种方法！免翻墙/国内直连0746个Nano-banana 精选提示词，持续更新中08在VSCode配置Java开发环境的保姆级教程（适配各类AI编程IDE）09本地部署阿里最新开源的Z-Image10Meta第三代“分割一切”模型——SAM 3本地部署教程：首支持文本提示分割，400万概念、30毫秒响应，检测分割追踪一网打尽