Adds a rosbag_validate binary. (#536)

So far it creates statistics over timing information of the sensor data that Cartographer cares about. It also reports if time jumps backwards for a sensor.
Also updates the GitHub issue template to ask users to run this binary when reporting issues.

I verified that this tool is sufficient to analyze all the timing related issues mentioned in #529.

.github/ISSUE_TEMPLATE

@@ -1,13 +1,25 @@
 Thank you for filing an issue!
 
-It would help us tremendously if you provide the following so that we can easily
+It would help us tremendously if you run through the following checklist. This
-reproduce your issue:
+ensures that we have the most information to quickly understand, analyze,
+reproduce and eventually resolve your issue.
 
-1. A link to a Git repository containing a branch of `cartographer_ros`
+Please
-   containing all the configuration, launch, and URDF files required to
-   reproduce your issue.
-2. A link to a bag file we can use to reproduce your issue.
 
-We will likely be unable to help you without both.
+1. run `rosbag_validate` which does some checks on your sensor data. This
+   tool often finds issues that can explain poor performance and must be fixed
+   at recording time. Please post the full output of the tool into a
+   GitHub Gist at https://gist.github.com/, then link it in the issue even if
+   it does not report anything. You can run the tool like this:
 
-Please remove this boilerplate text before submitting your issue.
+   rosrun cartographer_ros cartographer_rosbag_validate -bag_filename <bag filename>
+
+2. post a link to a Git repository containing a branch of
+   `cartographer_ros` containing all the configuration, launch, and URDF files
+   required to reproduce your issue.
+3. post a link to a bag file we can use to reproduce your issue. Put it on
+   Google Drive, Dropbox, any webserver or wherever it is publicly
+   downloadable.
+4. remove this boilerplate text before submitting your issue.
+
+We will likely be unable to help you without all of these points addressed.

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -66,3 +66,14 @@ install(TARGETS cartographer_occupancy_grid_node
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+
+google_binary(cartographer_rosbag_validate
+  SRCS
+    rosbag_validate_main.cc
+)
+
+install(TARGETS cartographer_rosbag_validate
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

cartographer_ros/cartographer_ros/rosbag_validate_main.cc

@@ -0,0 +1,127 @@
+/*
+ * Copyright 2017 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <iostream>
+#include <map>
+#include <string>
+
+#include "cartographer/common/histogram.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "nav_msgs/Odometry.h"
+#include "ros/ros.h"
+#include "ros/time.h"
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "tf2_eigen/tf2_eigen.h"
+#include "tf2_msgs/TFMessage.h"
+#include "tf2_ros/buffer.h"
+#include "urdf/model.h"
+
+DEFINE_string(bag_filename, "", "Bag to process.");
+
+namespace cartographer_ros {
+namespace {
+
+struct TimestampData {
+  ros::Time last_timestamp;
+  string topic;
+  ::cartographer::common::Histogram histogram;
+};
+
+void Run(const string& bag_filename) {
+  rosbag::Bag bag;
+  bag.open(bag_filename, rosbag::bagmode::Read);
+  rosbag::View view(bag);
+  const ::ros::Time begin_time = view.getBeginTime();
+
+  std::map<string, TimestampData> timestamp_data;
+  for (const rosbag::MessageInstance& message : view) {
+    string frame_id;
+    ros::Time time;
+    if (message.isType<sensor_msgs::PointCloud2>()) {
+      auto msg = message.instantiate<sensor_msgs::PointCloud2>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+    } else if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
+      auto msg = message.instantiate<sensor_msgs::MultiEchoLaserScan>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+    } else if (message.isType<sensor_msgs::LaserScan>()) {
+      auto msg = message.instantiate<sensor_msgs::LaserScan>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+    } else if (message.isType<sensor_msgs::Imu>()) {
+      auto msg = message.instantiate<sensor_msgs::Imu>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+    } else if (message.isType<nav_msgs::Odometry>()) {
+      auto msg = message.instantiate<nav_msgs::Odometry>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+    } else {
+      continue;
+    }
+
+    if (!timestamp_data.count(frame_id)) {
+      timestamp_data.emplace(
+          frame_id, TimestampData{time, message.getTopic(),
+                                  ::cartographer::common::Histogram()});
+    } else {
+      auto& entry = timestamp_data.at(frame_id);
+      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();
+      }
+      const double delta_t_sec = (time - entry.last_timestamp).toSec();
+      if (delta_t_sec < 0) {
+        LOG(ERROR) << "Sensor with frame_id \"" << frame_id
+                   << "\" jumps backwards in time. Make sure that the bag "
+                      "contains the data for each frame_id sorted by "
+                      "header.stamp, i.e. the order in which they were "
+                      "acquired from the sensor.";
+      }
+      entry.histogram.Add(delta_t_sec);
+      entry.last_timestamp = time;
+    }
+  }
+  bag.close();
+
+  constexpr int kNumBucketsForHistogram = 10;
+  for (const auto& entry_pair : timestamp_data) {
+    LOG(INFO) << "Time delta histogram for consecutive messages on topic \""
+              << entry_pair.second.topic << "\" (frame_id: \""
+              << entry_pair.first << "\"):\n"
+              << entry_pair.second.histogram.ToString(kNumBucketsForHistogram);
+  }
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  FLAGS_alsologtostderr = true;
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
+  ::cartographer_ros::Run(FLAGS_bag_filename);
+}