Check failed: time >= imu_tracker->time()

[Robotuks]

Hello!

I am trying to use 2 LiDARs for 2D map, but I get the following error:

darius@darius-GE62VR-6RF:~$ roslaunch cartographer_ros demo_kevin_2d.launch bag_filename:=/media/darius/Data/linux_download/kevin_bags/2018-02-01-15-06-16.bag 
... logging to /home/darius/.ros/log/95ec97aa-0c9f-11e8-8680-84ef18523dd4/roslaunch-darius-GE62VR-6RF-15059.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://darius-GE62VR-6RF:44369/

SUMMARY

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

NODES
  /
    cartographer_node (cartographer_ros/cartographer_node)
    cartographer_occupancy_grid_node (cartographer_ros/cartographer_occupancy_grid_node)
    playbag (rosbag/play)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[robot_state_publisher-1]: started with pid [15076]
process[cartographer_node-2]: started with pid [15077]
process[cartographer_occupancy_grid_node-3]: started with pid [15078]
process[rviz-4]: started with pid [15085]
/opt/ros/kinetic/lib/rviz/rviz: /home/darius/anaconda2/lib/libcurl.so.4: no version information available (required by /opt/ros/kinetic/lib/libresource_retriever.so)
/opt/ros/kinetic/lib/rviz/rviz: /home/darius/anaconda2/lib/libtiff.so.5: no version information available (required by /usr/lib/x86_64-linux-gnu/libfreeimage.so.3)
process[playbag-5]: started with pid [15112]
[ INFO] [1518075806.429946377]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer_ros/configuration_files/kevin_2d.lua' for 'kevin_2d.lua'.
[ INFO] [1518075806.430290938]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1518075806.430335738]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1518075806.430400389]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1518075806.430438062]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1518075806.430618271]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1518075806.430665334]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1518075806.430733604]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1518075806.430778304]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1518075806.430903914]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1518075806.430952803]: I0208 09:43:26.000000 15077 configuration_file_resolver.cc:41] Found '/home/darius/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1518075806.438316598]: I0208 09:43:26.000000 15077 submaps.cc:203] Added submap 1
[ INFO] [1518075806.438395942]: I0208 09:43:26.000000 15077 map_builder_bridge.cc:91] Added trajectory with ID '0'.
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
[ INFO] [1518075806.957445251, 1517490376.960060300]: I0208 09:43:26.000000 15077 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '636530872815397149'.
[ INFO] [1518075806.957575132, 1517490376.960060300]: I0208 09:43:26.000000 15077 local_trajectory_builder.cc:242] Extrapolator not yet initialized.
[ INFO] [1518075806.957638107, 1517490376.960060300]: I0208 09:43:26.000000 15077 local_trajectory_builder.cc:103] Extrapolator is still initializing.
[ INFO] [1518075806.957668797, 1517490376.960060300]: I0208 09:43:26.000000 15077 local_trajectory_builder.cc:103] Extrapolator is still initializing.
F0208 09:43:26.957763 15077 pose_extrapolator.cc:229] Check failed: time >= imu_tracker->time() (636530872815382276 vs. 636530872815444024) 
[FATAL] [1518075806.958029721, 1517490376.960060300]: F0208 09:43:26.000000 15077 pose_extrapolator.cc:229] Check failed: time >= imu_tracker->time() (636530872815382276 vs. 636530872815444024) 
libpng warning: iCCP: known incorrect sRGB profile
*** Check failure stack trace: ***
    @     0x7f9da950f5cd  google::LogMessage::Fail()
    @     0x7f9da9511433  google::LogMessage::SendToLog()
    @     0x7f9da950f15b  google::LogMessage::Flush()
    @     0x7f9da9511e1e  google::LogMessageFatal::~LogMessageFatal()
    @           0x56df1e  cartographer::mapping::PoseExtrapolator::ExtrapolateRotation()
    @           0x56e860  cartographer::mapping::PoseExtrapolator::ExtrapolatePose()
    @           0x5b8a73  cartographer::mapping_2d::LocalTrajectoryBuilder::AddRangeData()
    @           0x577664  cartographer::mapping::GlobalTrajectoryBuilder<>::AddSensorData()
    @           0x636f45  cartographer::sensor::Dispatchable<>::AddToTrajectoryBuilder()
    @           0x6351fa  cartographer::mapping::CollatedTrajectoryBuilder::HandleCollatedSensorData()
    @           0x635c2e  _ZNSt17_Function_handlerIFvRKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteISB_EEEZNS9_7mapping25CollatedTrajectoryBuilderC4EPNSA_17CollatorInterfaceEiRKSt3setINSG_26TrajectoryBuilderInterface8SensorIdESt4lessISM_ESaISM_EES8_ISL_SC_ISL_EEEUlS7_SE_E_E9_M_invokeERKSt9_Any_dataS7_OSE_
    @           0x5d373a  _ZNSt17_Function_handlerIFvSt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteIS3_EEEZNS2_8Collator13AddTrajectoryEiRKSt13unordered_setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4hashISF_ESt8equal_toISF_ESaISF_EERKSt8functionIFvRKSF_S6_EEEUlS6_E_E9_M_invokeERKSt9_Any_dataOS6_
    @           0x5cb494  cartographer::sensor::OrderedMultiQueue::Dispatch()
    @           0x5cc3c4  cartographer::sensor::OrderedMultiQueue::Add()
    @           0x5d3b10  cartographer::sensor::Collator::AddSensorData()
    @           0x63404d  cartographer::mapping::CollatedTrajectoryBuilder::AddData()
    @           0x636e3e  cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData()
    @           0x56412d  cartographer_ros::SensorBridge::HandleRangefinder()
    @           0x5649f5  cartographer_ros::SensorBridge::HandleLaserScan()
    @           0x564bf7  cartographer_ros::SensorBridge::HandleLaserScanMessage()
    @           0x531369  cartographer_ros::Node::HandleLaserScanMessage()
    @           0x54316e  boost::detail::function::void_function_obj_invoker1<>::invoke()
    @           0x54e759  ros::SubscriptionCallbackHelperT<>::call()
    @     0x7f9da6d2044d  ros::SubscriptionQueue::call()
    @     0x7f9da6cc6210  ros::CallbackQueue::callOneCB()
    @     0x7f9da6cc7683  ros::CallbackQueue::callAvailable()
    @     0x7f9da6d24511  ros::SingleThreadedSpinner::spin()
    @     0x7f9da6d094cb  ros::spin()
    @           0x529934  cartographer_ros::(anonymous namespace)::Run()
    @           0x5276d4  main
    @     0x7f9da5762830  __libc_start_main
    @           0x529489  _start
