diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index 389753b..7709c50 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -60,6 +60,9 @@ DEFINE_bool(load_frozen_state, true, DEFINE_bool(keep_running, false, "Keep running the offline node after all messages from the bag " "have been processed."); +DEFINE_double(skip_seconds, 0, + "Optional amount of seconds to skip from the beginning " + "(i.e. when the earliest bag starts.). "); namespace cartographer_ros { @@ -233,12 +236,21 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { // TODO(gaschler): Warn if resolved topics are not in bags. std::unordered_map bag_index_to_trajectory_id; + const ros::Time begin_time = playable_bag_multiplexer.PeekMessageTime(); while (playable_bag_multiplexer.IsMessageAvailable()) { + if (!::ros::ok()) { + return; + } + const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage(); const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple); const int bag_index = std::get<1>(next_msg_tuple); const bool is_last_message_in_bag = std::get<2>(next_msg_tuple); + if (msg.getTime() < (begin_time + ros::Duration(FLAGS_skip_seconds))) { + continue; + } + int trajectory_id; // Lazily add trajectories only when the first message arrives in order // to avoid blocking the sensor queue. @@ -257,9 +269,6 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { trajectory_id = bag_index_to_trajectory_id.at(bag_index); } - if (!::ros::ok()) { - return; - } const auto bag_topic = std::make_pair( bag_index, node.node_handle()->resolveName(msg.getTopic(), false /* resolve */)); diff --git a/cartographer_ros/cartographer_ros/playable_bag.cc b/cartographer_ros/cartographer_ros/playable_bag.cc index 820706f..d9d17ed 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.cc +++ b/cartographer_ros/cartographer_ros/playable_bag.cc @@ -45,12 +45,12 @@ PlayableBag::PlayableBag( AdvanceUntilMessageAvailable(); } -ros::Time PlayableBag::PeekMessageTime() { +ros::Time PlayableBag::PeekMessageTime() const { CHECK(IsMessageAvailable()); return buffered_messages_.front().getTime(); } -std::tuple PlayableBag::GetBeginEndTime() { +std::tuple PlayableBag::GetBeginEndTime() const { return std::make_tuple(view_->getBeginTime(), view_->getEndTime()); } @@ -67,13 +67,13 @@ rosbag::MessageInstance PlayableBag::GetNextMessage() { return msg; } -bool PlayableBag::IsMessageAvailable() { +bool PlayableBag::IsMessageAvailable() const { return !buffered_messages_.empty() && (buffered_messages_.front().getTime() < buffered_messages_.back().getTime() - buffer_delay_); } -int PlayableBag::bag_id() { return bag_id_; } +int PlayableBag::bag_id() const { return bag_id_; } void PlayableBag::AdvanceOneMessage() { CHECK(!finished_); @@ -106,7 +106,7 @@ void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) { static_cast(playable_bags_.size() - 1)}); } -bool PlayableBagMultiplexer::IsMessageAvailable() { +bool PlayableBagMultiplexer::IsMessageAvailable() const { return !next_message_queue_.empty(); } @@ -126,4 +126,9 @@ PlayableBagMultiplexer::GetNextMessage() { !current_bag.IsMessageAvailable()); } +ros::Time PlayableBagMultiplexer::PeekMessageTime() const { + CHECK(IsMessageAvailable()); + return next_message_queue_.top().message_timestamp; +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/cartographer_ros/playable_bag.h index e2a3865..423f141 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.h +++ b/cartographer_ros/cartographer_ros/playable_bag.h @@ -37,12 +37,12 @@ class PlayableBag { ros::Time end_time, ros::Duration buffer_delay, FilteringEarlyMessageHandler filtering_early_message_handler); - ros::Time PeekMessageTime(); + ros::Time PeekMessageTime() const; rosbag::MessageInstance GetNextMessage(); - bool IsMessageAvailable(); - std::tuple GetBeginEndTime(); + bool IsMessageAvailable() const; + std::tuple GetBeginEndTime() const; - int bag_id(); + int bag_id() const; private: void AdvanceOneMessage(); @@ -72,7 +72,8 @@ class PlayableBagMultiplexer { bool /* is_last_message_in_bag */> GetNextMessage(); - bool IsMessageAvailable(); + bool IsMessageAvailable() const; + ros::Time PeekMessageTime() const; private: struct BagMessageItem {