cartographer_ros
cartographer_ros copied to clipboard
Published
20 hours ago •
cartographer-project
cartographer-project
Check failed: odometry_data.pose.IsValid()
Hi,
When I want to run cartographer in gazebo, error occur:
global_trajectory_builder.cc:100] Check failed: odometry_data.pose.IsValid() { t: [inf, -inf, -0.000397655], q: [6.2123e-09, 3.52316e-08, -0.106439, -7.50879e-08] }
Be inspired by this issue, I check my /odom, the pose msg is something like below, the value seems like no problem, the norm of the quaternion is 1. Could you please provide some advice? Thanks a lot.
header:
seq: 1554
stamp:
secs: 370
nsecs: 92000000
frame_id: "odom"
child_frame_id: "base_footprint"
pose:
pose:
position:
x: -5.10870279874e-05
y: 9.32454944017e-05
z: 1.53271700887e-08
orientation:
x: -9.63391262863e-08
y: 2.13254059424e-09
z: 7.15440031607e-07
w: 1.0
covariance: [1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
twist:
linear:
x: -3.3529998755e-06
y: -4.87689774247e-09
z: 0.0
angular:
x: 0.0
y: 0.0
z: 3.47463370907e-09
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
The completely error information is:
SUMMARY
========
PARAMETERS
* /rosdistro: kinetic
* /rosversion: 1.12.17
* /use_sim_time: True
NODES
/
cartographer_node (cartographer_ros/cartographer_node)
cartographer_occupancy_grid_node (cartographer_ros/cartographer_occupancy_grid_node)
rviz (rviz/rviz)
ROS_MASTER_URI=http://localhost:11311
process[cartographer_node-1]: started with pid [6815]
process[cartographer_occupancy_grid_node-2]: started with pid [6816]
[ INFO] [1609163640.381360868]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/ros/catkin_ws/src/ros_basic_tutorials/mbot_slam/mbot_navigation/config/lidar.lua' for 'lidar.lua'.
[ INFO] [1609163640.381767062]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1609163640.381800245]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1609163640.381845676]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1609163640.381870932]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1609163640.381987745]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1609163640.382019373]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1609163640.382065073]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1609163640.382089075]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1609163640.382204120]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1609163640.382229368]: I1228 21:54:00.000000 6815 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1609163640.391712109]: I1228 21:54:00.000000 6815 map_builder_bridge.cc:135] Added trajectory with ID '0'.
process[rviz-3]: started with pid [6906]
[ INFO] [1609163640.714377411, 477.030000000]: I1228 21:54:00.000000 6815 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '621355972770280000'.
F1228 21:54:00.736969 6815 global_trajectory_builder.cc:100] Check failed: odometry_data.pose.IsValid() { t: [inf, -inf, -0.000397655], q: [6.2123e-09, 3.52316e-08, -0.106439, -7.50879e-08] }
[FATAL] [1609163640.737147250, 477.053000000]: F1228 21:54:00.000000 6815 global_trajectory_builder.cc:100] Check failed: odometry_data.pose.IsValid() { t: [inf, -inf, -0.000397655], q: [6.2123e-09, 3.52316e-08, -0.106439, -7.50879e-08] }
*** Check failure stack trace: ***
@ 0x7f5a35a415cd google::LogMessage::Fail()
@ 0x7f5a35a43433 google::LogMessage::SendToLog()
@ 0x7f5a35a4115b google::LogMessage::Flush()
@ 0x7f5a35a43e1e google::LogMessageFatal::~LogMessageFatal()
@ 0x715143 cartographer::mapping::(anonymous namespace)::GlobalTrajectoryBuilder<>::AddSensorData()
@ 0x63a48d cartographer::sensor::Dispatchable<>::AddToTrajectoryBuilder()
@ 0x6fd72a cartographer::mapping::CollatedTrajectoryBuilder::HandleCollatedSensorData()
@ 0x6fe14e _ZNSt17_Function_handlerIFvRKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteISB_EEEZNS9_7mapping25CollatedTrajectoryBuilderC4ERKNSG_5proto24TrajectoryBuilderOptionsEPNSA_17CollatorInterfaceEiRKSt3setINSG_26TrajectoryBuilderInterface8SensorIdESt4lessISQ_ESaISQ_EES8_ISP_SC_ISP_EEEUlS7_SE_E_E9_M_invokeERKSt9_Any_dataS7_OSE_
@ 0x6489ca _ZNSt17_Function_handlerIFvSt10unique_ptrIN12cartographer6sensor4DataESt14default_deleteIS3_EEEZNS2_8Collator13AddTrajectoryEiRKN4absl13flat_hash_setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEENS9_18container_internal10StringHashENSH_12StringHashEq2EqESaISG_EEERKSt8functionIFvRKSG_S6_EEEUlS6_E_E9_M_invokeERKSt9_Any_dataOS6_
@ 0x64b74d cartographer::sensor::OrderedMultiQueue::Dispatch()
@ 0x64bf77 cartographer::sensor::OrderedMultiQueue::Add()
@ 0x648d50 cartographer::sensor::Collator::AddSensorData()
@ 0x6fcb0d cartographer::mapping::CollatedTrajectoryBuilder::AddData()
@ 0x63a95d cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData()
@ 0x603a8c cartographer_ros::SensorBridge::HandleOdometryMessage()
@ 0x5cbfe9 cartographer_ros::Node::HandleOdometryMessage()
@ 0x5ee6be boost::detail::function::void_function_obj_invoker1<>::invoke()
@ 0x5f95b5 ros::SubscriptionCallbackHelperT<>::call()
@ 0x7f5a31aebc8d ros::SubscriptionQueue::call()
@ 0x7f5a31a919e0 ros::CallbackQueue::callOneCB()
@ 0x7f5a31a9341b ros::CallbackQueue::callAvailable()
@ 0x7f5a31aefd39 ros::SingleThreadedSpinner::spin()
@ 0x7f5a31ad4d8b ros::spin()
@ 0x5c5103 cartographer_ros::(anonymous namespace)::Run()
@ 0x5c1d24 main
@ 0x7f5a30485840 __libc_start_main
@ 0x5c4bc9 _start
@ (nil) (unknown)
[cartographer_node-1] process has died [pid 6815, exit code -6, cmd /home/td/slam/cartographer_ws/install_isolated/lib/cartographer_ros/cartographer_node -configuration_directory /home/td/ros/catkin_ws/src/ros_basic_tutorials/mbot_slam/mbot_navigation/config -configuration_basename lidar.lua scan:=scan __name:=cartographer_node __log:=/home/td/.ros/log/d6e926c0-4913-11eb-8524-2016b9896a4d/cartographer_node-1.log].
log file: /home/td/.ros/log/d6e926c0-4913-11eb-8524-2016b9896a4d/cartographer_node-1*.log
The config file is:
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_footprint",
published_frame = "base_footprint",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 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.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
The launch file is:
<launch>
<param name="/use_sim_time" value="true" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find mbot_navigation)/config
-configuration_basename lidar.lua"
output="screen">
<remap from="scan" to="scan" />
</node>
<!-- cartographer_occupancy_grid_node -->
<node pkg="cartographer_ros" type="cartographer_occupancy_grid_node"
name="cartographer_occupancy_grid_node"
args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>
EDIT:
I run the Deutsches Museum bag, there is also an error:
F1229 10:32:34.000000 4231 msg_conversion.cc:135] Check failed: msg.range_min >= 0.f (-2.35183 vs. 0),
The entire output is:
SUMMARY
========
PARAMETERS
* /robot_description: <...>
* /rosdistro: kinetic
* /rosversion: 1.12.17
* /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)
auto-starting new master
process[master]: started with pid [4201]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to 1b1228be-497e-11eb-979e-2016b9896a4d
process[rosout-1]: started with pid [4214]
started core service [/rosout]
process[robot_state_publisher-2]: started with pid [4221]
process[cartographer_node-3]: started with pid [4231]
process[cartographer_occupancy_grid_node-4]: started with pid [4241]
[ INFO] [1609209154.070916107]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer_ros/configuration_files/backpack_2d.lua' for 'backpack_2d.lua'.
process[rviz-5]: started with pid [4272]
process[playbag-6]: started with pid [4332]
[ INFO] [1609209154.098070154]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1609209154.098114614]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1609209154.098438471]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1609209154.098469468]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1609209154.098846313]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1609209154.098878578]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1609209154.099184443]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1609209154.099212800]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1609209154.099576490]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1609209154.099617918]: I1229 10:32:34.000000 4231 configuration_file_resolver.cc:41] Found '/home/td/slam/cartographer_ws/install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1609209154.111654973]: I1229 10:32:34.000000 4231 map_builder_bridge.cc:135] Added trajectory with ID '0'.
F1229 10:32:34.585171 4231 msg_conversion.cc:135] Check failed: msg.range_min >= 0.f (-2.35183 vs. 0)
[FATAL] [1609209154.585319449, 1432647016.505443161]: F1229 10:32:34.000000 4231 msg_conversion.cc:135] Check failed: msg.range_min >= 0.f (-2.35183 vs. 0)
*** Check failure stack trace: ***
@ 0x7f42f8bf25cd google::LogMessage::Fail()
@ 0x7f42f8bf4433 google::LogMessage::SendToLog()
@ 0x7f42f8bf215b google::LogMessage::Flush()
@ 0x7f42f8bf4e1e google::LogMessageFatal::~LogMessageFatal()
@ 0x61b35b _ZN16cartographer_ros12_GLOBAL__N_136LaserScanToPointCloudWithIntensitiesIN11sensor_msgs19MultiEchoLaserScan_ISaIvEEEEESt5tupleIJN12cartographer6sensor25PointCloudWithIntensitiesENSt6chrono10time_pointINS7_6common23UniversalTimeScaleClockENSA_8durationIlSt5ratioILl1ELl10000000EEEEEEEERKT_
@ 0x61b47d cartographer_ros::ToPointCloudWithIntensities()
@ 0x60524a cartographer_ros::SensorBridge::HandleMultiEchoLaserScanMessage()
@ 0x5cc461 cartographer_ros::Node::HandleMultiEchoLaserScanMessage()
@ 0x5ee4de boost::detail::function::void_function_obj_invoker1<>::invoke()
@ 0x5fab15 ros::SubscriptionCallbackHelperT<>::call()
@ 0x7f42f4c9cc8d ros::SubscriptionQueue::call()
@ 0x7f42f4c429e0 ros::CallbackQueue::callOneCB()
@ 0x7f42f4c4441b ros::CallbackQueue::callAvailable()
@ 0x7f42f4ca0d39 ros::SingleThreadedSpinner::spin()
@ 0x7f42f4c85d8b ros::spin()
@ 0x5c5103 cartographer_ros::(anonymous namespace)::Run()
@ 0x5c1d24 main
@ 0x7f42f3636840 __libc_start_main
@ 0x5c4bc9 _start
@ (nil) (unknown)
[cartographer_node-3] process has died [pid 4231, exit code -6, cmd /home/td/slam/cartographer_ws/install_isolated/lib/cartographer_ros/cartographer_node -configuration_directory /home/td/slam/cartographer_ws/install_isolated/share/cartographer_ros/configuration_files -configuration_basename backpack_2d.lua echoes:=horizontal_laser_2d __name:=cartographer_node __log:=/home/td/.ros/log/1b1228be-497e-11eb-979e-2016b9896a4d/cartographer_node-3.log].
log file: /home/td/.ros/log/1b1228be-497e-11eb-979e-2016b9896a4d/cartographer_node-3*.log
Your sensor data looks broken. inf values in odometry and negative laser readings don't make sense.
@MichaelGrupp
Thanks for your reply.
I also run the example of Deutsches Museum bag as said in the EDIT part. There is also an error:F1229 10:32:34.000000 4231 msg_conversion.cc:135] Check failed: msg.range_min >= 0.f (-2.35183 vs. 0).
Could you please provide some advice? Thanks a lot.
As I said, negative laser readings don't make sense. I can't help you with debugging why your sensor driver publishes these values.
@MichaelGrupp Thanks for your reply.
But the Deutsches Museum bag is the demo bag provide by cartographer ros tutorial. It should not have this problem:F1229 10:32:34.000000 4231 msg_conversion.cc:135] Check failed: msg.range_min >= 0.f (-2.35183 vs. 0)
