ros2 humble版本，采用slam_toolbox 进行导航定位，出现导航到某一处位置时候地图会乱

2939227899

标题：ros2 humble版本，采用slam_toolbox 进行导航定位，出现导航到某一处位置时候地图会乱，直接炸开的类型，并不是单纯的地图偏移。

背景(可选)：采用slam_toolbox 替换amcl进行导航定位，为了实现更加稳定的定位以及导航时地图不偏移，导致导航过程失败。

问题描述：前部分导航，都很顺利，只要导航到某一部分的时候，地图就会“炸开”，目前不知道什么原因引起的，地图图片如下：
e5c9aa6d-6fe7-4ce1-8e0b-ab949fb4b723-QQ_1724307122413.png

前面部分导航都很好只要到了这一部分，就会出现问题
出现问题的部分如下：
9cc6a886-b5dc-40cd-a739-9c80d7138e89-QQ_1724307248227.png

问题故障如下图：
e20f84b3-8e8a-4ce8-9d75-fcc21d45ebe3-QQ_1724307309052.png

到该部分终端出现报错情况如下：

[component_container_isolated-1] [ERROR] [1724307285.907588056] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown result response, ignoring...
[component_container_isolated-1] [ERROR] [1724307285.925845834] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown goal response, ignoring...

[localization_slam_toolbox_node-4] W0822 14:14:46.426656  4452 preprocessor.cc:62] Specified options.num_threads: 50 exceeds maximum available from the threading model Ceres was compiled with: 2.  Bounding to maximum number available.

完整终端：

