From 5b06f035583590945dc2d0f014c9899be30b9da6 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Fri, 13 Oct 2017 17:26:50 +0200 Subject: [PATCH] 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 | 28 ++-- .../cartographer_ros/CMakeLists.txt | 11 ++ .../cartographer_ros/rosbag_validate_main.cc | 127 ++++++++++++++++++ 3 files changed, 158 insertions(+), 8 deletions(-) create mode 100644 cartographer_ros/cartographer_ros/rosbag_validate_main.cc diff --git a/.github/ISSUE_TEMPLATE b/.github/ISSUE_TEMPLATE index d61104a..1966427 100644 --- a/.github/ISSUE_TEMPLATE +++ b/.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 -reproduce your issue: +It would help us tremendously if you run through the following checklist. This +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` - 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. +Please -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 + +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. diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 61bdc21..04c0090 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/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} +) diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc new file mode 100644 index 0000000..b3943e9 --- /dev/null +++ b/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 +#include +#include + +#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 timestamp_data; + for (const rosbag::MessageInstance& message : view) { + string frame_id; + ros::Time time; + if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + } else if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + } else if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + } else if (message.isType()) { + auto msg = message.instantiate(); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + } else if (message.isType()) { + auto msg = message.instantiate(); + 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); +}