diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 6323d61..bdaf5c4 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/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 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 diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 8061f64..05a4c96 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/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_; diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index a0e3f3e..b819121 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/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; diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index 08bbef9..bc3e1b1 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/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 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 bag_index_to_trajectory_id; const ros::Time begin_time = // If no bags were loaded, we cannot peek the time of first message. diff --git a/cartographer_ros/cartographer_ros/playable_bag.cc b/cartographer_ros/cartographer_ros/playable_bag.cc index d9d17ed..b40ee11 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.cc +++ b/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( diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/cartographer_ros/playable_bag.h index c53f963..8526a86 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.h +++ b/cartographer_ros/cartographer_ros/playable_bag.h @@ -43,6 +43,7 @@ class PlayableBag { std::tuple GetBeginEndTime() const; int bag_id() const; + std::set topics() const { return topics_; } private: void AdvanceOneMessage(); @@ -59,6 +60,7 @@ class PlayableBag { std::deque buffered_messages_; const ::ros::Duration buffer_delay_; FilteringEarlyMessageHandler filtering_early_message_handler_; + std::set topics_; }; class PlayableBagMultiplexer { @@ -75,6 +77,8 @@ class PlayableBagMultiplexer { bool IsMessageAvailable() const; ros::Time PeekMessageTime() const; + std::set topics() const { return topics_; } + private: struct BagMessageItem { ros::Time message_timestamp; @@ -90,6 +94,7 @@ class PlayableBagMultiplexer { std::priority_queue, BagMessageItem::TimestampIsGreater> next_message_queue_; + std::set topics_; }; } // namespace cartographer_ros