[cartographer_node-2] process has died [pid 15077, exit code -6, cmd /home/darius/cartographer_ws/install_isolated/lib/cartographer_ros/cartographer_node -configuration_directory /home/darius/cartographer_ws/install_isolated/share/cartographer_ros/configuration_files -configuration_basename kevin_2d.lua odom:=KEVIN/caddy/caddy_controller/odom scan_1:=KEVIN/front_laser/front_scan scan_2:=KEVIN/back_laser/back_scan __name:=cartographer_node __log:=/home/darius/.ros/log/95ec97aa-0c9f-11e8-8680-84ef18523dd4/cartographer_node-2.log].
log file: /home/darius/.ros/log/95ec97aa-0c9f-11e8-8680-84ef18523dd4/cartographer_node-2*.log
================================================================================REQUIRED process [rviz-4] has died!
process has finished cleanly
log file: /home/darius/.ros/log/95ec97aa-0c9f-11e8-8680-84ef18523dd4/rviz-4*.log
Initiating shutdown!
================================================================================
[playbag-5] killing on exit
[rviz-4] killing on exit
[cartographer_occupancy_grid_node-3] killing on exit
[robot_state_publisher-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done`

rosbag_validate :

My launch and config files : https://github.com/Robotuks/cartography_kevin Bag file: https://drive.google.com/open?id=1zPD5XZDP4nfwQyessiwYX5HX59vVGwqu

Then I try to run this bag with one LiDAR, everything works fine, doesn't matter which one I choose. Any ideas why it doesn't work? Might it be because data from LiDARs comes not at the same time? (Difference ~50 ms)

[ojura]

Since you're splitting each laser measurement into 10 subdivisions, you should set num_accumulated_range data to 20, since you have two lasers.

[gaschler]

Could you please modify cartographer_ros SensorBridge::HandleLaserScan and replace

    for (auto& point : subdivision) {
      point[3] -= time_to_subdivision_end;
    }

by

    for (auto& point : subdivision) {
      point[3] = 0;
    }

If this fixes it, this may be interesting.

(In the future, please post the output of rosbag_validate as text rather than as an image.) In your bag file, serialization and sensor times do not match. Cartographer just looks at the sensor times, but other subscribers such as rviz may not work.

[Robotuks]

That fixed the problem, now it works. Thanks for your help.

[kajep09]

Could you be a little bit more specific in what fixed the problem? Correctly tuning the ratio of subdivisions and accumulated range data or trying the code modification that gaschler suggested?

[kajep09]

@gaschler I have been having the same problem as @Robotuks described. I have two laser scanners (connected by ethernet to the same computer) and if I configure cartographer to use 2 laser scanners I get the following error:

 Check failed: time >= imu_tracker->time() (636542891459665714 vs. 636542891459685582) 
[FATAL] [1518692345.992819099] [ros.cartographer_ros]: F0215 11:59:05.000000 11344 pose_extrapolator.cc:229] Check failed: time >= imu_tracker->time() (636542891459665714 vs. 636542891459685582) 

After changing SensorBridge::HandleLaserScan like you described it is possible for me to launch cartographer with 2 laser scanners and create a map.

Before running into this issue and your reply, I had been forced to use a laser scanner merging node to merge the two separate laser scans into one and assigning that a new (current ros time) timestamp.

I hope this information can help you track down the problem/bug (if it indeed is a bug).

[gaschler]

Thanks for reproducing and confirming. I believe this is a bug.

[Robotuks]

@kajep09 Sorry for being unclear. Code modification suggested by @gaschler fixed the problem. Ratio of subdivisions and accumulated range data only improved performance of cartographer