From 58eea8202c3eed602cd26ae91b4c90dbe284286d Mon Sep 17 00:00:00 2001 From: Daniel feldman Date: Tue, 16 Nov 2021 13:50:51 -0800 Subject: [PATCH 1/5] Apply cleanly the setquery patch --- lib/ros_value.h | 11 +++++++++++ lib/view.cc | 25 +++++++++++++++++++++++++ lib/view.h | 3 +++ 3 files changed, 39 insertions(+) diff --git a/lib/ros_value.h b/lib/ros_value.h index 6d00348..e9c2bfb 100644 --- a/lib/ros_value.h +++ b/lib/ros_value.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -356,6 +357,16 @@ class RosValue { friend class MessageParser; }; +template +bool operator<(const RosValue::ros_time_t& lhs, const std::chrono::duration& rhs) { + return std::chrono::nanoseconds{lhs.to_nsec()} < rhs; +} + +template +bool operator>(const RosValue::ros_time_t& lhs, const std::chrono::duration& rhs) { + return std::chrono::nanoseconds{lhs.to_nsec()} > rhs; +} + template<> const std::string RosValue::as() const; diff --git a/lib/view.cc b/lib/view.cc index fcdd547..f67e935 100644 --- a/lib/view.cc +++ b/lib/view.cc @@ -200,6 +200,31 @@ View View::getMessages(std::initializer_list topics) { return getMessages(std::vector(topics.begin(), topics.end())); } +void View::setQuery(const std::vector &topics, std::chrono::nanoseconds start_time, std::chrono::nanoseconds end_time) { + bag_wrappers_.clear(); + + for (const auto& bag : bags_) { + bag_wrappers_[bag] = std::make_shared(); + bag_wrappers_[bag]->bag = bag; + + for (const auto &topic : topics) { + if (!bag->topic_connection_map_.count(topic)) { + continue; + } + + for (const auto &connection_record : bag->topic_connection_map_.at(topic)) { + for (const auto &block : connection_record->blocks) { + if (block.into_chunk->info.end_time > start_time && block.into_chunk->info.start_time < end_time) { + bag_wrappers_[bag]->chunks_to_parse.emplace(block.into_chunk); + } + } + + bag_wrappers_[bag]->connection_ids.emplace(connection_record->id); + } + } + } +} + RosValue::ros_time_t View::getStartTime() { RosValue::ros_time_t start_time; start_time.secs = UINT32_MAX; diff --git a/lib/view.h b/lib/view.h index 92c0fb1..f7025d7 100644 --- a/lib/view.h +++ b/lib/view.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -121,6 +122,7 @@ class View { View getMessages(const std::string &topic); View getMessages(const std::vector &topics); View getMessages(std::initializer_list topics); + void setQuery(const std::vector &topics, std::chrono::nanoseconds start_time, std::chrono::nanoseconds end_time); RosValue::ros_time_t getStartTime(); RosValue::ros_time_t getEndTime(); @@ -159,6 +161,7 @@ class View { } private: + std::chrono::nanoseconds start_time_, end_time_; std::vector> bags_; std::unordered_map, std::shared_ptr> bag_wrappers_; }; From b5aa79b948aa984446370fa4eb0e143051bc2167 Mon Sep 17 00:00:00 2001 From: Daniel feldman Date: Tue, 16 Nov 2021 21:21:36 -0800 Subject: [PATCH 2/5] fix test --- lib/view.cc | 36 +++++++++--------------------------- lib/view.h | 2 ++ test/embag_test.cc | 33 +++++++++++++++++++++++++++++---- 3 files changed, 40 insertions(+), 31 deletions(-) diff --git a/lib/view.cc b/lib/view.cc index f67e935..086804c 100644 --- a/lib/view.cc +++ b/lib/view.cc @@ -172,6 +172,12 @@ View View::getMessages(const std::string &topic) { } View View::getMessages(const std::vector &topics) { + std::chrono::nanoseconds start_time_ns {getStartTime().to_nsec()}; + std::chrono::nanoseconds end_time_ns {getEndTime().to_nsec()}; + return getMessages(topics, start_time_ns, end_time_ns); +} + +View View::getMessages(const std::vector &topics, std::chrono::nanoseconds start_time, std::chrono::nanoseconds end_time) { bag_wrappers_.clear(); for (const auto& bag : bags_) { @@ -185,9 +191,10 @@ View View::getMessages(const std::vector &topics) { for (const auto &connection_record : bag->topic_connection_map_.at(topic)) { for (const auto &block : connection_record->blocks) { - bag_wrappers_[bag]->chunks_to_parse.emplace(block.into_chunk); + if (block.into_chunk->info.end_time > start_time && block.into_chunk->info.start_time < end_time) { + bag_wrappers_[bag]->chunks_to_parse.emplace(block.into_chunk); + } } - bag_wrappers_[bag]->connection_ids.emplace(connection_record->id); } } @@ -200,31 +207,6 @@ View View::getMessages(std::initializer_list topics) { return getMessages(std::vector(topics.begin(), topics.end())); } -void View::setQuery(const std::vector &topics, std::chrono::nanoseconds start_time, std::chrono::nanoseconds end_time) { - bag_wrappers_.clear(); - - for (const auto& bag : bags_) { - bag_wrappers_[bag] = std::make_shared(); - bag_wrappers_[bag]->bag = bag; - - for (const auto &topic : topics) { - if (!bag->topic_connection_map_.count(topic)) { - continue; - } - - for (const auto &connection_record : bag->topic_connection_map_.at(topic)) { - for (const auto &block : connection_record->blocks) { - if (block.into_chunk->info.end_time > start_time && block.into_chunk->info.start_time < end_time) { - bag_wrappers_[bag]->chunks_to_parse.emplace(block.into_chunk); - } - } - - bag_wrappers_[bag]->connection_ids.emplace(connection_record->id); - } - } - } -} - RosValue::ros_time_t View::getStartTime() { RosValue::ros_time_t start_time; start_time.secs = UINT32_MAX; diff --git a/lib/view.h b/lib/view.h index f7025d7..1327d21 100644 --- a/lib/view.h +++ b/lib/view.h @@ -122,6 +122,8 @@ class View { View getMessages(const std::string &topic); View getMessages(const std::vector &topics); View getMessages(std::initializer_list topics); + View getMessages(const std::vector &topics, std::chrono::nanoseconds start_time, std::chrono::nanoseconds end_time); + void setQuery(const std::vector &topics, std::chrono::nanoseconds start_time, std::chrono::nanoseconds end_time); RosValue::ros_time_t getStartTime(); RosValue::ros_time_t getEndTime(); diff --git a/test/embag_test.cc b/test/embag_test.cc index 31e167a..34b3eb1 100644 --- a/test/embag_test.cc +++ b/test/embag_test.cc @@ -135,11 +135,11 @@ class ViewTest : public ::testing::Test { TEST_F(ViewTest, View) { - const Embag::RosValue::ros_time_t start_time{1604515190, 231374463}; - const Embag::RosValue::ros_time_t end_time{1604515197, 820012098}; + const Embag::RosValue::ros_time_t start_time_{1604515190, 231374463}; + const Embag::RosValue::ros_time_t end_time_{1604515197, 820012098}; - ASSERT_EQ(view_.getStartTime(), start_time); - ASSERT_EQ(view_.getEndTime(), end_time); + ASSERT_EQ(view_.getStartTime(), start_time_); + ASSERT_EQ(view_.getEndTime(), end_time_); const auto topics = view_.topics(); const auto topic_set = std::unordered_set(topics.begin(), topics.end()); @@ -227,6 +227,31 @@ TEST_F(ViewTest, MessagesForTopic) { } } +TEST_F(ViewTest, MessagesBetweenTimestamps) { + std::chrono::nanoseconds start_time_ns {view_.getStartTime().to_nsec()}; + std::chrono::nanoseconds end_time_ns {view_.getEndTime().to_nsec()}; + start_time_ns += std::chrono::seconds{1}; + + std::vector all_topics{ + "/base_pose_ground_truth", + "/base_scan", + "/luminar_pointcloud", + }; + + int no_time_boundaries_count = 0; + for (const auto &message : view_.getMessages(all_topics)){ + no_time_boundaries_count++; + } + + int with_time_boundaries_count = 0; + for (const auto &message : view_.getMessages(all_topics, start_time_ns, end_time_ns)) { + with_time_boundaries_count++; + } + +ASSERT_GT(no_time_boundaries_count, with_time_boundaries_count); +} + + class StreamTest : public ::testing::Test { protected: std::string bag_path_ = "test/test.bag"; From 748c991be0cab81696d865d78c4989dc97a6e696 Mon Sep 17 00:00:00 2001 From: daniel feldman <61597775+def-ai@users.noreply.github.com> Date: Wed, 17 Nov 2021 09:37:14 -0800 Subject: [PATCH 3/5] remove useless _ --- test/embag_test.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/embag_test.cc b/test/embag_test.cc index 34b3eb1..261caec 100644 --- a/test/embag_test.cc +++ b/test/embag_test.cc @@ -135,11 +135,11 @@ class ViewTest : public ::testing::Test { TEST_F(ViewTest, View) { - const Embag::RosValue::ros_time_t start_time_{1604515190, 231374463}; - const Embag::RosValue::ros_time_t end_time_{1604515197, 820012098}; + const Embag::RosValue::ros_time_t start_time{1604515190, 231374463}; + const Embag::RosValue::ros_time_t end_time{1604515197, 820012098}; - ASSERT_EQ(view_.getStartTime(), start_time_); - ASSERT_EQ(view_.getEndTime(), end_time_); + ASSERT_EQ(view_.getStartTime(), start_time); + ASSERT_EQ(view_.getEndTime(), end_time); const auto topics = view_.topics(); const auto topic_set = std::unordered_set(topics.begin(), topics.end()); From 87f7e9fb11e7e36700f403c153835694a4ec9fd5 Mon Sep 17 00:00:00 2001 From: daniel feldman <61597775+def-ai@users.noreply.github.com> Date: Wed, 17 Nov 2021 13:08:09 -0800 Subject: [PATCH 4/5] Update view.h --- lib/view.h | 1 - 1 file changed, 1 deletion(-) diff --git a/lib/view.h b/lib/view.h index 1327d21..0278f4b 100644 --- a/lib/view.h +++ b/lib/view.h @@ -163,7 +163,6 @@ class View { } private: - std::chrono::nanoseconds start_time_, end_time_; std::vector> bags_; std::unordered_map, std::shared_ptr> bag_wrappers_; }; From 4530ad911e9f8860409dd6fe6736567caa03bfce Mon Sep 17 00:00:00 2001 From: daniel feldman <61597775+def-ai@users.noreply.github.com> Date: Wed, 17 Nov 2021 13:28:19 -0800 Subject: [PATCH 5/5] remove old setquery --- lib/view.h | 1 - 1 file changed, 1 deletion(-) diff --git a/lib/view.h b/lib/view.h index 0278f4b..2f56893 100644 --- a/lib/view.h +++ b/lib/view.h @@ -124,7 +124,6 @@ class View { View getMessages(std::initializer_list topics); View getMessages(const std::vector &topics, std::chrono::nanoseconds start_time, std::chrono::nanoseconds end_time); - void setQuery(const std::vector &topics, std::chrono::nanoseconds start_time, std::chrono::nanoseconds end_time); RosValue::ros_time_t getStartTime(); RosValue::ros_time_t getEndTime();