Adds a dump_timing flag. (#541)

Moves #529 forward.
master
Holger Rapp 2017-10-16 14:23:56 +02:00 committed by GitHub
parent 5b06f03558
commit fe28d33d38
1 changed files with 71 additions and 18 deletions

View File

@ -14,11 +14,13 @@
* limitations under the License. * limitations under the License.
*/ */
#include <fstream>
#include <iostream> #include <iostream>
#include <map> #include <map>
#include <string> #include <string>
#include "cartographer/common/histogram.h" #include "cartographer/common/histogram.h"
#include "cartographer/common/make_unique.h"
#include "gflags/gflags.h" #include "gflags/gflags.h"
#include "glog/logging.h" #include "glog/logging.h"
#include "nav_msgs/Odometry.h" #include "nav_msgs/Odometry.h"
@ -36,24 +38,52 @@
#include "urdf/model.h" #include "urdf/model.h"
DEFINE_string(bag_filename, "", "Bag to process."); DEFINE_string(bag_filename, "", "Bag to process.");
DEFINE_bool(dump_timing, false,
"Dump per-sensor timing information in files called "
"timing_<frame_id>.csv in the current directory.");
namespace cartographer_ros { namespace cartographer_ros {
namespace { namespace {
struct TimestampData { struct PerFrameId {
ros::Time last_timestamp; ros::Time last_timestamp;
string topic; string topic;
::cartographer::common::Histogram histogram; ::cartographer::common::Histogram histogram;
std::unique_ptr<std::ofstream> timing_file;
}; };
void Run(const string& bag_filename) { std::unique_ptr<std::ofstream> CreateTimingFile(const string& frame_id) {
auto timing_file = ::cartographer::common::make_unique<std::ofstream>(
std::string("timing_") + frame_id + ".csv", std::ios_base::out);
(*timing_file) << "# Timing information for sensor with frame id: "
<< frame_id << std::endl
<< "# Columns are in order" << std::endl
<< "# - packet index of the packet in the bag, first packet is 1"
<< std::endl
<< "# - timestamp when rosbag wrote the packet, i.e. "
"rosbag::MessageInstance::getTime().toNSec()"
<< std::endl
<< "# - timestamp when data was acquired, i.e. "
"message.header.stamp.toNSec()"
<< std::endl
<< "#" << std::endl
<< "# The data can be read in python using" << std::endl
<< "# import numpy" << std::endl
<< "# np.loadtxt(<filename>, dtype='uint64')" << std::endl;
return timing_file;
}
void Run(const string& bag_filename, const bool dump_timing) {
rosbag::Bag bag; rosbag::Bag bag;
bag.open(bag_filename, rosbag::bagmode::Read); bag.open(bag_filename, rosbag::bagmode::Read);
rosbag::View view(bag); rosbag::View view(bag);
const ::ros::Time begin_time = view.getBeginTime();
std::map<string, TimestampData> timestamp_data; std::map<string, PerFrameId> per_frame_id;
size_t message_index = 0;
for (const rosbag::MessageInstance& message : view) { for (const rosbag::MessageInstance& message : view) {
++message_index;
string frame_id; string frame_id;
ros::Time time; ros::Time time;
if (message.isType<sensor_msgs::PointCloud2>()) { if (message.isType<sensor_msgs::PointCloud2>()) {
@ -80,17 +110,18 @@ void Run(const string& bag_filename) {
continue; continue;
} }
if (!timestamp_data.count(frame_id)) { bool first_packet = false;
timestamp_data.emplace( if (!per_frame_id.count(frame_id)) {
frame_id, TimestampData{time, message.getTopic(), per_frame_id.emplace(
::cartographer::common::Histogram()}); frame_id,
} else { PerFrameId{time, message.getTopic(),
auto& entry = timestamp_data.at(frame_id); ::cartographer::common::Histogram(),
if (entry.topic != message.getTopic()) { dump_timing ? CreateTimingFile(frame_id) : nullptr});
LOG(ERROR) << "frame_id \"" << frame_id first_packet = true;
<< "\" is send on multiple topics. It was seen at least on " }
<< entry.topic << " and " << message.getTopic();
} auto& entry = per_frame_id.at(frame_id);
if (!first_packet) {
const double delta_t_sec = (time - entry.last_timestamp).toSec(); const double delta_t_sec = (time - entry.last_timestamp).toSec();
if (delta_t_sec < 0) { if (delta_t_sec < 0) {
LOG(ERROR) << "Sensor with frame_id \"" << frame_id LOG(ERROR) << "Sensor with frame_id \"" << frame_id
@ -100,18 +131,40 @@ void Run(const string& bag_filename) {
"acquired from the sensor."; "acquired from the sensor.";
} }
entry.histogram.Add(delta_t_sec); entry.histogram.Add(delta_t_sec);
entry.last_timestamp = time; }
if (entry.topic != message.getTopic()) {
LOG(ERROR) << "frame_id \"" << frame_id
<< "\" is send on multiple topics. It was seen at least on "
<< entry.topic << " and " << message.getTopic();
}
entry.last_timestamp = time;
if (dump_timing) {
CHECK(entry.timing_file != nullptr);
(*entry.timing_file) << message_index << "\t"
<< message.getTime().toNSec() << "\t"
<< time.toNSec() << std::endl;
} }
} }
bag.close(); bag.close();
constexpr int kNumBucketsForHistogram = 10; constexpr int kNumBucketsForHistogram = 10;
for (const auto& entry_pair : timestamp_data) { for (const auto& entry_pair : per_frame_id) {
LOG(INFO) << "Time delta histogram for consecutive messages on topic \"" LOG(INFO) << "Time delta histogram for consecutive messages on topic \""
<< entry_pair.second.topic << "\" (frame_id: \"" << entry_pair.second.topic << "\" (frame_id: \""
<< entry_pair.first << "\"):\n" << entry_pair.first << "\"):\n"
<< entry_pair.second.histogram.ToString(kNumBucketsForHistogram); << entry_pair.second.histogram.ToString(kNumBucketsForHistogram);
} }
if (dump_timing) {
for (const auto& entry_pair : per_frame_id) {
entry_pair.second.timing_file->close();
CHECK(*entry_pair.second.timing_file)
<< "Could not write timing information for \"" << entry_pair.first
<< "\"";
}
}
} }
} // namespace } // namespace
@ -123,5 +176,5 @@ int main(int argc, char** argv) {
google::ParseCommandLineFlags(&argc, &argv, true); google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
::cartographer_ros::Run(FLAGS_bag_filename); ::cartographer_ros::Run(FLAGS_bag_filename, FLAGS_dump_timing);
} }