Cartographer_ros: Robot not moving in 3D SLAM with IMU, timing issues

[dirtshell]

I am attempting to perform 3D SLAM using an IMU and a Quanergy lidar. 2D SLAM works great, but when I attempt to perform 3D SLAM everything falls apart, and I believe it has to do with my IMU.

Bag file with my sensor data: https://drive.google.com/open?id=1Ae5PBmEnmEEX0T2CkuPqLYhjDD_S4aYB
Launch file for offline run: https://gist.github.com/dirtshell/98a2907c1ec0d8a69882d45ef15f3158
Config file for offline run: https://gist.github.com/dirtshell/a5494138b66e9b817747d0c055ba65f9
URDF file: https://gist.github.com/dirtshell/37f538ad37049ecaa6ba83ca68af9014

When I run rosbag_validate, I get an error stating W0131 15:39:50.854450 28056 rosbag_validate_main.cc:288] frame_id "0" on topic /an_device/Imu has serialization time 1517427880.233376522 but sensor time 1517427879.850005000 differing by -0.383372 s. and then a seg fault. I am unable to really understand what this error means though.

Interestingly enough, when I try to run the sim by tweaking the config file and setting the imu_sampling_ratio to a value like 1.0, it breaks cartographer and I get the following error: [FATAL] [1517431711.033191933, 1517427880.733185482]: F0131 15:48:31.000000 30106 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (636530246803500050 vs. 636530246803500050)

I think these two issues may be related. Again, when I run 2D (without IMU data) everything looks great. If I run 2D with IMU data it looks pretty bad, but thats just a tuning / GPS correction issue. But when I run 3D SLAM with IMU data, the robot does not move and a single giant point cloud is generated by overlapping submaps, until the cloud clears and is replaced by the current submap.

What I think may be the source of this timing issue is that the Quanergy lidar allows you to send a PPS signal to synchronize the IMU's timestamp with the lidar's timestamp. This would explain why the times are the same, causing the check to fail. It could also explain why the serialization time and the sensor time also differ. Is this setup abnormal, or should it be supported by cartographer in the future? Or am I simply doing something wrong?

Thanks for your help,
Jacob Kenin

[gaschler]

Can you please re-compile the latest version of rosbag_validate and pose the rest of the output? (There was a bug in it that was fixed last week.)

[dirtshell]

