diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc index 627b661..b663ee7 100644 --- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc +++ b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc @@ -149,7 +149,31 @@ class RangeDataChecker { template void CheckMessage(const MessageType& message) { const std::string& frame_id = message.header.frame_id; - RangeChecksum current_checksum = ComputeRangeChecksum(message); + ros::Time current_time_stamp = message.header.stamp; + RangeChecksum current_checksum; + ros::Duration range_duration; + ReadRangeMessage(message, ¤t_checksum, &range_duration); + ros::Time current_time_first_point = current_time_stamp - range_duration; + auto previous_time_stamp_it = + frame_id_to_previous_time_stamp_.find(frame_id); + if (previous_time_stamp_it != frame_id_to_previous_time_stamp_.end() && + previous_time_stamp_it->second >= current_time_first_point) { + LOG_FIRST_N(WARNING, 3) + << "Sensor with frame_id \"" << frame_id + << "\" measurements overlap in time " + << "Previous range message, ending at time stamp " + << previous_time_stamp_it->second + << ", must finish before current range message, " + << "which ranges from " << current_time_first_point << " to " + << current_time_stamp; + double overlap = + (previous_time_stamp_it->second - current_time_first_point).toSec(); + auto it = frame_id_to_max_overlap_duration_.find(frame_id); + if (it == frame_id_to_max_overlap_duration_.end() || + overlap > frame_id_to_max_overlap_duration_.at(frame_id)) { + frame_id_to_max_overlap_duration_[frame_id] = overlap; + } + } if (current_checksum.first == 0) { return; } @@ -160,11 +184,21 @@ class RangeDataChecker { LOG_FIRST_N(ERROR, 3) << "Sensor with frame_id \"" << frame_id << "\" sends exactly the same range measurements multiple times. " - << "Range data at time " << message.header.stamp - << " equals preceding data."; + << "Range data at time " << current_time_stamp + << " equals preceding data with " << current_checksum.first + << " points."; } } frame_id_to_range_checksum_[frame_id] = current_checksum; + frame_id_to_previous_time_stamp_[frame_id] = message.header.stamp; + } + + void PrintReport() { + for (auto& it : frame_id_to_max_overlap_duration_) { + LOG(WARNING) << "Sensor with frame_id \"" << it.first + << "\" range measurements have longest overlap of " + << it.second << " s"; + } } private: @@ -172,7 +206,9 @@ class RangeDataChecker { RangeChecksum; template - RangeChecksum ComputeRangeChecksum(const MessageType& message) { + static void ReadRangeMessage(const MessageType& message, + RangeChecksum* range_checksum, + ros::Duration* range_duration) { auto point_cloud_with_intensities = ToPointCloudWithIntensities(message); const cartographer::sensor::TimedPointCloud& point_cloud = std::get<0>(point_cloud_with_intensities).points; @@ -180,10 +216,29 @@ class RangeDataChecker { for (const Eigen::Vector4f& point : point_cloud) { points_sum += point; } - return {point_cloud.size(), points_sum}; + if (point_cloud.size() > 0) { + double first_point_relative_time = point_cloud[0][3]; + double last_point_relative_time = (*point_cloud.rbegin())[3]; + if (first_point_relative_time > 0) { + LOG_FIRST_N(ERROR, 1) + << "Sensor with frame_id \"" << message.header.frame_id + << "\" has range data that has positive relative time " + << first_point_relative_time << " s, must negative or zero."; + } + if (last_point_relative_time != 0) { + LOG_FIRST_N(ERROR, 1) + << "Sensor with frame_id \"" << message.header.frame_id + << "\" has range data whose last point has relative time " + << last_point_relative_time << " s, must be zero."; + } + } + *range_checksum = {point_cloud.size(), points_sum}; + *range_duration = ros::Duration(-point_cloud[0][3]); } std::map frame_id_to_range_checksum_; + std::map frame_id_to_previous_time_stamp_; + std::map frame_id_to_max_overlap_duration_; }; void Run(const std::string& bag_filename, const bool dump_timing) { @@ -292,6 +347,8 @@ void Run(const std::string& bag_filename, const bool dump_timing) { } bag.close(); + range_data_checker.PrintReport(); + if (num_imu_messages > 0) { double average_imu_acceleration = sum_imu_acceleration / num_imu_messages; if (std::isnan(average_imu_acceleration) ||