cartographer-project / cartographer

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.19k stars 2.26k forks source link

fast_correlative_scan_matcher_2d.cc:68] Check failed: non_ascending_maxima_.size() == 0 #1396

Open narutojxl opened 6 years ago

narutojxl commented 6 years ago

Hello, i use cartographer 1.0, cartographer_ros 1.0, and ceres 1.14.0, laser is sick Tim571, when i running cartographer_ros demo_backpack_2d.launch, it always throw the same error when the robot bringup and teleop for a moment , here is terminal log and cfg file.

`hans@robo00:~$ roslaunch cartographer_ros demo_backpack_2d_jxl.launch ... logging to /home/xlh/.ros/log/3a8399b8-a466-11e8-ad96-00045f9a45a3/roslaunch-robo00-8919.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt WARNING: disk usage in log directory [/home/xlh/.ros/log] is over 1GB. It's recommended that you use the 'rosclean' command.

WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. started roslaunch server http://192.168.0.3:51039/

SUMMARY

PARAMETERS

NODES / base_controller (base_controller/base_controller_node) cartographer_node (cartographer_ros/cartographer_node) cartographer_occupancy_grid_node (cartographer_ros/cartographer_occupancy_grid_node) diagnostic_aggregator (diagnostic_aggregator/aggregator_node) laser_to_sonar1 (tf/static_transform_publisher) laser_to_sonar2 (tf/static_transform_publisher) laser_to_sonar3 (tf/static_transform_publisher) mobile_base (nodelet/nodelet) mobile_base_nodelet_manager (nodelet/nodelet) odom_output_tim (slam/odom_output_tim) rviz (rviz/rviz) sick_tim551_2050001 (sick_tim/sick_tim551_2050001) sonar2PointCloud (sonar2PointCloud/sonar2PointCloud_node)

auto-starting new master process[master]: started with pid [8931] ROS_MASTER_URI=http://localhost:11311

setting /run_id to 3a8399b8-a466-11e8-ad96-00045f9a45a3 WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[rosout-1]: started with pid [8944] started core service [/rosout] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[mobile_base_nodelet_manager-2]: started with pid [8961] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[mobile_base-3]: started with pid [8981] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[diagnostic_aggregator-4]: started with pid [9057] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[base_controller-5]: started with pid [9150] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[odom_output_tim-6]: started with pid [9212] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[sick_tim551_2050001-7]: started with pid [9237] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[laser_to_sonar1-8]: started with pid [9284] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[laser_to_sonar2-9]: started with pid [9295] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[laser_to_sonar3-10]: started with pid [9306] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[sonar2PointCloud-11]: started with pid [9317] process[cartographer_node-12]: started with pid [9377] [ INFO] [1534761963.690817928]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer_ros/configuration_files/backpack_2d.lua' for 'backpack_2d.lua'. [ INFO] [1534761963.691257266]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'. [ INFO] [1534761963.691353470]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'. [ INFO] [1534761963.691473374]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'. [ INFO] [1534761963.691556027]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'. [ INFO] [1534761963.691794007]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'. [ INFO] [1534761963.691876981]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'. [ INFO] [1534761963.691990039]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'. [ INFO] [1534761963.692068258]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'. [ INFO] [1534761963.692253200]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'. [ INFO] [1534761963.692331554]: I0820 10:46:03.000000 9377 configuration_file_resolver.cc:41] Found '/home/xlh/carto_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'. [ INFO] [1534761963.703406127]: I0820 10:46:03.000000 9377 submap_2d.cc:187] Added submap 1 [ INFO] [1534761963.703538057]: I0820 10:46:03.000000 9377 map_builder_bridge.cc:130] Added trajectory with ID '0'. process[cartographer_occupancy_grid_node-13]: started with pid [9421] WARNING: Package name "sonar2PointCloud" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores. process[rviz-14]: started with pid [9439] [ INFO] [1534761964.093777614]: I0820 10:46:04.000000 9377 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '636703587639779658'. [ INFO] [1534761964.094069308]: I0820 10:46:04.000000 9377 local_trajectory_builder_2d.cc:138] Extrapolator is still initializing. [ INFO] [1534761978.792286521]: I0820 10:46:18.000000 9377 collated_trajectory_builder.cc:72] scan rate: 150.07 Hz 6.66e-03 s +/- 6.99e-03 s (pulsed at 99.42% real time) [ INFO] [1534761993.796587716]: I0820 10:46:33.000000 9377 collated_trajectory_builder.cc:72] scan rate: 150.12 Hz 6.66e-03 s +/- 6.99e-03 s (pulsed at 100.37% real time) [ INFO] [1534762008.799085319]: I0820 10:46:48.000000 9377 collated_trajectory_builder.cc:72] scan rate: 150.12 Hz 6.66e-03 s +/- 6.99e-03 s (pulsed at 100.36% real time) [ INFO] [1534762014.197834605]: I0820 10:46:54.000000 9377 motion_filter.cc:42] Motion filter reduced the number of nodes to 2%. [ INFO] [1534762023.891620280]: I0820 10:47:03.000000 9377 collated_trajectory_builder.cc:72] scan rate: 150.01 Hz 6.67e-03 s +/- 6.84e-03 s (pulsed at 100.00% real time) [ INFO] [1534762038.891824676]: I0820 10:47:18.000000 9377 collated_trajectory_builder.cc:72] scan rate: 149.95 Hz 6.67e-03 s +/- 6.61e-03 s (pulsed at 99.99% real time) [ INFO] [1534762053.892321378]: I0820 10:47:33.000000 9377 collated_trajectory_builder.cc:72] scan rate: 149.84 Hz 6.67e-03 s +/- 6.75e-03 s (pulsed at 99.79% real time) [ INFO] [1534762063.095931249]: I0820 10:47:43.000000 9377 submap_2d.cc:187] Added submap 2 [ INFO] [1534762063.298007341]: I0820 10:47:43.000000 9403 constraint_builder_2d.cc:281] 0 computations resulted in 0 additional constraints. [ INFO] [1534762063.298205472]: I0820 10:47:43.000000 9403 constraint_builder_2d.cc:283] Score histogram: Count: 0 [ INFO] [1534762064.194739357]: I0820 10:47:44.000000 9377 motion_filter.cc:42] Motion filter reduced the number of nodes to 9.5%. [ INFO] [1534762068.919330829]: I0820 10:47:48.000000 9377 collated_trajectory_builder.cc:72] scan rate: 149.86 Hz 6.67e-03 s +/- 8.31e-03 s (pulsed at 100.45% real time) [ INFO] [1534762083.992749864]: I0820 10:48:03.000000 9377 collated_trajectory_builder.cc:72] scan rate: 150.13 Hz 6.66e-03 s +/- 6.71e-03 s (pulsed at 100.00% real time) [ INFO] [1534762092.709681707]: I0820 10:48:12.000000 9377 submap_2d.cc:187] Added submap 3 F0820 10:48:12.898792 9401 fast_correlative_scan_matcher_2d.cc:68] Check failed: non_ascendingmaxima.size() == 0 (214 vs. 0) [FATAL] [1534762092.899261155]: F0820 10:48:12.000000 9401 fast_correlative_scan_matcher_2d.cc:68] Check failed: non_ascendingmaxima.size() == 0 (214 vs. 0) Check failure stack trace: @ 0xb76e5efc (unknown) @ 0xb76e5e13 (unknown) @ 0xb76e585f (unknown) @ 0xb76e88b0 (unknown) @ 0x8307b93 cartographer::mapping::scan_matching::(anonymous namespace)::SlidingWindowMaximum::CheckIsEmpty() @ 0x83085c4 cartographer::mapping::scan_matching::PrecomputationGrid2D::PrecomputationGrid2D() @ 0x8308f1b cartographer::mapping::scan_matching::PrecomputationGridStack2D::PrecomputationGridStack2D() @ 0x830a3aa cartographer::mapping::scan_matching::FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D() @ 0x82fddd5 _ZNSt17_Function_handlerIFvvEZN12cartographer7mapping11constraints19ConstraintBuilder2D31DispatchScanMatcherConstructionERKNS2_8SubmapIdEPKNS2_6Grid2DEEUlvE_E9_M_invokeERKSt9_Any_data @ 0x8295037 cartographer::common::Task::Execute() @ 0x828d3eb cartographer::common::ThreadPoolInterface::Execute() @ 0x828e4c9 cartographer::common::ThreadPool::DoWork() @ 0x828e7ae _ZNSt6thread5_ImplISt12_Bind_simpleIFZN12cartographer6common10ThreadPoolC1EiEUlvE_vEEE6_M_runEv @ 0xb6b4bd1e (unknown) @ 0xb76a2f72 start_thread @ 0xb698243e (unknown) [cartographer_node-12] process has died [pid 9377, exit code -6, cmd /home/xlh/carto_ws/install_isolated/lib/cartographer_ros/cartographer_node -configuration_directory /home/xlh/carto_ws/install_isolated/share/cartographer_ros/configuration_files -configuration_basename backpack_2d.lua __name:=cartographer_node __log:=/home/xlh/.ros/log/3a8399b8-a466-11e8-ad96-00045f9a45a3/cartographer_node-12.log]. log file: /home/xlh/.ros/log/3a8399b8-a466-11e8-ad96-00045f9a45a3/cartographer_node-12*.log ^C[rviz-14] killing on exit [cartographer_occupancy_grid_node-13] killing on exit [sonar2PointCloud-11] killing on exit [laser_to_sonar3-10] killing on exit [laser_to_sonar1-8] killing on exit [laser_to_sonar2-9] killing on exit [odom_output_tim-6] killing on exit [sick_tim551_2050001-7] killing on exit [base_controller-5] killing on exit [diagnostic_aggregator-4] killing on exit [mobile_base-3] killing on exit [mobile_base_nodelet_manager-2] killing on exit [rosout-1] killing on exit [master] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done hans@robo00:~$ `