cartographer_rosbag_validate -bag_filename /home/jake/Downloads/2018-01-31-14-44-39.bag 
W0201 13:07:28.768036 20354 rosbag_validate_main.cc:289] frame_id "0" on topic /an_device/Imu has serialization time 1517427880.233376522 but sensor time 1517427879.850005000 differing by -0.383372 s.
E0201 13:07:29.152909 20354 rosbag_validate_main.cc:324] IMU data (frame_id: "0") has a large gap, largest is 0.05026 s, recommended is [0.0005, 0.005] s with no jitter.
I0201 13:07:29.153095 20354 rosbag_validate_main.cc:333] Time delta histogram for consecutive messages on topic "/an_device/Imu" (frame_id: "0"):
Count: 2147  Min: 0.000000  Max: 0.050260  Mean: 0.012483
[0.000000, 0.005026)         ###############    Count: 1611 (75.034935%)    Total: 1611 (75.034935%)
[0.005026, 0.010052)                            Count: 0 (0.000000%)    Total: 1611 (75.034935%)
[0.010052, 0.015078)                            Count: 0 (0.000000%)    Total: 1611 (75.034935%)
[0.015078, 0.020104)                            Count: 0 (0.000000%)    Total: 1611 (75.034935%)
[0.020104, 0.025130)                            Count: 0 (0.000000%)    Total: 1611 (75.034935%)
[0.025130, 0.030156)                            Count: 0 (0.000000%)    Total: 1611 (75.034935%)
[0.030156, 0.035182)                            Count: 0 (0.000000%)    Total: 1611 (75.034935%)
[0.035182, 0.040208)                            Count: 0 (0.000000%)    Total: 1611 (75.034935%)
[0.040208, 0.045234)                            Count: 0 (0.000000%)    Total: 1611 (75.034935%)
[0.045234, 0.050260]                   #####    Count: 536 (24.965067%) Total: 2147 (100.000000%)
E0201 13:07:29.153122 20354 rosbag_validate_main.cc:317] Point data (frame_id: "Sensor") has a large gap, largest is 0.105787 s, recommended is [0.0005, 0.05] s with no jitter.
I0201 13:07:29.153159 20354 rosbag_validate_main.cc:333] Time delta histogram for consecutive messages on topic "/Sensor/points" (frame_id: "Sensor"):
Count: 269  Min: 0.092672  Max: 0.105787  Mean: 0.099331
[0.092672, 0.093984)                       #    Count: 18 (6.691450%)   Total: 18 (6.691450%)
[0.093984, 0.095295)                       #    Count: 17 (6.319703%)   Total: 35 (13.011152%)
[0.095295, 0.096607)                      ##    Count: 22 (8.178438%)   Total: 57 (21.189590%)
[0.096607, 0.097918)                       #    Count: 16 (5.947956%)   Total: 73 (27.137547%)
[0.097918, 0.099230)                    ####    Count: 48 (17.843866%)  Total: 121 (44.981411%)
[0.099230, 0.100541)                   #####    Count: 71 (26.394053%)  Total: 192 (71.375465%)
[0.100541, 0.101853)                       #    Count: 17 (6.319703%)   Total: 209 (77.695168%)
[0.101853, 0.103164)                      ##    Count: 25 (9.293680%)   Total: 234 (86.988846%)
[0.103164, 0.104476)                       #    Count: 15 (5.576208%)   Total: 249 (92.565056%)
[0.104476, 0.105787]                       #    Count: 20 (7.434944%)   Total: 269 (100.000000%)

The first error it talks about mentions the IMU messages have some gaps in it. This is probably because I have to set imu_sampling_ratio to 0.2 in order to not get the Check failed: data.time... error.

The second part mentions that the lidar is not sending messages frequently enough. After consulting the docs for the Quanergy M8 lidar, it appears that the gap of 0.105787s is accurate to the lidar's specced out frequency of 10 Hz. Does this mean that my lidar is not well suited to SLAM with cartographer, or is there some tuning I can do to work around this?

[gaschler]

This is probably because I have to set imu_sampling_ratio to 0.2 in order to not get the Check failed: data.time... error.

This is not a good work-around. Then, it drops imu messages and uses it even less frequently.

IMU data (frame_id: "0") has a large gap, largest is 0.05026 s

This is still ok.

I get the following error: [FATAL] [1517431711.033191933, 1517427880.733185482]: F0131 15:48:31.000000 30106 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (636530246803500050 vs. 636530246803500050)
I think this can only be thrown by global slam and is related to IMU timestamps.

Can you compile in "RelWithDebInfo" mode and provide a stack trace?

the lidar's specced out frequency of 10 Hz

It's still ok, you could tune similar to cartographer_fetch.

[dirtshell]

So I had to modify my offline launch file because it was made for an older version of cartographer. This just entailed making some of the argument names plural. When I reran the bag after setting imu_sampling_ratio to 1.0, I get the following output:

franklin@lcsaa-data:~/code/lcsaa_slam$ roslaunch cartographer_ros offline_lcsaa_3d.launch bag_filenames:=/home/franklin/Downloads/2018-01-31-14-44-39.bag 
... logging to /home/franklin/.ros/log/4177502e-0787-11e8-89b8-f44d306ec39b/roslaunch-lcsaa-data-18046.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://lcsaa-data:35779/

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)

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

