search

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.16k stars 2.25k forks source link

Empty range_data causes fatal error #823

Closed ryanmeasel closed 6 years ago

ryanmeasel commented 6 years ago

I collected a ROS bag for a unit with 2 VLPs and an IMU using cartographer_ros and cartographer was able to run for the full duration of the scan. When the bag is processed however, a fatal error occurs:

[FATAL] [1516059899.205516676, 1515519557.762312447]: F0115 15:44:59.000000 12672 local_trajectory_builder.cc:71] Check failed: !range_data.returns.empty()

The error is correct as the bag does contain a LiDAR packet with an empty data payload near that timestamp. Does this need to be fatal? It seems like the packet could simply be discarded. Also, why would cartographer continue to run without error during collection but not during playback?

I have enclosed the full run output as well as the bag validation:

$ roslaunch cartographer_ros offline_backpack_2vlp.launch bag_filenames:=/home/ryan/Downloads/cartographer_2018-01-09-09-38-35.bag 
... logging to /home/ryan/.ros/log/0570f20a-fa4a-11e7-9e23-408d5c0720fc/roslaunch-snorlax-12654.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://snorlax:46769/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.12
 * /use_sim_time: True

NODES
  /
    cartographer_occupancy_grid_node (cartographer_ros/cartographer_occupancy_grid_node)
    cartographer_offline_node (cartographer_ros/cartographer_offline_node)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[rviz-1]: started with pid [12671]