[component_container_isolated-1] [WARN] [1724307286.908843602] [local_costmap.local_costmap]: Sensor origin at (3.97, -2.78 -0.00) is out of map bounds (2.50, -4.25, 0.00) to (5.47, -1.27, 0.78). The costmap cannot raytrace for it.
[component_container_isolated-1] [ERROR] [1724307287.086988075] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown result response, ignoring...
[component_container_isolated-1] [WARN] [1724307287.092854175] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [ERROR] [1724307287.123760977] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown goal response, ignoring...
[component_container_isolated-1] [INFO] [1724307287.136044760] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [WARN] [1724307287.207703405] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1724307287.217314417] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1724307287.221412562] [amcl]: AMCL cannot publish a pose or update the transform. Please set the initial pose...
[component_container_isolated-1] [WARN] [1724307287.610525548] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1724307287.657328986] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1724307287.702827668] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
ne]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1724307287.702827668] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [INFO] [1724307287.733120878] [amcl]: Received a 217 X 210 map @ 0.050 m/pix
[component_container_isolated-1] [INFO] [1724307287.789936056] [amcl]: createLaserObject
[component_container_isolated-1] [WARN] [1724307287.822017774] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[planner_server-2] [INFO] [1724307287.860205830] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 217 X 210 at 0.050000 m/pix
[rviz2-3] [INFO] [1724307287.904209150] [rviz2]: Trying to create a map of size 217 x 210 using 1 swatches
[component_container_isolated-1] [WARN] [1724307288.002069562] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1724307288.103269495] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [ERROR] [1724307288.143548020] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown result response, ignoring...
[rviz2-3] [INFO] [1724307287.904209150] [rviz2]: Trying to create a map of size 217 x 210 using 1 swatches
[component_container_isolated-1] [WARN] [1724307288.002069562] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1724307288.103269495] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [ERROR] [1724307288.143548020] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown result response, ignoring...
[planner_server-2] [WARN] [1724307288.146537402] [planner_server]: GridBased: failed to create plan, no valid path found.
[planner_server-2] [WARN] [1724307288.147374266] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (2.77, -4.15)
[planner_server-2] [WARN] [1724307288.147420845] [planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container_isolated-1] [ERROR] [1724307288.162093732] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown goal response, ignoring...
[component_container_isolated-1] [INFO] [1724307288.186396412] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [WARN] [1724307288.251799468] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [INFO] [1724307288.254396977] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 217 X 210 at 0.050000 m/pix
[rviz2-3] [INFO] [1724307288.414445809] [rviz2]: Trying to create a map of size 217 x 210 using 1 swatches
[component_container_isolated-1] [WARN] [1724307288.454270114] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [WARN] [1724307288.503808791] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [ERROR] [1724307289.217755150] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown goal response, ignoring...
[component_container_isolated-1] [WARN] [1724307289.227601898] [BehaviorTreeEngine]: Behavior Tree tick rate 100.00 was exceeded!
[component_container_isolated-1] [INFO] [1724307289.227947803] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-1] [ERROR] [1724307289.229403189] [bt_navigator_navigate_to_pose_rclcpp_node.rclcpp_action]: unknown result response, ignoring...
[planner_server-2] [INFO] [1724307289.232845313] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-1] [WARN] [1724307289.235189

luanch调用如下：

#!/usr/bin/python3

# Copyright (c) 2022, www.guyuehome.com
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command


def generate_launch_description():
    navigation2_dir = get_package_share_directory('originbot_navigation')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    navigation2_launch_dir = os.path.join(get_package_share_directory('rm_navigation'))

    log_level = LaunchConfiguration('log_level')
    use_respawn = LaunchConfiguration('use_respawn')
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    world = LaunchConfiguration('world')
    
    map_yaml_path = LaunchConfiguration('map',default=os.path.join(navigation2_dir,'maps','map.yaml'))
    
    nav2_param_path = LaunchConfiguration('params_file',default=os.path.join(navigation2_dir,'param','nav2_params.yaml'))
   
    slam_toolbox_map_dir = PathJoinSubstitution([navigation2_dir, 'maps', world])
    slam_toolbox_localization_file_dir = os.path.join(navigation2_dir, 'param', 'mapper_params_localization.yaml')

    rviz_config_dir = os.path.join(nav2_bringup_dir,'rviz','nav2_default_view.rviz')

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time',default_value=use_sim_time,description='Use simulation (Gazebo) clock if true'),
        DeclareLaunchArgument('map',default_value=map_yaml_path,description='Full path to map file to load'),
        DeclareLaunchArgument('params_file',default_value=nav2_param_path,description='Full path to param file to load'),
         DeclareLaunchArgument(
        'world',
        default_value='map',
        description='Select world (map file, pcd file, world file share the same name prefix as the this parameter)'),
        
        
        
        DeclareLaunchArgument(
        'slam_params_file',
        default_value=os.path.join(get_package_share_directory("originbot_navigation"),
                                   'param', 'mapper_params_localization.yaml'),
        description='Full path to the ROS2 parameters file to use for the slam_toolbox node'),



        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([nav2_bringup_dir,'/launch','/bringup_launch.py']),
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path}.items(),
        ),
        Node(
            package='nav2_planner',
             executable='planner_server',
             name='planner_server',
             output='screen',
             respawn=use_respawn,
             respawn_delay=2.0,
             arguments=['--ros-args', '--log-level', log_level]),
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
        Node(
        parameters=[
          slam_toolbox_localization_file_dir,
          {'use_sim_time': use_sim_time,
           'map_file_name': slam_toolbox_map_dir,
           'map_start_pose': [0.0, 0.0, 0.0]}
        ],
        package='slam_toolbox',
        executable='localization_slam_toolbox_node',
        name='slam_toolbox',
        output='screen')
        
       
    ])

slam_toolbox 配置文件如下;