setting /run_id to 4177502e-0787-11e8-89b8-f44d306ec39b
process[rosout-1]: started with pid [18070]
started core service [/rosout]
process[rviz-2]: started with pid [18078]
process[cartographer_offline_node-3]: started with pid [18095]
process[cartographer_occupancy_grid_node-4]: started with pid [18096]
[ INFO] [1517513814.050457009]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer_ros/configuration_files/lcsaa_3d.lua' for 'lcsaa_3d.lua'.
[ INFO] [1517513814.050934695]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1517513814.050996989]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1517513814.051112296]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1517513814.051163962]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1517513814.051378854]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1517513814.051429482]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1517513814.051516575]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1517513814.051564212]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1517513814.051718327]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1517513814.051772624]: I0201 14:36:54.000000 18095 configuration_file_resolver.cc:41] Found '/home/franklin/code/lcsaa_slam/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1517513814.099618540]: I0201 14:36:54.000000 18095 playable_bag.cc:63] Processed 0 of 27.1152 seconds of bag /home/franklin/Downloads/2018-01-31-14-44-39.bag
[ INFO] [1517513814.100811182]: I0201 14:36:54.000000 18095 submaps.cc:323] Added submap 1
[ INFO] [1517513814.100890613]: I0201 14:36:54.000000 18095 map_builder_bridge.cc:91] Added trajectory with ID '0'.
[ INFO] [1517513814.100937109]: I0201 14:36:54.000000 18095 offline_node.cc:237] Assigned trajectory 0 to bag /home/franklin/Downloads/2018-01-31-14-44-39.bag
[ INFO] [1517513814.106440244, 1517427880.233451886]: I0201 14:36:54.000000 18095 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '636530246802790050'.
F0201 14:36:54.121949 18095 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (636530246803000310 vs. 636530246803000310) 
[FATAL] [1517513814.122243207, 1517427880.683274429]: F0201 14:36:54.000000 18095 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (636530246803000310 vs. 636530246803000310) 
*** Check failure stack trace: ***
    @     0x7f5f301a75cd  google::LogMessage::Fail()
    @     0x7f5f301a9433  google::LogMessage::SendToLog()
    @     0x7f5f301a715b  google::LogMessage::Flush()
    @     0x7f5f301a9e1e  google::LogMessageFatal::~LogMessageFatal()
    @           0x6cab16  cartographer::mapping_3d::pose_graph::OptimizationProblem::AddImuData()
    @           0x6b734e  cartographer::mapping_3d::PoseGraph::AddImuData()
    @           0x5f0b1d  cartographer::sensor::Dispatchable<>::AddToTrajectoryBuilder()
    @           0x5ef5ba  cartographer::mapping::CollatedTrajectoryBuilder::HandleCollatedSensorData()
    @           0x5effee  _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_
    @           0x60880a  _ZNSt17_Function_handlerIFvSt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteIS3_EEEZNS2_8Collator13AddTrajectoryEiRKSt13unordered_setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4hashISF_ESt8equal_toISF_ESaISF_EERKSt8functionIFvRKSF_S6_EEEUlS6_E_E9_M_invokeERKSt9_Any_dataOS6_
    @           0x60a5e4  cartographer::sensor::OrderedMultiQueue::Dispatch()
    @           0x60b514  cartographer::sensor::OrderedMultiQueue::Add()
    @           0x608be0  cartographer::sensor::Collator::AddSensorData()
    @           0x5ee40d  cartographer::mapping::CollatedTrajectoryBuilder::AddData()
    @           0x5f06d5  cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData()
    @           0x5cc793  cartographer_ros::SensorBridge::HandleImuMessage()
    @           0x5a46e7  cartographer_ros::Node::HandleImuMessage()
    @           0x587dc5  cartographer_ros::RunOfflineNode()
    @           0x5820c6  main
    @     0x7f5f2bd53830  __libc_start_main
    @           0x5855a9  _start
    @              (nil)  (unknown)