- and here is the backup_2d.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 = "odom", odom_frame = "odom", provide_odom_frame = false,

publish_frame_projected_to_2d = false, 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 = 10,

num_point_clouds = 0,

lookup_transform_timeout_sec = 0.2, ---0.2 submap_publish_period_sec = 0.3, -- 0.3

pose_publish_period_sec = 50e-3, --5e-3 ---: map--->odom TF

trajectory_publish_period_sec = 100e-3, --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_2d = true TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 15 --10 TRAJECTORY_BUILDER_2D.use_imu_data = false

return options`

@kdaun, here is tha bag file link, bag file link , and the rosbag file info is as follows [bag_info.txt] . (https://github.com/googlecartographer/cartographer/files/2301854/bag_info.txt) When i paste the info directly on here, the format is messy,sorry for that. If you need any information , please let me know , thank you very much!

kdaun commented 6 years ago

Thank you for the detailed overview. Can you please record a bag file and provide the output of rosbag_validate and rosbag info of your bag file.

kdaun commented 6 years ago

Thank you for providing the output of rosbag info in such a convenient way.

Please also run rosbag_validate, e.g.: rosrun cartographer_ros cartographer_rosbag_validate -bag_filename ${HOME}/example_dir/example.bag And post the text output here.

kdaun commented 6 years ago

Thank you for providing the output of rosbag_validate. Let's look at the output.

E0823 09:44:06.304395 13436 rosbag_validate_main.cc:131] Input contains transform message from frame_id "map" to child_frame_id "odom". This is almost always output published by cartographer and should not appear as input. (Unless you have some complex remove_frames

It looks, like the bag file is recorded with Cartographer running. Can you please record a bag without Cartographer running and run rosbag_validate again?

W0823 09:44:06.304738 13436 rosbag_validate_main.cc:103] frame_id gyro_link time 1534761994760554313: IMU linear acceleration is 0 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 0 0 0

E0823 09:44:06.304785 13436 rosbag_validate_main.cc:315] Sensor with frame_id "gyro_link" jumps backwards in time. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor.

E0823 09:44:06.506513 13436 rosbag_validate_main.cc:365] Average IMU linear acceleration is 0 m/s^2, expected is [9.5, 10.5] m/s^2. Linear acceleration data should include gravity and be given in m/s^2.

These messages indicate that your IMU does not expose the acceleration and the sensor time stamps jump back in time. As long as we are not using the IMU this should not cause any issues.

E0823 09:44:06.327728 13436 rosbag_validate_main.cc:160] Sensor with frame_id "laser" is not sequential in time.Previous range message ends at time 636703588086866674, current one at time 636703588086851791

These messages indicate that range observations are not sequential in time. Cartographer requires sequential range observations. Please ensure the time stamping is correct.

In order to further evaluate the issue:

PS: Please answer to this issues instead of editing the initial issue, then the issues is more comprehensible for other people.

narutojxl commented 6 years ago

Sorry for that. This is the bag rosbag_validate info and rosbag info without Cartographer running: rosbag_validate info_and_rosbag_info _without_Cartographer.txt Scan data is captured directly with ros sick sensor driver package.

kdaun commented 6 years ago

The output for this rosbag looks ok with respect to the /scan messages. Do you observe the Check failed: non_ascending_maxima_.size() == 0 error when you play the bag file and run Cartographer?

narutojxl commented 6 years ago

No, when i played the bag file and running cartographer on my computer, not on robot system, i didn't see the error Check failed: non_ascending_maxima_.size() == 0 in the carto running terminal.

Sbergdahl commented 6 years ago

Is there any solution to this yet? Were encountering the same error. It seems to be tirggered especially by sudden rotations although It will trigger sooner or later anyway unless the robot is standing still.