slam_toolbox:
  ros__parameters:
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    # ROS Parameters
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: /scan
    mode: localization #localization

    # if you'd like to start localizing on bringup in a map and pose
    #map_file_name: 'cloister.yaml'
    map_start_pose: [0.0, 0.0, 0.0]

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02 #if 0 never publishes odometry
    map_update_interval: 5.0
    resolution: 0.05
    min_laser_range: 0.0 #for rastering images
    max_laser_range: 20.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps

    # General Parameters
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.5
    minimum_travel_heading: 0.5
    scan_buffer_size: 3
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1  
    link_scan_maximum_distance: 1.5
    do_loop_closing: true 
    loop_match_minimum_chain_size: 3
    loop_match_maximum_variance_coarse: 3.0  
    loop_match_minimum_response_coarse: 0.35    
    loop_match_minimum_response_fine: 0.45

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1 

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03
    loop_search_maximum_distance: 3.0

    # Scan Matcher Parameters
    distance_variance_penalty: 0.5      
    angle_variance_penalty: 1.0    

    fine_search_angle_offset: 0.00349     
    coarse_search_angle_offset: 0.349   
    coarse_angle_resolution: 0.0349        
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

尝试过的解决方法：

1.尝试过重新建图，故障依旧存在，但是地图“炸开”的位置改变了，导航到这里才会炸开，变为如下图
814b0a20-ed29-4769-bed4-9d148d53fffe-QQ_1724307970447.png

2.更换调用的RIVZ包，问题依旧。

因此前来请教各位老师和大家，希望大家帮我看看问题出现在哪，谢谢大家。

机器人小伊

针对您在使用ROS 2 Humble版本和slam_toolbox进行导航定位时遇到的问题，这里有几个可能的原因和相应的解决步骤：

1. 地图坐标系和机器人坐标系不一致

问题描述：

地图和机器人之间的坐标转换可能存在问题，导致在特定区域时地图显示异常。

解决步骤：

确保odom_frame、map_frame和base_frame在slam_toolbox的配置文件中正确设置，并且与机器人的实际坐标系相匹配。
检查TF树（使用ros2 run tf2_ros tf2_echo base_footprint map等命令）以确保坐标转换正确无误。

2. 地图分辨率和机器人定位精度不匹配

问题描述：

地图的分辨率可能过高或过低，导致机器人在某些区域定位不准确。

解决步骤：

调整slam_toolbox中的resolution参数，使其与机器人的定位精度相匹配。
检查并调整机器人的传感器配置，确保传感器数据能够准确反映环境。

3. 传感器数据问题

问题描述：

激光传感器数据可能存在噪声或异常值，导致slam_toolbox在处理数据时出错。

解决步骤：

检查激光传感器的数据输出，确保数据稳定且准确。
如果可能，尝试使用其他传感器（如视觉传感器）进行辅助定位。

4. slam_toolbox配置问题

问题描述：

slam_toolbox的配置参数可能不适合当前环境或机器人。

解决步骤：

仔细审查slam_toolbox的配置文件，特别是与扫描匹配、循环闭合等相关的参数。
尝试调整这些参数，看是否能改善定位效果。

5. 导航算法和路径规划问题

问题描述：

导航算法可能无法为机器人规划出有效的路径，或者路径规划器在处理复杂环境时出错。

解决步骤：

检查导航算法和路径规划器的配置，确保它们适合当前环境。
尝试使用不同的导航算法或路径规划器，看是否能解决问题。

6. 性能和资源限制

问题描述：

系统资源（如CPU、内存）可能不足以支持slam_toolbox和导航系统的正常运行。

解决步骤：

监控系统的资源使用情况，确保有足够的资源供slam_toolbox和导航系统使用。
如果资源不足，考虑升级硬件或优化系统配置。

7. 软件和依赖项问题

问题描述：

使用的ROS 2版本、slam_toolbox版本或其他依赖项可能存在已知的bug或兼容性问题。

解决步骤：

检查ROS 2和slam_toolbox的官方文档和社区论坛，看是否有相关的bug报告或解决方案。
尝试更新ROS 2和slam_toolbox到最新版本，或回退到较稳定的版本。

希望这些建议能帮助您解决问题。如果问题仍然存在，您可能需要更详细地检查系统的日志和配置，或者寻求来自ROS 2社区或slam_toolbox开发者的帮助。

以上为生成回答，仅供参考～

2939227899

tf树情况如下：
54223b7b-b54c-4368-bb0c-a5617fd739f0-QQ_1724309826777.png

DizzyNg

请问解决了吗这个问题我也遇到了相同的问题