[cartographer_offline_node-3] process has died [pid 18095, exit code -6, cmd /home/franklin/code/lcsaa_slam/install_isolated/lib/cartographer_ros/cartographer_offline_node -configuration_directory /home/franklin/code/lcsaa_slam/install_isolated/share/cartographer_ros/configuration_files -configuration_basenames lcsaa_3d.lua -urdf_filenames /home/franklin/code/lcsaa_slam/install_isolated/share/cartographer_ros/urdf/lcsaa.urdf -bag_filenames /home/franklin/Downloads/2018-01-31-14-44-39.bag points2:=/Sensor/points imu:=/an_device/Imu __name:=cartographer_offline_node __log:=/home/franklin/.ros/log/4177502e-0787-11e8-89b8-f44d306ec39b/cartographer_offline_node-3.log].
log file: /home/franklin/.ros/log/4177502e-0787-11e8-89b8-f44d306ec39b/cartographer_offline_node-3*.log
================================================================================REQUIRED process [rviz-2] has died!
process has finished cleanly
log file: /home/franklin/.ros/log/4177502e-0787-11e8-89b8-f44d306ec39b/rviz-2*.log
Initiating shutdown!
================================================================================
[cartographer_occupancy_grid_node-4] killing on exit
[rviz-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

[Joschka1]

I have a similar error. I used the 2D backpack configuration and launch files and modified it for me. I think my IMU time data is responsible for my issue too.

F0207 15:12:30.260974  9670 ordered_multi_queue.cc:112] Check failed: last_dispatched_time_ <= next_data->GetTime() (636441804467395932 vs. 636441804467315628) Non-sorted data added to queue: '(0, scan)'
[FATAL] [1518012750.261263430, 1508583646.819265097]: F0207 15:12:30.000000  9670 ordered_multi_queue.cc:112] Check failed: last_dispatched_time_ <= next_data->GetTime() (636441804467395932 vs. 636441804467315628) Non-sorted data added to queue: '(0, scan)'
*** Check failure stack trace: ***
    @     0x7fdf9229acad  google::LogMessage::Fail()
    @     0x7fdf9229cb13  google::LogMessage::SendToLog()
    @     0x7fdf9229a808  google::LogMessage::Flush()
    @     0x7fdf9229d579  google::LogMessageFatal::~LogMessageFatal()
    @     0x555971517cd6  (unknown)
    @     0x55597151896f  (unknown)
    @     0x555971510f51  (unknown)
    @     0x55597154e79c  (unknown)
    @     0x555971550dbf  (unknown)
    @     0x555971431728  (unknown)
    @     0x55597140d38b  (unknown)
    @     0x5559713e56c1  (unknown)
    @     0x5559713e018a  (unknown)
    @     0x7fdf87da1f2a  __libc_start_main
    @     0x5559713e354a  (unknown)

[dirtshell]

I have no issue getting 2D SLAM working. Are you saying you modified the 2D config and launch and made them use 3D and it works? If so, can you share your files?

[Joschka1]

of course, here are my config files:

cartographer_ros_2DSLAM_config.zip

[sradmard]

Hi,
I am getting a similar error every now and then using my turtlebot robot after a recent upgrade of my cartographer_ros and cartographer_turtlebot packages. It is a 2D SLAM in my case. Here is the error that I am receiving:

F0213 19:54:03.081979 17386 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (636541772430040077 vs. 636541772430040077) *** Check failure stack trace: *** @ 0x7f4b497bf5cd google::LogMessage::Fail() @ 0x7f4b497c1433 google::LogMessage::SendToLog() @ 0x7f4b497bf15b google::LogMessage::Flush() @ 0x7f4b497c1e1e google::LogMessageFatal::~LogMessageFatal() @ 0x698c06 cartographer::mapping_2d::pose_graph::OptimizationProblem::AddImuData() @ 0x63e9ee cartographer::mapping_2d::PoseGraph::AddImuData() @ 0x68322d cartographer::sensor::Dispatchable<>::AddToTrajectoryBuilder() @ 0x681cca cartographer::mapping::CollatedTrajectoryBuilder::HandleCollatedSensorData() @ 0x6826fe _ZNSt17_Function_handlerIFvRKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteISB_EEEZNS9_7mapping25CollatedTrajectoryBuilderC4EPNSA_17CollatorInterfaceEiRKSt13unordered_setIS5_St4hashIS5_ESt8equal_toIS5_ESaIS5_EES8_INSG_26TrajectoryBuilderInterfaceESC_IST_EEEUlS7_SE_E_E9_M_invokeERKSt9_Any_dataS7_OSE_ @ 0x5e33aa _ZNSt17_Function_handlerIFvSt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteIS3_EEEZNS2_8Collator13AddTrajectoryEiRKSt13unordered_setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4hashISF_ESt8equal_toISF_ESaISF_EERKSt8functionIFvRKSF_S6_EEEUlS6_E_E9_M_invokeERKSt9_Any_dataOS6_ @ 0x5ec464 cartographer::sensor::OrderedMultiQueue::Dispatch() @ 0x5ed394 cartographer::sensor::OrderedMultiQueue::Add() @ 0x5e3780 cartographer::sensor::Collator::AddSensorData() @ 0x680e6d cartographer::mapping::CollatedTrajectoryBuilder::AddData() @ 0x68390e cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData() @ 0x5ae9cd cartographer_ros::SensorBridge::HandleRangefinder() @ 0x5af295 cartographer_ros::SensorBridge::HandleLaserScan() @ 0x5af497 cartographer_ros::SensorBridge::HandleLaserScanMessage() @ 0x5875c9 cartographer_ros::Node::HandleLaserScanMessage() @ 0x5985fe boost::detail::function::void_function_obj_invoker1<>::invoke() @ 0x5a3779 ros::SubscriptionCallbackHelperT<>::call() @ 0x7f4b4742c44d ros::SubscriptionQueue::call() @ 0x7f4b473d2210 ros::CallbackQueue::callOneCB() @ 0x7f4b473d3683 ros::CallbackQueue::callAvailable() @ 0x7f4b47430511 ros::SingleThreadedSpinner::spin() @ 0x7f4b474154cb ros::spin() @ 0x57fc94 cartographer_ros::(anonymous namespace)::Run() @ 0x57d1a4 main @ 0x7f4b45e22830 __libc_start_main @ 0x57f7e9 _start @ (nil) (unknown)

I would really appreciate any hints on that.

[cschuet]

It seems your IMU data is not strictly time ordered. It's fine that
different sensors might not be time ordered relative to one another, but
strict time ordering of different messages for the same sensor_id is an
inherent assumption in Cartographer, hence the check .

On Wed, 14 Feb 2018, 05:07 Sina Radmard, [e-mail] wrote:

Hi,
I am getting a similar error every now and then using my turtlebot robot
after a recent upgrade of my cartographer_ros and cartographer_turtlebot
packages. It is a 2D SLAM in my case. Here is the error that I am receiving:

F0213 19:54:03.081979 17386 map_by_time.h:43] Check failed: data.time >
std::prev(trajectory.end())->first (636541772430040077 vs.
636541772430040077) [FATAL] [1518580443.082764606]: F0213 19:54:03.000000
17386 map_by_time.h:43] Check failed: data.time >
std::prev(trajectory.end())->first (636541772430040077 vs.
636541772430040077) * Check failure stack trace: * @ 0x7f4b497bf5cd
google::LogMessage::Fail() @ 0x7f4b497c1433 google::LogMessage::SendToLog()
@ 0x7f4b497bf15b google::LogMessage::Flush() @ 0x7f4b497c1e1e
google::LogMessageFatal::~LogMessageFatal() @ 0x698c06
cartographer::mapping_2d::pose_graph::OptimizationProblem::AddImuData() @
0x63e9ee cartographer::mapping_2d::PoseGraph::AddImuData() @ 0x68322d
cartographer::sensor::Dispatchable<>::AddToTrajectoryBuilder() @ 0x681cca
cartographer::mapping::CollatedTrajectoryBuilder::HandleCollatedSensorData()
@ 0x6826fe
_ZNSt17_Function_handlerIFvRKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteISB_EEEZNS9_7mapping25CollatedTrajectoryBuilderC4EPNSA_17CollatorInterfaceEiRKSt13unordered_setIS5_St4hashIS5_ESt8equal_toIS5_ESaIS5_EES8_INSG_26TrajectoryBuilderInterfaceESC_IST_EEEUlS7_SE_E_E9_M_invokeERKSt9_Any_dataS7_OSE_
@ 0x5e33aa
_ZNSt17_Function_handlerIFvSt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteIS3_EEEZNS2_8Collator13AddTrajectoryEiRKSt13unordered_setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4hashISF_ESt8equal_toISF_ESaISF_EERKSt8functionIFvRKSF_S6_EEEUlS6_E_E9_M_invokeERKSt9_Any_dataOS6_
@ 0x5ec464 cartographer::sensor::OrderedMultiQueue::Dispatch() @ 0x5ed394
cartographer::sensor::OrderedMultiQueue::Add() @ 0x5e3780
cartographer::sensor::Collator::AddSensorData() @ 0x680e6d
cartographer::mapping::CollatedTrajectoryBuilder::AddData() @ 0x68390e
cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData() @
0x5ae9cd cartographer_ros::SensorBridge::HandleRangefinder() @ 0x5af295
cartographer_ros::SensorBridge::HandleLaserScan() @ 0x5af497
cartographer_ros::SensorBridge::HandleLaserScanMessage() @ 0x5875c9
cartographer_ros::Node::HandleLaserScanMessage() @ 0x5985fe
boost::detail::function::void_function_obj_invoker1<>::invoke() @ 0x5a3779
ros::SubscriptionCallbackHelperT<>::call() @ 0x7f4b4742c44d
ros::SubscriptionQueue::call() @ 0x7f4b473d2210
ros::CallbackQueue::callOneCB() @ 0x7f4b473d3683
ros::CallbackQueue::callAvailable() @ 0x7f4b47430511
ros::SingleThreadedSpinner::spin() @ 0x7f4b474154cb ros::spin() @ 0x57fc94
cartographer_ros::(anonymous namespace)::Run() @ 0x57d1a4 main @
0x7f4b45e22830 __libc_start_main @ 0x57f7e9 _start @ (nil) (unknown)

—
You are receiving this because you are subscribed to this thread.
Reply to this email directly, view it on GitHub
https://github.com/googlecartographer/cartographer_ros/issues/699#issuecomment-365490196,
or mute the thread
https://github.com/notifications/unsubscribe-auth/AX4C8b9jVawq3SkkZNvzMCj5n-fpcM_yks5tUlvkgaJpZM4R0pZO
.

[dirtshell]

Alright, so I have been doing some digging.

Here is a rostopic echo of the /an_device/Imu topic I subscribe to:

header: 
  seq: 31740
  stamp: 
    secs: 1519062981
    nsecs: 550039000
  frame_id: "0"
orientation: 
  x: -0.0347433499992
  y: 0.0323738381267
  z: 0.956651985645
  w: -0.287335932255
orientation_covariance: [0.019947076216340065, 0.0, 0.0, 0.0, 0.0037924565840512514, 0.0, 0.0, 0.0, 0.003790998598560691]
angular_velocity: 
  x: -0.00177334109321
  y: -0.000638203171548
  z: 0.000503105809912
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.317184567451
  y: -0.951718568802
  z: -9.74884033203
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
header: 
  seq: 31741
  stamp: 
    secs: 1519062981
    nsecs: 550039000
  frame_id: "0"
orientation: 
  x: -0.0347433499992
  y: 0.0323738381267
  z: 0.956651985645
  w: -0.287335932255
orientation_covariance: [0.019953863695263863, 0.0, 0.0, 0.0, 0.0037960887420922518, 0.0, 0.0, 0.0, 0.003794672666117549]
angular_velocity: 
  x: -0.00177334109321
  y: -0.000638203171548
  z: 0.000503105809912
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.317184567451
  y: -0.951718568802
  z: -9.74884033203
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