process[cartographer_offline_node-2]: started with pid [12672]
process[cartographer_occupancy_grid_node-3]: started with pid [12673]
[ INFO] [1516059876.933323027]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer_ros/configuration_files/mapping_backpack_2vlp.lua' for 'mapping_backpack_2vlp.lua'.
[ INFO] [1516059876.933592198]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1516059876.933623122]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1516059876.933673764]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1516059876.933699973]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1516059876.933818685]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1516059876.933847147]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1516059876.933889700]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1516059876.933909724]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1516059876.933986135]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1516059876.934009268]: I0115 15:44:36.000000 12672 configuration_file_resolver.cc:41] Found '/home/ryan/catkin_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1516059876.941930782]: I0115 15:44:36.000000 12672 submaps.cc:295] Added submap 1
[ INFO] [1516059876.942006980]: I0115 15:44:36.000000 12672 map_builder_bridge.cc:90] Added trajectory with ID '0'.
[ INFO] [1516059883.537822137, 1515519515.758172890]: I0115 15:44:43.000000 12672 offline_node.cc:183] Processed 0 of 696.342 bag time seconds...
[ INFO] [1516059883.538446351, 1515519515.766045467]: I0115 15:44:43.000000 12672 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '636511163157662360'.
[ INFO] [1516059883.538633079, 1515519515.768428106]: I0115 15:44:43.000000 12672 local_trajectory_builder.cc:67] IMU not yet initialized.
[ INFO] [1516059883.538751940, 1515519515.768444131]: I0115 15:44:43.000000 12672 local_trajectory_builder.cc:67] IMU not yet initialized.
[ INFO] [1516059891.949071815, 1515519542.140733122]: I0115 15:44:51.000000 12672 collated_trajectory_builder.cc:69] /horiz_lidar_points rate: 1492.86 Hz 6.70e-04 s +/- 5.21e-04 s (pulsed at 313.58% real time)
[ INFO] [1516059891.949156152, 1515519542.140733122]: I0115 15:44:51.000000 12672 collated_trajectory_builder.cc:69] /imu/data rate: 99.98 Hz 1.00e-02 s +/- 1.29e-03 s (pulsed at 313.98% real time)
[ INFO] [1516059891.949574730, 1515519542.140733122]: I0115 15:44:51.000000 12672 collated_trajectory_builder.cc:69] /vert_lidar_points rate: 1492.13 Hz 6.70e-04 s +/- 5.66e-04 s (pulsed at 314.03% real time)
[ INFO] [1516059892.140332175, 1515519542.620934904]: I0115 15:44:52.000000 12672 motion_filter.cc:42] Motion filter reduced the number of nodes to 15.6%.
[ INFO] [1516059893.972021512, 1515519547.017301169]: I0115 15:44:53.000000 12672 submaps.cc:295] Added submap 2
[ INFO] [1516059894.511402298, 1515519548.165072409]: I0115 15:44:54.000000 12672 offline_node.cc:183] Processed 32.4069 of 696.342 bag time seconds...
[ INFO] [1516059899.198204031, 1515519557.732330222]: I0115 15:44:59.000000 12672 submaps.cc:295] Added submap 3
F0115 15:44:59.205286 12672 local_trajectory_builder.cc:71] Check failed: !range_data.returns.empty() 
[FATAL] [1516059899.205516676, 1515519557.762312447]: F0115 15:44:59.000000 12672 local_trajectory_builder.cc:71] Check failed: !range_data.returns.empty() 
*** Check failure stack trace: ***
    @     0x7f6311ad35cd  google::LogMessage::Fail()
    @     0x7f6311ad5433  google::LogMessage::SendToLog()
    @     0x7f6311ad315b  google::LogMessage::Flush()
    @     0x7f6311ad5e1e  google::LogMessageFatal::~LogMessageFatal()
    @           0x6438cb  cartographer::mapping_3d::LocalTrajectoryBuilder::AddRangeData()
    @           0x5df408  cartographer::mapping::GlobalTrajectoryBuilder<>::AddSensorData()
    @           0x6d4595  cartographer::sensor::Dispatchable<>::AddToTrajectoryBuilder()
    @           0x6d2aca  cartographer::mapping::CollatedTrajectoryBuilder::HandleCollatedSensorData()
    @           0x6d34fe  _ZNSt17_Function_handlerIFvRKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteISB_EEEZNS9_7mapping25CollatedTrajectoryBuilderC4EPNSA_8CollatorEiRKSt13unordered_setIS5_St4hashIS5_ESt8equal_toIS5_ESaIS5_EES8_INSG_26TrajectoryBuilderInterfaceESC_IST_EEEUlS7_SE_E_E9_M_invokeERKSt9_Any_dataS7_OSE_
    @           0x6bda3a  _ZNSt17_Function_handlerIFvSt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteIS3_EEEZNS2_8Collator13AddTrajectoryEiRKSt13unordered_setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4hashISF_ESt8equal_toISF_ESaISF_EERKSt8functionIFvRKSF_S6_EEEUlS6_E_E9_M_invokeERKSt9_Any_dataOS6_
    @           0x6bfa14  cartographer::sensor::OrderedMultiQueue::Dispatch()
    @           0x6c0944  cartographer::sensor::OrderedMultiQueue::Add()
    @           0x6bdd3f  cartographer::sensor::Collator::AddSensorData()
    @           0x6d1c72  cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData()
    @           0x6d3bc5  cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData()
    @           0x5b6d83  cartographer_ros::SensorBridge::HandleImuMessage()
    @           0x598d47  cartographer_ros::Node::HandleImuMessage()
    @           0x57e058  cartographer_ros::RunOfflineNode()
    @           0x577cc7  main
    @     0x7f630d680830  __libc_start_main
    @           0x57ab69  _start
    @              (nil)  (unknown)
