12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Validate tool checks per-point time stamps. (#737) 

Checks for per-point timing issues in a bag file.
Feature is tracked in #529.
master
gaschler 2018-02-26 18:16:59 +01:00 committed by Wally B. Feed
parent 5251634d3d
commit be592f55fa
1 changed files with 62 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
cartographer_ros/cartographer_ros/rosbag_validate_main.cc
View File

@ -149,7 +149,31 @@ class RangeDataChecker {
template <typename MessageType>
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, &current_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 <typename MessageType>
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<std::string, RangeChecksum> frame_id_to_range_checksum_;
std::map<std::string, ros::Time> frame_id_to_previous_time_stamp_;
std::map<std::string, double> 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) ||