As you can see from this, sometimes the timestamp in the headers will overlap, meaning that I may get two conflicting positions for the timestamp "1519062981s 550039000 ns". I am wondering if this overlap could be the cause for the timing issues? As @cschuet pointed out, this may violate the "strict time ordering" that cartographer is expecting. The sequence IDs are all in order, but there are still multiple readings for a given timestamp.

Perhaps Cartographer should be using sequence numbers in the event of timestamp conflicts?

[dirtshell]

Well I originally thought I had fixed my issue. I rewrote my IMU driver and now all my IMU messages have unique timestamps, but in order to avoid getting the Check failed: data.time > std::prev(trajectory.end())->first error I still end up having to change my imu_sample_rate value to 0.1, which is less than desirable.

The funniest part is that when I run 3d SLAM and don't move the lidar + IMU combo, my IMU seems to be drifting and just rotating all the read points around the origin, generating this trippy circle.

You can see my scanned points in yellow, and the generated "map" in green. It also takes cartographer a long time to generate this map. I imagine that has to do with the fact that cartographer is struggling to match all the points. Turns out this was a result of drift in the z-axis angular velocity and an incorrectly configured URDF. Recalibrating my IMU and changing the rotation of my IMU in the URDF fixed this issue. I am still getting the Check failed error though.

[sradmard]

Thank you @cschuet for your comment. I also double checked and it appeares that my imu readings from /mobile_base/sensors/imu_data_raw topic sometimes has one time stamp for two consecutive imu readings, or even sometimes the time stamp reduces, which is totally weird and I need to fix it.
However, the strange thing is that this problem exist always and I get messed up time stamps frequently for imu, while the cartographer does not crash always, even though my imu_sampling_ratio = 1. Based on my understanding, when imu_sampling_ratio is 1, it is supposed to look at all incoming imu data, so why does it only crash sometimes, and randomly?!

[gaschler]

Thanks for digging down to the root cause for the failed check [1517431711.033191933, 1517427880.733185482]: F0131 15:48:31.000000 30106 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (636530246803500050 vs. 636530246803500050), which seems to be IMU input with not strictly increasing header timestamps.

I tend to say that this is not a cartographer bug but rather invalid input. We need make some assumptions about the input data, otherwise we'd have dozens of new code paths and corner cases.

However, I added an action item to check and catch this in the validate tool https://github.com/googlecartographer/cartographer_ros/issues/529.

[sradmard]

Based on all of the recommendations over here, I started filtering my imu data on the topic /mobile_base/sensors/imu_data_raw, and instead pass the /imu_timestamp_filtered data to my cartographer node. My filter basically discards any message with decreasing or equal timestamps in consecutive imu messages.
However, I am still getting the same cartographer failure:

F0228 15:59:50.081979 15485 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (636541772430040077 vs. 636541772430040077) [FATAL] [1519862390.658199176]: F0228 15:59:50.000000 15485 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (636541772430040077 vs. 636541772430040077)

Any ideas for further investigation?
Thank you so much in advance.

[dirtshell]

@sradmard so I find that if I go all the way with my sample rate, then I get the error still. I dug further and the issue seems to be related to the queue they use, but at that point I just gave up and cheated.

You can edit the map_by_time.h and change the part on line 43 where it says CHECK_GT, and change it to CHECK_GE. I have been running the codebase like this for a bit, and I haven't been having any issues. I don't know if this breaks something else though. Your mileage may vary.

[hvpandya-TRI]

For anyone who stumbles upon this issue in the future, here is a gist of the core problem and a quick fix/hack as suggested by @dirtshell

PROBLEM:

Cartographer checks each sensor message's time_stamp to make sure consecutive messages have strictly ascending (increasing) time_stamps. Certain faulty drivers (or plugins for simulators like gazebo) may publish consecutive messages with the same time_stamp.