[cartographer_offline_node-2] process has died [pid 12672, exit code -6, cmd /home/ryan/catkin_ws/install_isolated/lib/cartographer_ros/cartographer_offline_node -configuration_directory /home/ryan/catkin_ws/install_isolated/share/cartographer_ros/configuration_files -configuration_basename mapping_backpack_2vlp.lua -urdf_filename /home/ryan/catkin_ws/install_isolated/share/cartographer_ros/urdf/backpack_2vlp.urdf -bag_filenames /home/ryan/Downloads/cartographer_2018-01-09-09-38-35.bag imu:=/imu/data points2_1:=horiz_lidar_points points2_2:=vert_lidar_points __name:=cartographer_offline_node __log:=/home/ryan/.ros/log/0570f20a-fa4a-11e7-9e23-408d5c0720fc/cartographer_offline_node-2.log].
log file: /home/ryan/.ros/log/0570f20a-fa4a-11e7-9e23-408d5c0720fc/cartographer_offline_node-2*.log
================================================================================REQUIRED process [rviz-1] has died!
process has finished cleanly
log file: /home/ryan/.ros/log/0570f20a-fa4a-11e7-9e23-408d5c0720fc/rviz-1*.log
Initiating shutdown!
================================================================================
[cartographer_occupancy_grid_node-3] killing on exit
[rviz-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Bag validation:

$ ~/catkin_ws/devel_isolated/cartographer_ros/lib/cartographer_ros/cartographer_rosbag_validate -bag_filename=/home/ryan/Downloads/cartographer_2018-01-09-09-38-35.bag 
W0115 17:02:18.558490 15326 rosbag_validate_main.cc:288] frame_id "vertical_vlp16_link" on topic /vert_lidar_points has serialization time 1515519533.561962325 but sensor time 1515519533.438705000 differing by -0.123257 s.
W0115 17:02:19.435412 15326 rosbag_validate_main.cc:103] frame_id imu_link time 1515519546587567962: IMU linear acceleration is 2.23423 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 0.0473449 002.21687 -0.273885
W0115 17:02:19.473037 15326 rosbag_validate_main.cc:103] frame_id imu_link time 1515519547117451051: IMU linear acceleration is 1.76797 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 00.829108 0-1.45733 -0.560778
W0115 17:02:19.702103 15326 rosbag_validate_main.cc:103] frame_id imu_link time 1515519550425856436: IMU linear acceleration is 1.46097 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration -0.657987 -0.736386 001.07667
E0115 17:02:58.492913 15326 rosbag_validate_main.cc:286] frame_id "imu_link" on topic /imu/data has serialization time 1515520085.428931493 but sensor time 1515520084.618938451 differing by -0.809993 s.
E0115 17:02:58.505409 15326 rosbag_validate_main.cc:286] frame_id "imu_link" on topic /imu/data has serialization time 1515520085.430441436 but sensor time 1515520084.628844729 differing by -0.801597 s.
E0115 17:02:58.505434 15326 rosbag_validate_main.cc:286] frame_id "imu_link" on topic /imu/data has serialization time 1515520085.430446097 but sensor time 1515520084.638740276 differing by -0.791706 s.
E0115 17:03:07.583269 15326 rosbag_validate_main.cc:316] Point data (frame_id: "horizontal_vlp16_link") has a large gap, largest is 0.924466 s, recommended is [0.0005, 0.05] s with no jitter.
I0115 17:03:07.594286 15326 rosbag_validate_main.cc:332] Time delta histogram for consecutive messages on topic "/horiz_lidar_points" (frame_id: "horizontal_vlp16_link"):
Count: 1030597  Min: 0.000001  Max: 0.924466  Mean: 0.000674
[0.000001, 0.092448)    ####################    Count: 1030564 (99.996796%) Total: 1030564 (99.996796%)
[0.092448, 0.184894)                            Count: 15 (0.001455%)   Total: 1030579 (99.998260%)
[0.184894, 0.277340)                            Count: 5 (0.000485%)    Total: 1030584 (99.998741%)
[0.277340, 0.369787)                            Count: 6 (0.000582%)    Total: 1030590 (99.999321%)
[0.369787, 0.462234)                            Count: 1 (0.000097%)    Total: 1030591 (99.999420%)
[0.462234, 0.554680)                            Count: 1 (0.000097%)    Total: 1030592 (99.999512%)
[0.554680, 0.647126)                            Count: 3 (0.000291%)    Total: 1030595 (99.999809%)
[0.647126, 0.739573)                            Count: 1 (0.000097%)    Total: 1030596 (99.999901%)
[0.739573, 0.832020)                            Count: 0 (0.000000%)    Total: 1030596 (99.999901%)
[0.832020, 0.924466]                            Count: 1 (0.000097%)    Total: 1030597 (99.999992%)
E0115 17:03:07.594502 15326 rosbag_validate_main.cc:323] IMU data (frame_id: "imu_link") has a large gap, largest is 0.421201 s, recommended is [0.0005, 0.005] s with no jitter.
I0115 17:03:07.595463 15326 rosbag_validate_main.cc:332] Time delta histogram for consecutive messages on topic "/imu/data" (frame_id: "imu_link"):
Count: 69416  Min: 0.000024  Max: 0.421201  Mean: 0.010029
[0.000024, 0.042141)    ####################    Count: 69402 (99.979836%)   Total: 69402 (99.979836%)
[0.042141, 0.084259)                            Count: 9 (0.012965%)    Total: 69411 (99.992798%)
[0.084259, 0.126377)                            Count: 0 (0.000000%)    Total: 69411 (99.992798%)
[0.126377, 0.168495)                            Count: 1 (0.001441%)    Total: 69412 (99.994240%)
[0.168495, 0.210612)                            Count: 0 (0.000000%)    Total: 69412 (99.994240%)
[0.210612, 0.252730)                            Count: 0 (0.000000%)    Total: 69412 (99.994240%)
[0.252730, 0.294848)                            Count: 0 (0.000000%)    Total: 69412 (99.994240%)
[0.294848, 0.336966)                            Count: 1 (0.001441%)    Total: 69413 (99.995682%)
[0.336966, 0.379083)                            Count: 1 (0.001441%)    Total: 69414 (99.997116%)
[0.379083, 0.421201]                            Count: 2 (0.002881%)    Total: 69416 (100.000000%)
E0115 17:03:07.598158 15326 rosbag_validate_main.cc:316] Point data (frame_id: "vertical_vlp16_link") has a large gap, largest is 0.925124 s, recommended is [0.0005, 0.05] s with no jitter.
I0115 17:03:07.609241 15326 rosbag_validate_main.cc:332] Time delta histogram for consecutive messages on topic "/vert_lidar_points" (frame_id: "vertical_vlp16_link"):
Count: 1030630  Min: 0.000001  Max: 0.925124  Mean: 0.000674
[0.000001, 0.092513)    ####################    Count: 1030598 (99.996895%) Total: 1030598 (99.996895%)
[0.092513, 0.185026)                            Count: 12 (0.001164%)   Total: 1030610 (99.998062%)
[0.185026, 0.277538)                            Count: 9 (0.000873%)    Total: 1030619 (99.998940%)
[0.277538, 0.370050)                            Count: 5 (0.000485%)    Total: 1030624 (99.999420%)
[0.370050, 0.462563)                            Count: 2 (0.000194%)    Total: 1030626 (99.999611%)
[0.462563, 0.555075)                            Count: 2 (0.000194%)    Total: 1030628 (99.999809%)
[0.555075, 0.647587)                            Count: 0 (0.000000%)    Total: 1030628 (99.999809%)
[0.647587, 0.740099)                            Count: 1 (0.000097%)    Total: 1030629 (99.999901%)
[0.740099, 0.832612)                            Count: 0 (0.000000%)    Total: 1030629 (99.999901%)
[0.832612, 0.925124]                            Count: 1 (0.000097%)    Total: 1030630 (100.000000%)
gaschler commented 6 years ago

W0115 17:02:19.435412 15326 rosbag_validate_main.cc:103] frame_id imu_link time 1515519546587567962: IMU linear acceleration is 2.23423 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 0.0473449 002.21687 -0.273885

Please configure your IMU that it includes gravity. It won't work otherwise. Actually, we only look at gravity direction and drop the rest of the acceleration vector.

[FATAL] [1516059899.205516676, 1515519557.762312447]: F0115 15:44:59.000000 12672 local_trajectory_builder.cc:71] Check failed: !range_data.returns.empty()

Yeah, this is a rather strict check. You could replace it by a return nullptr in your fork or make sure your range data message are non-empty.

cschuet commented 6 years ago

@ryanmeasel You could also try to leave the code untouched and simply filter out the empty point cloud ROS messages using a carefully crafted invocation of [rosbag filter(http://wiki.ros.org/rosbag/Commandline#filter) I guess :)