From a95d25e65374a6a2bac496db802cb97b957ead73 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20BARTH=C3=89L=C3=89MY?= Date: Fri, 17 Mar 2017 23:35:24 +0100 Subject: [PATCH] fix rosbag::View::iterator copy assignment operator (#1017) * refactor test_rosbag_storage * fix rosbag::View::iterator copy assignment operator the compiler-generated copy assignment operator did lead to segfault and memory leaks. --- .../src/create_and_iterate_bag.cpp | 127 +++++++++++------- tools/rosbag_storage/include/rosbag/view.h | 1 + tools/rosbag_storage/src/view.cpp | 13 ++ 3 files changed, 94 insertions(+), 47 deletions(-) diff --git a/test/test_rosbag_storage/src/create_and_iterate_bag.cpp b/test/test_rosbag_storage/src/create_and_iterate_bag.cpp index 37d08525c2..23c91dcd86 100644 --- a/test/test_rosbag_storage/src/create_and_iterate_bag.cpp +++ b/test/test_rosbag_storage/src/create_and_iterate_bag.cpp @@ -10,71 +10,104 @@ #include "boost/foreach.hpp" #include +void create_test_bag(const std::string &filename) +{ + rosbag::Bag bag; + bag.open(filename, rosbag::bagmode::Write); + + std_msgs::String str; + str.data = std::string("foo"); + + std_msgs::Int32 i; + i.data = 42; + + bag.write("chatter", ros::Time::now(), str); + bag.write("numbers", ros::Time::now(), i); + + bag.close(); +} -TEST(rosbag_storage, create_and_iterate_bag) +const char* bag_filename = "/tmp/rosbag_storage_create_and_iterate_bag.bag"; + +TEST(rosbag_storage, iterator_copy_constructor) { - const char* bag_filename = "/tmp/rosbag_storage_create_and_iterate_bag.bag"; - { - rosbag::Bag bag; - bag.open(bag_filename, rosbag::bagmode::Write); - - std_msgs::String str; - str.data = std::string("foo"); - - std_msgs::Int32 i; - i.data = 42; - - bag.write("chatter", ros::Time::now(), str); - bag.write("numbers", ros::Time::now(), i); - - bag.close(); - } + // copy ctor + rosbag::Bag bag; + bag.open(bag_filename, rosbag::bagmode::Read); + rosbag::View view(bag, rosbag::TopicQuery("numbers")); + rosbag::View::const_iterator it0 = view.begin(); + EXPECT_EQ(42, it0->instantiate()->data); + rosbag::View::const_iterator it1(it0); + EXPECT_EQ(it0, it1); + EXPECT_EQ(42, it1->instantiate()->data); + ++it1; + EXPECT_NE(it0, it1); + EXPECT_EQ(42, it0->instantiate()->data); +} - { - rosbag::Bag bag; - bag.open(bag_filename, rosbag::bagmode::Read); +TEST(rosbag_storage, iterator_copy_assignment) +{ + // copy assignment + rosbag::Bag bag; + bag.open(bag_filename, rosbag::bagmode::Read); + rosbag::View view(bag, rosbag::TopicQuery("numbers")); + rosbag::View::const_iterator it0 = view.begin(); + EXPECT_EQ(42, it0->instantiate()->data); + rosbag::View::const_iterator it1; + it1 = it0; + EXPECT_EQ(it0, it1); + EXPECT_EQ(42, it1->instantiate()->data); + ++it1; + EXPECT_NE(it0, it1); + EXPECT_EQ(42, it0->instantiate()->data); +} + +TEST(rosbag_storage, iterate_bag) +{ + rosbag::Bag bag; + bag.open(bag_filename, rosbag::bagmode::Read); - std::vector topics; - topics.push_back(std::string("chatter")); - topics.push_back(std::string("numbers")); + std::vector topics; + topics.push_back(std::string("chatter")); + topics.push_back(std::string("numbers")); - rosbag::View view(bag, rosbag::TopicQuery(topics)); + rosbag::View view(bag, rosbag::TopicQuery(topics)); - BOOST_FOREACH(rosbag::MessageInstance const m, view) + BOOST_FOREACH(rosbag::MessageInstance const m, view) + { + std_msgs::String::ConstPtr s = m.instantiate(); + if (s != NULL) { - std_msgs::String::ConstPtr s = m.instantiate(); - if (s != NULL) + if(s->data == std::string("foo")) { + printf("Successfully checked string foo\n"); + } + else { - if(s->data == std::string("foo")) { - printf("Successfully checked string foo\n"); - } - else - { - printf("Failed checked string foo\n"); - FAIL(); - } + printf("Failed checked string foo\n"); + FAIL(); } + } - std_msgs::Int32::ConstPtr i = m.instantiate(); - if (i != NULL) + std_msgs::Int32::ConstPtr i = m.instantiate(); + if (i != NULL) + { + if (i->data == 42) { + printf("Successfully checked value 42\n"); + } + else { - if (i->data == 42) { - printf("Successfully checked value 42\n"); - } - else - { - printf("Failed checked value 42.\n"); - FAIL(); - } + printf("Failed checked value 42.\n"); + FAIL(); } } - - bag.close(); } + + bag.close(); } int main(int argc, char **argv) { ros::Time::init(); + create_test_bag(bag_filename); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/tools/rosbag_storage/include/rosbag/view.h b/tools/rosbag_storage/include/rosbag/view.h index bce5676fea..ea9048adb9 100644 --- a/tools/rosbag_storage/include/rosbag/view.h +++ b/tools/rosbag_storage/include/rosbag/view.h @@ -63,6 +63,7 @@ class ROSBAG_DECL View { public: iterator(iterator const& i); + iterator &operator=(iterator const& i); iterator(); ~iterator(); diff --git a/tools/rosbag_storage/src/view.cpp b/tools/rosbag_storage/src/view.cpp index 7448c861a3..b388b4c1a4 100644 --- a/tools/rosbag_storage/src/view.cpp +++ b/tools/rosbag_storage/src/view.cpp @@ -59,6 +59,19 @@ View::iterator::iterator(View* view, bool end) : view_(view), view_revision_(0), View::iterator::iterator(const iterator& i) : view_(i.view_), iters_(i.iters_), view_revision_(i.view_revision_), message_instance_(NULL) { } +View::iterator &View::iterator::operator=(iterator const& i) { + if (this != &i) { + view_ = i.view_; + iters_ = i.iters_; + view_revision_ = i.view_revision_; + if (message_instance_ != NULL) { + delete message_instance_; + message_instance_ = NULL; + } + } + return *this; +} + void View::iterator::populate() { assert(view_ != NULL);