SOLUTION:

Edit the map_by_time.h and change CHECK_GT(check greater than) to CHECK_GE(check greater than or equal to). This will let cartographer process consecutive sensor messages even with same time_stamp and will cause FATAL error only if there is a jump back in time.

In theory, as long as the driver/plugin is not publishing random/garbage data, this fix should not have negative side effects.

[ojura]

I think it would be better to handle this at ROS level, i.e. just preprocess your bag to remove measurements with non-increasing stamps, or in the ROS node, avoid calling cartographer_ros::Node::Handle<Sensor> if the stamp is not strictly greater than the last one.

[Duke-Allen]

@dirtshell @cschuet
Hello, I am using a 16-lines LIDAR and IMU to build 3D map by cartographer.
I tun the parameters and run it with no error, the results are as follows:

There is a big gap with the real environment. I think the map is messy. What is the problem of this result? How can I solve it?
This is the validate result of my bag:

• E0227 22:30:57.092067 19056 rosbag_validate_main.cc:365] Average IMU linear acceleration is 10.6084 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.
  I0227 22:30:57.093359 19056 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/IMU" (frame_id: "imu"):
  Count: 37968 Min: 0.004133 Max: 0.016211 Mean: 0.010000
  [0.004133, 0.005340) Count: 2 (0.005268%) Total: 2 (0.005268%)
  [0.005340, 0.006548) Count: 3 (0.007901%) Total: 5 (0.013169%)
  [0.006548, 0.007756) Count: 8 (0.021070%) Total: 13 (0.034239%)
  [0.007756, 0.008964) Count: 828 (2.180784%) Total: 841 (2.215023%)
  [0.008964, 0.010172) ################### Count: 35958 (94.706070%) Total: 36799 (96.921089%)
  [0.010172, 0.011380) Count: 345 (0.908660%) Total: 37144 (97.829750%)
  [0.011380, 0.012588) Count: 820 (2.159714%) Total: 37964 (99.989464%)
  [0.012588, 0.013796) Count: 1 (0.002634%) Total: 37965 (99.992096%)
  [0.013796, 0.015004) Count: 0 (0.000000%) Total: 37965 (99.992096%)
  [0.015004, 0.016211] Count: 3 (0.007901%) Total: 37968 (100.000000%)
  E0227 22:30:57.094650 19056 rosbag_validate_main.cc:382] Point data (frame_id: "rslidar") has a large gap, largest is 0.101409 s, recommended is [0.0005, 0.05] s with no jitter.
  I0227 22:30:57.094745 19056 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/rslidar_points" (frame_id: "rslidar"):
  Count: 3766 Min: 0.100213 Max: 0.101409 Mean: 0.100801
  [0.100213, 0.100333) Count: 1 (0.026553%) Total: 1 (0.026553%)
  [0.100333, 0.100452) Count: 1 (0.026553%) Total: 2 (0.053107%)
  [0.100452, 0.100572) Count: 9 (0.238980%) Total: 11 (0.292087%)
  [0.100572, 0.100691) Count: 33 (0.876261%) Total: 44 (1.168348%)
  [0.100691, 0.100811) ############## Count: 2717 (72.145515%) Total: 2761 (73.313858%)
  [0.100811, 0.100931) ##### Count: 972 (25.809877%) Total: 3733 (99.123741%)
  [0.100931, 0.101050) Count: 25 (0.663834%) Total: 3758 (99.787575%)
  [0.101050, 0.101170) Count: 6 (0.159320%) Total: 3764 (99.946892%)
  [0.101170, 0.101289) Count: 1 (0.026553%) Total: 3765 (99.973450%)
  [0.101289, 0.101409] Count: 1 (0.026553%) Total: 3766 (100.000000%)

Here are my files:
demo_rsldiar_3d.launch
rslidar_3d.launch
mydemo_3d.rviz
rslidar_2d.urdf
rslidar_scan_3d.lua
my data bag
Do you know the problem?