Cartographer 代码阅读分析-2

接着上一篇的分析,
主要分析ceres_scan_matcher_.Match() 与
sparse_pose_graph_->AddScan()。

先看ceres_scan_matcher_.Match() ,转到mapping_2d\scan_matching\ 里找它的头文件与源文件。

CeresScanMatcher::Match()主要就是构造约束条件,然后调用ceres库完成优化,如何优化不讲,讲讲约束条件是如何构造的。这个函数里有3个约束条件的构造。

//激光点云占据栅格的代价函数,当点云匹配到原地图中的自由状态的栅格时,代价增加;当匹配到占据状态的栅格时,代价减小。同时还有一个点云占据系数,可在配置文件中设置。

 new OccupiedSpaceCostFunctor(
       options_.occupied_space_weight() /                  
       std::sqrt(static_cast<double>(point_cloud.size())),
       point_cloud,
       probability_grid);

// 位置代价函数,当估计位置与初始给定位置距离增大时,代价也增大,同时还有一个平移权重系数,在配置文件中可设定。

new TranslationDeltaCostFunctor(
             options_.<
wjs@wjs-desktop:~/Drone_Slam$ ros2 launch fishbot_grapher test_grapher_3.launch.py [INFO] [launch]: All log files can be found below /home/wjs/.ros/log/2025-07-27-21-26-18-750077-wjs-desktop-2603 [INFO] [launch]: Default logging verbosity is set to INFO pkg_share =/home/wjs/Drone_Slam/install/fishbot_grapher/share/fishbot_grapher<== model_path =/home/wjs/Drone_Slam/install/fishbot_grapher/share/fishbot_grapher/urdf/fishbot_base.urdf<== [INFO] [sllidar_node-1]: process started with pid [2604] [INFO] [wit_ros2_imu-2]: process started with pid [2606] [INFO] [test01-3]: process started with pid [2608] [INFO] [robot_state_publisher-4]: process started with pid [2610] [INFO] [cartographer_node-5]: process started with pid [2612] [INFO] [cartographer_occupancy_grid_node-6]: process started with pid [2616] [sllidar_node-1] [INFO] [1753622779.123428206] [sllidar_node]: SLLidar running on ROS2 package SLLidar.ROS2 SDK Version:1.0.1, SLLIDAR SDK Version:2.0.0 [sllidar_node-1] [ERROR] [1753622779.166949150] [sllidar_node]: Error, operation time out. SL_RESULT_OPERATION_TIMEOUT! [ERROR] [sllidar_node-1]: process has died [pid 2604, exit code 255, cmd '/home/wjs/Drone_Slam/install/sllidar_ros2/lib/sllidar_ros2/sllidar_node --ros-args -r __node:=sllidar_node --params-file /tmp/launch_params_09et7i92']. [robot_state_publisher-4] [WARN] [1753622779.340852788] [robot_state_publisher]: No robot_description parameter, but command-line argument available. Assuming argument is name of URDF file. This backwards compatibility fallback will be removed in the future. [robot_state_publisher-4] [INFO] [1753622779.403618429] [robot_state_publisher]: got segment base_link [robot_state_publisher-4] [INFO] [1753622779.404146730] [robot_state_publisher]: got segment imu_link [robot_state_publisher-4] [INFO] [1753622779.404233622] [robot_state_publisher]: got segment laser_link [cartographer_node-5] [INFO] [1753622779.592485766] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/home/wjs/Drone_Slam/install/fishbot_grapher/share/fishbot_grapher/config/test02.lua' for 'test02.lua'. [cartographer_node-5] [INFO] [1753622779.600612764] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'. [cartographer_node-5] [INFO] [1753622779.600914554] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'. [cartographer_node-5] [INFO] [1753622779.602523216] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'. [cartographer_node-5] [INFO] [1753622779.602795339] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'. [cartographer_node-5] [INFO] [1753622779.604405038] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'. [cartographer_node-5] [INFO] [1753622779.604828741] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'. [cartographer_node-5] [INFO] [1753622779.606033589] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'. [cartographer_node-5] [INFO] [1753622779.606329861] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'. [cartographer_node-5] [INFO] [1753622779.607970765] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'. [cartographer_node-5] [INFO] [1753622779.608301075] [cartographer logger]: I0727 21:26:19.000000 2612 configuration_file_resolver.cc:41] Found '/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'. [cartographer_node-5] F0727 21:26:19.623965 2612 lua_parameter_dictionary.cc:399] Check failed: HasKey(key) Key 'collate_landmarks' not in dictionary: [cartographer_node-5] { [cartographer_node-5] collate_fixed_frame = true, [cartographer_node-5] trajectory_builder_2d = { [cartographer_node-5] adaptive_voxel_filter = { [cartographer_node-5] max_length = 0.500000, [cartographer_node-5] max_range = 50.000000, [cartographer_node-5] min_num_points = 200.000000, [cartographer_node-5] }, [cartographer_node-5] ceres_scan_matcher = { [cartographer_node-5] ceres_solver_options = { [cartographer_node-5] max_num_iterations = 20.000000, [cartographer_node-5] num_threads = 1.000000, [cartographer_node-5] use_nonmonotonic_steps = false, [cartographer_node-5] }, [cartographer_node-5] occupied_space_weight = 1.000000, [cartographer_node-5] rotation_weight = 40.000000, [cartographer_node-5] translation_weight = 10.000000, [cartographer_node-5] }, [cartographer_node-5] imu_gravity_time_constant = 10.000000, [cartographer_node-5] loop_closure_adaptive_voxel_filter = { [cartographer_node-5] max_length = 0.900000, [cartographer_node-5] max_range = 50.000000, [cartographer_node-5] min_num_points = 100.000000, [cartographer_node-5] }, [cartographer_node-5] max_range = 8.000000, [cartographer_node-5] max_z = 2.000000, [cartographer_node-5] min_range = 0.300000, [cartographer_node-5] min_z = -0.800000, [cartographer_node-5] missing_data_ray_length = 1.000000, [cartographer_node-5] motion_filter = { [cartographer_node-5] max_angle_radians = 0.017453, [cartographer_node-5] max_distance_meters = 0.200000, [cartographer_node-5] max_time_seconds = 5.000000, [cartographer_node-5] }, [cartographer_node-5] num_accumulated_range_data = 1.000000, [cartographer_node-5] pose_extrapolator = { [cartographer_node-5] constant_velocity = { [cartographer_node-5] imu_gravity_time_constant = 10.000000, [cartographer_node-5] pose_queue_duration = 0.001000, [cartographer_node-5] }, [cartographer_node-5] imu_based = { [cartographer_node-5] gravity_constant = 9.806000, [cartographer_node-5] imu_acceleration_weight = 1.000000, [cartographer_node-5] imu_rotation_weight = 1.000000, [cartographer_node-5] odometry_rotation_weight = 1.000000, [cartographer_node-5] odometry_translation_weight = 1.000000, [cartographer_node-5] pose_queue_duration = 5.000000, [cartographer_node-5] pose_rotation_weight = 1.000000, [cartographer_node-5] pose_translation_weight = 1.000000, [cartographer_node-5] solver_options = { [cartographer_node-5] max_num_iterations = 10.000000, [cartographer_node-5] num_threads = 1.000000, [cartographer_node-5] use_nonmonotonic_steps = false, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] use_imu_based = false, [cartographer_node-5] }, [cartographer_node-5] real_time_correlative_scan_matcher = { [cartographer_node-5] angular_search_window = 0.349066, [cartographer_node-5] linear_search_window = 0.100000, [cartographer_node-5] rotation_delta_cost_weight = 0.100000, [cartographer_node-5] translation_delta_cost_weight = 10.000000, [cartographer_node-5] }, [cartographer_node-5] submaps = { [cartographer_node-5] grid_options_2d = { [cartographer_node-5] grid_type = "PROBABILITY_GRID", [cartographer_node-5] resolution = 0.050000, [cartographer_node-5] }, [cartographer_node-5] num_range_data = 35.000000, [cartographer_node-5] range_data_inserter = { [cartographer_node-5] probability_grid_range_data_inserter = { [cartographer_node-5] hit_probability = 0.550000, [cartographer_node-5] insert_free_space = true, [cartographer_node-5] miss_probability = 0.490000, [cartographer_node-5] }, [cartographer_node-5] range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D", [cartographer_node-5] tsdf_range_data_inserter = { [cartographer_node-5] maximum_weight = 10.000000, [cartographer_node-5] normal_estimation_options = { [cartographer_node-5] num_normal_samples = 4.000000, [cartographer_node-5] sample_radius = 0.500000, [cartographer_node-5] }, [cartographer_node-5] project_sdf_distance_to_scan_normal = true, [cartographer_node-5] truncation_distance = 0.300000, [cartographer_node-5] update_free_space = false, [cartographer_node-5] update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.500000, [cartographer_node-5] update_weight_distance_cell_to_hit_kernel_bandwidth = 0.500000, [cartographer_node-5] update_weight_range_exponent = 0.000000, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] use_imu_data = true, [cartographer_node-5] use_online_correlative_scan_matching = true, [cartographer_node-5] voxel_filter_size = 0.025000, [cartographer_node-5] }, [cartographer_node-5] trajectory_builder_3d = { [cartographer_node-5] ceres_scan_matcher = { [cartographer_node-5] ceres_solver_options = { [cartographer_node-5] max_num_iterations = 12.000000, [cartographer_node-5] num_threads = 1.000000, [cartographer_node-5] use_nonmonotonic_steps = false, [cartographer_node-5] }, [cartographer_node-5] intensity_cost_function_options_0 = { [cartographer_node-5] huber_scale = 0.300000, [cartographer_node-5] intensity_threshold = 40.000000, [cartographer_node-5] weight = 0.500000, [cartographer_node-5] }, [cartographer_node-5] occupied_space_weight_0 = 1.000000, [cartographer_node-5] occupied_space_weight_1 = 6.000000, [cartographer_node-5] only_optimize_yaw = false, [cartographer_node-5] rotation_weight = 400.000000, [cartographer_node-5] translation_weight = 5.000000, [cartographer_node-5] }, [cartographer_node-5] high_resolution_adaptive_voxel_filter = { [cartographer_node-5] max_length = 2.000000, [cartographer_node-5] max_range = 15.000000, [cartographer_node-5] min_num_points = 150.000000, [cartographer_node-5] }, [cartographer_node-5] imu_gravity_time_constant = 10.000000, [cartographer_node-5] low_resolution_adaptive_voxel_filter = { [cartographer_node-5] max_length = 4.000000, [cartographer_node-5] max_range = 60.000000, [cartographer_node-5] min_num_points = 200.000000, [cartographer_node-5] }, [cartographer_node-5] max_range = 60.000000, [cartographer_node-5] min_range = 1.000000, [cartographer_node-5] motion_filter = { [cartographer_node-5] max_angle_radians = 0.004000, [cartographer_node-5] max_distance_meters = 0.100000, [cartographer_node-5] max_time_seconds = 0.500000, [cartographer_node-5] }, [cartographer_node-5] num_accumulated_range_data = 1.000000, [cartographer_node-5] pose_extrapolator = { [cartographer_node-5] constant_velocity = { [cartographer_node-5] imu_gravity_time_constant = 10.000000, [cartographer_node-5] pose_queue_duration = 0.001000, [cartographer_node-5] }, [cartographer_node-5] imu_based = { [cartographer_node-5] gravity_constant = 9.806000, [cartographer_node-5] imu_acceleration_weight = 1.000000, [cartographer_node-5] imu_rotation_weight = 1.000000, [cartographer_node-5] odometry_rotation_weight = 1.000000, [cartographer_node-5] odometry_translation_weight = 1.000000, [cartographer_node-5] pose_queue_duration = 5.000000, [cartographer_node-5] pose_rotation_weight = 1.000000, [cartographer_node-5] pose_translation_weight = 1.000000, [cartographer_node-5] solver_options = { [cartographer_node-5] max_num_iterations = 10.000000, [cartographer_node-5] num_threads = 1.000000, [cartographer_node-5] use_nonmonotonic_steps = false, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] use_imu_based = false, [cartographer_node-5] }, [cartographer_node-5] real_time_correlative_scan_matcher = { [cartographer_node-5] angular_search_window = 0.017453, [cartographer_node-5] linear_search_window = 0.150000, [cartographer_node-5] rotation_delta_cost_weight = 0.100000, [cartographer_node-5] translation_delta_cost_weight = 0.100000, [cartographer_node-5] }, [cartographer_node-5] rotational_histogram_size = 120.000000, [cartographer_node-5] submaps = { [cartographer_node-5] high_resolution = 0.100000, [cartographer_node-5] high_resolution_max_range = 20.000000, [cartographer_node-5] low_resolution = 0.450000, [cartographer_node-5] num_range_data = 160.000000, [cartographer_node-5] range_data_inserter = { [cartographer_node-5] hit_probability = 0.550000, [cartographer_node-5] intensity_threshold = 40.000000, [cartographer_node-5] miss_probability = 0.490000, [cartographer_node-5] num_free_space_voxels = 2.000000, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] use_intensities = false, [cartographer_node-5] use_online_correlative_scan_matching = false, [cartographer_node-5] voxel_filter_size = 0.150000, [cartographer_node-5] }, [cartographer_node-5] } [cartographer_node-5] [FATAL] [1753622779.626704366] [cartographer logger]: F0727 21:26:19.000000 2612 lua_parameter_dictionary.cc:399] Check failed: HasKey(key) Key 'collate_landmarks' not in dictionary: [cartographer_node-5] { [cartographer_node-5] collate_fixed_frame = true, [cartographer_node-5] trajectory_builder_2d = { [cartographer_node-5] adaptive_voxel_filter = { [cartographer_node-5] max_length = 0.500000, [cartographer_node-5] max_range = 50.000000, [cartographer_node-5] min_num_points = 200.000000, [cartographer_node-5] }, [cartographer_node-5] ceres_scan_matcher = { [cartographer_node-5] ceres_solver_options = { [cartographer_node-5] max_num_iterations = 20.000000, [cartographer_node-5] num_threads = 1.000000, [cartographer_node-5] use_nonmonotonic_steps = false, [cartographer_node-5] }, [cartographer_node-5] occupied_space_weight = 1.000000, [cartographer_node-5] rotation_weight = 40.000000, [cartographer_node-5] translation_weight = 10.000000, [cartographer_node-5] }, [cartographer_node-5] imu_gravity_time_constant = 10.000000, [cartographer_node-5] loop_closure_adaptive_voxel_filter = { [cartographer_node-5] max_length = 0.900000, [cartographer_node-5] max_range = 50.000000, [cartographer_node-5] min_num_points = 100.000000, [cartographer_node-5] }, [cartographer_node-5] max_range = 8.000000, [cartographer_node-5] max_z = 2.000000, [cartographer_node-5] min_range = 0.300000, [cartographer_node-5] min_z = -0.800000, [cartographer_node-5] missing_data_ray_length = 1.000000, [cartographer_node-5] motion_filter = { [cartographer_node-5] max_angle_radians = 0.017453, [cartographer_node-5] max_distance_meters = 0.200000, [cartographer_node-5] max_time_seconds = 5.000000, [cartographer_node-5] }, [cartographer_node-5] num_accumulated_range_data = 1.000000, [cartographer_node-5] pose_extrapolator = { [cartographer_node-5] constant_velocity = { [cartographer_node-5] imu_gravity_time_constant = 10.000000, [cartographer_node-5] pose_queue_duration = 0.001000, [cartographer_node-5] }, [cartographer_node-5] imu_based = { [cartographer_node-5] gravity_constant = 9.806000, [cartographer_node-5] imu_acceleration_weight = 1.000000, [cartographer_node-5] imu_rotation_weight = 1.000000, [cartographer_node-5] odometry_rotation_weight = 1.000000, [cartographer_node-5] odometry_translation_weight = 1.000000, [cartographer_node-5] pose_queue_duration = 5.000000, [cartographer_node-5] pose_rotation_weight = 1.000000, [cartographer_node-5] pose_translation_weight = 1.000000, [cartographer_node-5] solver_options = { [cartographer_node-5] max_num_iterations = 10.000000, [cartographer_node-5] num_threads = 1.000000, [cartographer_node-5] use_nonmonotonic_steps = false, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] use_imu_based = false, [cartographer_node-5] }, [cartographer_node-5] real_time_correlative_scan_matcher = { [cartographer_node-5] angular_search_window = 0.349066, [cartographer_node-5] linear_search_window = 0.100000, [cartographer_node-5] rotation_delta_cost_weight = 0.100000, [cartographer_node-5] translation_delta_cost_weight = 10.000000, [cartographer_node-5] }, [cartographer_node-5] submaps = { [cartographer_node-5] grid_options_2d = { [cartographer_node-5] grid_type = "PROBABILITY_GRID", [cartographer_node-5] resolution = 0.050000, [cartographer_node-5] }, [cartographer_node-5] num_range_data = 35.000000, [cartographer_node-5] range_data_inserter = { [cartographer_node-5] probability_grid_range_data_inserter = { [cartographer_node-5] hit_probability = 0.550000, [cartographer_node-5] insert_free_space = true, [cartographer_node-5] miss_probability = 0.490000, [cartographer_node-5] }, [cartographer_node-5] range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D", [cartographer_node-5] tsdf_range_data_inserter = { [cartographer_node-5] maximum_weight = 10.000000, [cartographer_node-5] normal_estimation_options = { [cartographer_node-5] num_normal_samples = 4.000000, [cartographer_node-5] sample_radius = 0.500000, [cartographer_node-5] }, [cartographer_node-5] project_sdf_distance_to_scan_normal = true, [cartographer_node-5] truncation_distance = 0.300000, [cartographer_node-5] update_free_space = false, [cartographer_node-5] update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.500000, [cartographer_node-5] update_weight_distance_cell_to_hit_kernel_bandwidth = 0.500000, [cartographer_node-5] update_weight_range_exponent = 0.000000, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] use_imu_data = true, [cartographer_node-5] use_online_correlative_scan_matching = true, [cartographer_node-5] voxel_filter_size = 0.025000, [cartographer_node-5] }, [cartographer_node-5] trajectory_builder_3d = { [cartographer_node-5] ceres_scan_matcher = { [cartographer_node-5] ceres_solver_options = { [cartographer_node-5] max_num_iterations = 12.000000, [cartographer_node-5] num_threads = 1.000000, [cartographer_node-5] use_nonmonotonic_steps = false, [cartographer_node-5] }, [cartographer_node-5] intensity_cost_function_options_0 = { [cartographer_node-5] huber_scale = 0.300000, [cartographer_node-5] intensity_threshold = 40.000000, [cartographer_node-5] weight = 0.500000, [cartographer_node-5] }, [cartographer_node-5] occupied_space_weight_0 = 1.000000, [cartographer_node-5] occupied_space_weight_1 = 6.000000, [cartographer_node-5] only_optimize_yaw = false, [cartographer_node-5] rotation_weight = 400.000000, [cartographer_node-5] translation_weight = 5.000000, [cartographer_node-5] }, [cartographer_node-5] high_resolution_adaptive_voxel_filter = { [cartographer_node-5] max_length = 2.000000, [cartographer_node-5] max_range = 15.000000, [cartographer_node-5] min_num_points = 150.000000, [cartographer_node-5] }, [cartographer_node-5] imu_gravity_time_constant = 10.000000, [cartographer_node-5] low_resolution_adaptive_voxel_filter = { [cartographer_node-5] max_length = 4.000000, [cartographer_node-5] max_range = 60.000000, [cartographer_node-5] min_num_points = 200.000000, [cartographer_node-5] }, [cartographer_node-5] max_range = 60.000000, [cartographer_node-5] min_range = 1.000000, [cartographer_node-5] motion_filter = { [cartographer_node-5] max_angle_radians = 0.004000, [cartographer_node-5] max_distance_meters = 0.100000, [cartographer_node-5] max_time_seconds = 0.500000, [cartographer_node-5] }, [cartographer_node-5] num_accumulated_range_data = 1.000000, [cartographer_node-5] pose_extrapolator = { [cartographer_node-5] constant_velocity = { [cartographer_node-5] imu_gravity_time_constant = 10.000000, [cartographer_node-5] pose_queue_duration = 0.001000, [cartographer_node-5] }, [cartographer_node-5] imu_based = { [cartographer_node-5] gravity_constant = 9.806000, [cartographer_node-5] imu_acceleration_weight = 1.000000, [cartographer_node-5] imu_rotation_weight = 1.000000, [cartographer_node-5] odometry_rotation_weight = 1.000000, [cartographer_node-5] odometry_translation_weight = 1.000000, [cartographer_node-5] pose_queue_duration = 5.000000, [cartographer_node-5] pose_rotation_weight = 1.000000, [cartographer_node-5] pose_translation_weight = 1.000000, [cartographer_node-5] solver_options = { [cartographer_node-5] max_num_iterations = 10.000000, [cartographer_node-5] num_threads = 1.000000, [cartographer_node-5] use_nonmonotonic_steps = false, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] use_imu_based = false, [cartographer_node-5] }, [cartographer_node-5] real_time_correlative_scan_matcher = { [cartographer_node-5] angular_search_window = 0.017453, [cartographer_node-5] linear_search_window = 0.150000, [cartographer_node-5] rotation_delta_cost_weight = 0.100000, [cartographer_node-5] translation_delta_cost_weight = 0.100000, [cartographer_node-5] }, [cartographer_node-5] rotational_histogram_size = 120.000000, [cartographer_node-5] submaps = { [cartographer_node-5] high_resolution = 0.100000, [cartographer_node-5] high_resolution_max_range = 20.000000, [cartographer_node-5] low_resolution = 0.450000, [cartographer_node-5] num_range_data = 160.000000, [cartographer_node-5] range_data_inserter = { [cartographer_node-5] hit_probability = 0.550000, [cartographer_node-5] intensity_threshold = 40.000000, [cartographer_node-5] miss_probability = 0.490000, [cartographer_node-5] num_free_space_voxels = 2.000000, [cartographer_node-5] }, [cartographer_node-5] }, [cartographer_node-5] use_intensities = false, [cartographer_node-5] use_online_correlative_scan_matching = false, [cartographer_node-5] voxel_filter_size = 0.150000, [cartographer_node-5] }, [cartographer_node-5] } [cartographer_node-5] *** Check failure stack trace: *** [cartographer_node-5] @ 0xffff861dd41c google::LogMessage::Fail() [cartographer_node-5] @ 0xffff861e46d0 google::LogMessage::SendToLog() [cartographer_node-5] @ 0xffff861dd0f4 google::LogMessage::Flush() [cartographer_node-5] @ 0xffff861deebc google::LogMessageFatal::~LogMessageFatal() [cartographer_node-5] @ 0xaaaadf20392c (unknown) [cartographer_node-5] @ 0xaaaadf20398c (unknown) [cartographer_node-5] @ 0xaaaadf203dc8 (unknown) [cartographer_node-5] @ 0xaaaadf2210a8 (unknown) [cartographer_node-5] @ 0xaaaadf1eaeec (unknown) [cartographer_node-5] @ 0xaaaadf14743c (unknown) [cartographer_node-5] @ 0xffff857c73fc (unknown) [cartographer_node-5] @ 0xffff857c74cc __libc_start_main [cartographer_node-5] @ 0xaaaadf14abf0 (unknown) [test01-3] Traceback (most recent call last): [test01-3] File "/home/wjs/.local/lib/python3.10/site-packages/serial/serialposix.py", line 322, in open [test01-3] self.fd = os.open(self.portstr, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK) [test01-3] FileNotFoundError: [Errno 2] No such file or directory: '/dev/mcu_usb' [test01-3] [test01-3] During handling of the above exception, another exception occurred: [test01-3] [test01-3] Traceback (most recent call last): [test01-3] File "/home/wjs/Drone_Slam/install/fishbot_grapher/lib/fishbot_grapher/test01", line 33, in <module> [test01-3] sys.exit(load_entry_point('fishbot-grapher==0.0.0', 'console_scripts', 'test01')()) [test01-3] File "/home/wjs/Drone_Slam/build/fishbot_grapher/fishbot_grapher/test01.py", line 101, in main [test01-3] tf_subscriber = TFSubscriber() [test01-3] File "/home/wjs/Drone_Slam/build/fishbot_grapher/fishbot_grapher/test01.py", line 20, in __init__ [test01-3] self.ser = serial.Serial("/dev/mcu_usb", 115200) [test01-3] File "/home/wjs/.local/lib/python3.10/site-packages/serial/serialutil.py", line 244, in __init__ [test01-3] self.open() [test01-3] File "/home/wjs/.local/lib/python3.10/site-packages/serial/serialposix.py", line 325, in open [test01-3] raise SerialException(msg.errno, "could not open port {}: {}".format(self._port, msg)) [test01-3] serial.serialutil.SerialException: [Errno 2] could not open port /dev/mcu_usb: [Errno 2] No such file or directory: '/dev/mcu_usb' [wit_ros2_imu-2] [INFO] [1753622781.380078547] [imu]: Serial port opening failure [ERROR] [test01-3]: process has died [pid 2608, exit code 1, cmd '/home/wjs/Drone_Slam/install/fishbot_grapher/lib/fishbot_grapher/test01 --ros-args']. [ERROR] [cartographer_node-5]: process has died [pid 2612, exit code -6, cmd '/opt/ros/humble/lib/cartographer_ros/cartographer_node -configuration_directory /home/wjs/Drone_Slam/install/fishbot_grapher/share/fishbot_grapher/config -configuration_basename test02.lua --ros-args -r __node:=cartographer_node --params-file /tmp/launch_params_clup20by'].
最新发布
07-28
<think>我们面对的是三个主要问题:1. SLLidar超时错误(SL_RESULT_OPERATION_TIMEOUT);2. Cartographer参数字典错误;3. 串口设备无法打开。下面逐一解决。 --- ### 一、SLLidar超时错误(SL_RESULT_OPERATION_TIMEOUT) 此错误表明雷达驱动无法与硬件建立通信。请按以下步骤排查: #### 1. **检查串口设备是否存在** ```bash ls /dev/ttyUSB* ``` - 如果无输出,尝试重新插拔雷达USB线,然后执行`dmesg | grep tty`查看内核日志[^2]。 - 若设备名为`/dev/ttyACM0`,则需在launch文件中修改`serial_port`参数。 #### 2. **设置串口权限** ```bash # 查看当前用户是否在dialout组 groups $USER # 若不在,添加并重新登录 sudo usermod -aG dialout $USER && newgrp dialout ``` - 临时解决方案:`sudo chmod 666 /dev/ttyUSB0` #### 3. **创建永久udev规则** ```bash echo 'KERNEL=="ttyUSB*", MODE="0666", GROUP="dialout"' | sudo tee /etc/udev/rules.d/50-sllidar.rules sudo udevadm control --reload sudo udevadm trigger ``` #### 4. **验证串口通信** 使用`minicom`测试雷达: ```bash sudo apt install minicom minicom -D /dev/ttyUSB0 -b 115200 ``` - 正确连接时应看到不断刷新的二进制数据流(非乱码)。 - 若出现乱码,尝试更换波特率(115200/256000/1000000)[^3]。 #### 5. **检查launch文件配置** 确保`sllidar_s2_launch.py`中参数正确: ```python parameters=[{ 'serial_port': '/dev/ttyUSB0', # 与实际设备一致 'serial_baudrate': 115200, # 匹配雷达型号(S2通常为115200) # ...其他参数 }] ``` #### 6. **切换RMW实现** ROS2的默认中间件可能导致串口冲突: ```bash # 临时切换为Fast DDS export RMW_IMPLEMENTATION=rmw_fastrtps_cpp ros2 launch sllidar_ros2 sllidar_s2_launch.py ``` 永久设置:将`export RMW_IMPLEMENTATION=rmw_fastrtps_cpp`加入`~/.bashrc`。 --- ### 二、Cartographer参数字典错误 错误信息通常为`KeyError`或参数未定义,表明配置文件格式错误。 #### 1. **检查YAML格式** - 使用在线工具(如YAML Lint)验证配置文件。 - 确保缩进为空格(非Tab),例如: ```yaml # 正确示例 trajectory_builder_2d: submaps: num_range_data: 35 # 注意缩进 ``` #### 2. **参数命名规范** Cartographer ROS2参数需严格匹配官方模板: - 对照[官方文档](https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html)检查参数名。 - 常见错误:`use_odometry`误写为`use_odom`。 #### 3. **加载参数的正确方式** 在launch文件中使用`Node`的`parameters`选项: ```python cartographer_node = Node( parameters=[ # 主配置文件 PathJoinSubstitution([FindPackageShare('cartographer_ros'), 'configuration_files', 'my_robot.lua']), # 参数覆盖文件(可选) PathJoinSubstitution([FindPackageShare('my_pkg'), 'config', 'custom_params.yaml']) ] ) ``` --- ### 三、串口设备无法打开 此问题通常与权限或设备路径相关,但已在前文雷达部分覆盖。补充要点: #### 1. **多设备竞争** 若同时连接多个串口设备,使用`udev`规则绑定固定端口: ```bash # 查询雷达USB的ID udevadm info -a -n /dev/ttyUSB0 | grep '{idVendor}' # 创建规则(示例:ID 10c4:ea60) echo 'SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="sllidar"' | sudo tee /etc/udev/rules.d/99-sllidar.rules ``` 重启后雷达将固定出现在`/dev/sllidar`。 #### 2. **硬件流控制禁用** 某些串口模块需关闭RTS/CTS: ```python # 在雷达驱动的串口初始化代码中添加 import serial ser = serial.Serial(port, baudrate, rtscts=False) # 关键参数 ``` --- ### 集成解决方案流程图 ```mermaid graph TD A[启动SLAM系统] --> B{雷达超时错误?} B -->|是| C[检查串口权限与设备路径] C --> D[测试minicom通信] D --> E{数据正常?} E -->|否| F[更换波特率/USB线] E -->|是| G[切换RMW实现] G --> H[重新启动] B -->|否| I{Cartographer参数错误?} I -->|是| J[验证YAML格式] J --> K[对照官方参数表] K --> L[修正参数名] L --> H I -->|否| M[进入SLAM正常流程] ``` > **注意**:所有硬件操作后需重新插拔USB并执行`sudo udevadm control --reload`。 §§相关问题§§ 1. 如何为ROS2设备创建稳定的符号链接? 2. Cartographer中如何配置多传感器融合? 3. ROS2串口通信中RTS/CTS流控制的作用是什么?</think>### ROS2 SLAM系统问题综合解决方案 针对SLLidar超时、Cartographer参数错误和串口设备问题,以下是系统化的解决方案: --- ### 一、SLLidar超时错误 (`SL_RESULT_OPERATION_TIMEOUT`) #### 1. **串口权限与配置** ```bash # 检查设备是否存在 ls /dev/ttyUSB* # 添加用户到dialout组 sudo usermod -aG dialout $USER && newgrp dialout # 创建永久udev规则 echo 'KERNEL=="ttyUSB*", MODE="0666", GROUP="dialout"' | sudo tee /etc/udev/rules.d/99-sllidar.rules sudo udevadm control --reload && sudo udevadm trigger ``` #### 2. **启动参数验证** 在`sllidar_s2_launch.py`中确认关键参数: ```python Node( parameters=[{ 'serial_port': '/dev/ttyUSB0', # 需与实际设备一致 'serial_baudrate': 115200, # S2雷达标准波特率 'frame_id': 'laser', 'inverted': False }] ) ``` #### 3. **硬件连接测试** ```bash # 使用minicom直接测试 sudo apt install minicom minicom -D /dev/ttyUSB0 -b 115200 ``` - ✅ 正常情况:持续输出二进制数据 - ❌ 无输出:检查USB线/电源(部分雷达需独立供电) --- ### 二、Cartographer参数字典错误 #### 1. **参数文件格式验证** ```yaml # 正确示例 (cartographer_params.yaml) trajectory_builder_2d { submaps { num_range_data: 35 # 必须使用空格缩进 } use_imu_data: false # 参数名需完全匹配 } ``` 常见错误: - 使用Tab缩进(必须空格) - 参数名拼写错误(如`use_odom` vs `use_odometry`) #### 2. **参数加载调试** 在launch文件中启用调试: ```python cartographer_node = Node( arguments=['--ros-args', '--log-level', 'debug'], output='screen' ) ``` 关键日志分析- `[ERROR] [cartographer]: KeyError 'xxx'` → 参数名错误 - `[WARN] [configuration]: Unused parameter` → 参数类型不匹配 #### 3. **配置继承机制** 使用base配置避免遗漏: ```lua include "map_builder.lua" TRAJECTORY_BUILDER_2D = { use_imu_data = false, -- 覆盖基础配置 submaps = { num_range_data = 35 } } return TRAJECTORY_BUILDER_2D ``` --- ### 三、串口设备无法打开 #### 1. **设备映射绑定** ```bash # 查询设备ID udevadm info -a -n /dev/ttyUSB0 | grep '{idVendor}' # 创建固定符号链接 (示例:ID 10c4:ea60) echo 'SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="sllidar"' | sudo tee /etc/udev/rules.d/98-sllidar.rules ``` 启动文件中使用固定路径: ```python serial_port: '/dev/sllidar' ``` #### 2. **串口冲突解决** ```python # 在驱动代码中添加硬件流控制关闭 import serial ser = serial.Serial(port, baudrate, rtscts=False) # 关键参数 ``` #### 3. **多设备优先级管理** ```bash # 设置设备加载顺序 echo 'SUBSYSTEM=="tty", KERNELS=="1-1.2", SYMLINK+="priority_tty"' >> /etc/udev/rules.d/97-usb-priority.rules ``` --- ### 集成解决方案流程 ```mermaid graph TD A[启动SLAM系统] --> B{雷达超时错误?} B -->|是| C[检查串口权限/设备路径] C --> D[测试minicom通信] D --> E{数据正常?} E -->|否| F[更换波特率/USB线] E -->|是| G[切换RMW实现] G --> H[重新启动] B -->|否| I{Cartographer错误?} I -->|是| J[验证YAML格式] J --> K[对照官方参数表] K --> L[修正参数名] L --> H I -->|否| M[进入SLAM流程] ``` > **关键提示**:每次硬件变更后需执行: > ```bash > source ~/Drone_Slam/install/setup.bash > sudo udevadm control --reload > ```
评论 7
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值