Warn for possible topic mismatch (#935)

FIXES=#929

cartographer_ros/cartographer_ros/node.cc

@@ -357,6 +357,9 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
   AddExtrapolator(trajectory_id, options);
   AddSensorSamplers(trajectory_id, options);
   LaunchSubscribers(options, topics, trajectory_id);
+  wall_timers_.push_back(node_handle_.createWallTimer(
+      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
+      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
   for (const auto& sensor_id : expected_sensor_ids) {
     subscribed_topics_.insert(sensor_id.id);
   }
@@ -790,4 +793,35 @@ void Node::LoadState(const std::string& state_filename,
   map_builder_bridge_.LoadState(state_filename, load_frozen_state);
 }
 
+void Node::MaybeWarnAboutTopicMismatch(
+    const ::ros::WallTimerEvent& unused_timer_event) {
+  ::ros::master::V_TopicInfo ros_topics;
+  ::ros::master::getTopics(ros_topics);
+  std::set<std::string> published_topics;
+  std::stringstream published_topics_string;
+  for (const auto& it : ros_topics) {
+    std::string resolved_topic = node_handle_.resolveName(it.name, false);
+    published_topics.insert(resolved_topic);
+    published_topics_string << resolved_topic << ",";
+  }
+  bool print_topics = false;
+  for (const auto& entry : subscribers_) {
+    int trajectory_id = entry.first;
+    for (const auto& subscriber : entry.second) {
+      std::string resolved_topic = node_handle_.resolveName(subscriber.topic);
+      if (published_topics.count(resolved_topic) == 0) {
+        LOG(WARNING) << "Expected topic \"" << subscriber.topic
+                     << "\" (trajectory " << trajectory_id << ")"
+                     << " (resolved topic \"" << resolved_topic << "\")"
+                     << " but no publisher is currently active.";
+        print_topics = true;
+      }
+    }
+  }
+  if (print_topics) {
+    LOG(WARNING) << "Currently available topics are: "
+                 << published_topics_string.str();
+  }
+}
+
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/node.h

@@ -171,6 +171,7 @@ class Node {
                           const TrajectoryOptions& options);
   cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(
       int trajectory_id) REQUIRES(mutex_);
+  void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&);
 
   const NodeOptions node_options_;
 

cartographer_ros/cartographer_ros/node_constants.h

@@ -43,6 +43,7 @@ constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
 constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
 constexpr char kConstraintListTopic[] = "constraint_list";
 constexpr double kConstraintPublishPeriodSec = 0.5;
+constexpr double kTopicMismatchCheckDelaySec = 3.0;
 
 constexpr int kInfiniteSubscriberQueueSize = 0;
 constexpr int kLatestOnlyPublisherQueueSize = 1;

cartographer_ros/cartographer_ros/offline_node.cc

@@ -238,7 +238,27 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
         }));
   }
 
-  // TODO(gaschler): Warn if resolved topics are not in bags.
+  std::set<std::string> bag_topics;
+  std::stringstream bag_topics_string;
+  for (const auto& topic : playable_bag_multiplexer.topics()) {
+    std::string resolved_topic = node.node_handle()->resolveName(topic, false);
+    bag_topics.insert(resolved_topic);
+    bag_topics_string << resolved_topic << ",";
+  }
+  bool print_topics = false;
+  for (const auto& entry : bag_topic_to_sensor_id) {
+    const std::string& resolved_topic = entry.first.second;
+    if (bag_topics.count(resolved_topic) == 0) {
+      LOG(WARNING) << "Expected resolved topic \"" << resolved_topic
+                   << "\" not found in bag file(s).";
+      print_topics = true;
+    }
+  }
+  if (print_topics) {
+    LOG(WARNING) << "Available topics in bag file(s) are "
+                 << bag_topics_string.str();
+  }
+
   std::unordered_map<int, int> bag_index_to_trajectory_id;
   const ros::Time begin_time =
       // If no bags were loaded, we cannot peek the time of first message.

cartographer_ros/cartographer_ros/playable_bag.cc

@@ -43,6 +43,9 @@ PlayableBag::PlayableBag(
       filtering_early_message_handler_(
           std::move(filtering_early_message_handler)) {
   AdvanceUntilMessageAvailable();
+  for (const auto* connection_info : view_->getConnections()) {
+    topics_.insert(connection_info->topic);
+  }
 }
 
 ros::Time PlayableBag::PeekMessageTime() const {
@@ -99,6 +102,9 @@ void PlayableBag::AdvanceUntilMessageAvailable() {
 }
 
 void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) {
+  for (const auto& topic : playable_bag.topics()) {
+    topics_.insert(topic);
+  }
   playable_bags_.push_back(std::move(playable_bag));
   CHECK(playable_bags_.back().IsMessageAvailable());
   next_message_queue_.emplace(

cartographer_ros/cartographer_ros/playable_bag.h

@@ -43,6 +43,7 @@ class PlayableBag {
   std::tuple<ros::Time, ros::Time> GetBeginEndTime() const;
 
   int bag_id() const;
+  std::set<std::string> topics() const { return topics_; }
 
  private:
   void AdvanceOneMessage();
@@ -59,6 +60,7 @@ class PlayableBag {
   std::deque<rosbag::MessageInstance> buffered_messages_;
   const ::ros::Duration buffer_delay_;
   FilteringEarlyMessageHandler filtering_early_message_handler_;
+  std::set<std::string> topics_;
 };
 
 class PlayableBagMultiplexer {
@@ -75,6 +77,8 @@ class PlayableBagMultiplexer {
   bool IsMessageAvailable() const;
   ros::Time PeekMessageTime() const;
 
+  std::set<std::string> topics() const { return topics_; }
+
  private:
   struct BagMessageItem {
     ros::Time message_timestamp;
@@ -90,6 +94,7 @@ class PlayableBagMultiplexer {
   std::priority_queue<BagMessageItem, std::vector<BagMessageItem>,
                       BagMessageItem::TimestampIsGreater>
       next_message_queue_;
+  std::set<std::string> topics_;
 };
 
 }  // namespace cartographer_ros