create_and_iterate_bag.cpp
Go to the documentation of this file.
1 #include "ros/time.h"
2 #include "rosbag/bag.h"
3 #include "rosbag/view.h"
4 #include "std_msgs/Int32.h"
5 #include "std_msgs/String.h"
6 
7 #include <string>
8 #include <vector>
9 
10 #include "boost/foreach.hpp"
11 #include <gtest/gtest.h>
12 
13 void create_test_bag(const std::string &filename)
14 {
15  rosbag::Bag bag;
16  bag.open(filename, rosbag::bagmode::Write);
17 
18  std_msgs::String str;
19  str.data = std::string("foo");
20 
21  std_msgs::Int32 i;
22  i.data = 42;
23 
24  bag.write("chatter", ros::Time::now(), str);
25  bag.write("numbers", ros::Time::now(), i);
26 
27  bag.close();
28 }
29 
30 const char* bag_filename = "/tmp/rosbag_storage_create_and_iterate_bag.bag";
31 
32 TEST(rosbag_storage, iterator_copy_constructor)
33 {
34  // copy ctor
35  rosbag::Bag bag;
37  rosbag::View view(bag, rosbag::TopicQuery("numbers"));
39  EXPECT_EQ(42, it0->instantiate<std_msgs::Int32>()->data);
41  EXPECT_EQ(it0, it1);
42  EXPECT_EQ(42, it1->instantiate<std_msgs::Int32>()->data);
43  ++it1;
44  EXPECT_NE(it0, it1);
45  EXPECT_EQ(42, it0->instantiate<std_msgs::Int32>()->data);
46 }
47 
48 TEST(rosbag_storage, iterator_copy_assignment)
49 {
50  // copy assignment
51  rosbag::Bag bag;
53  rosbag::View view(bag, rosbag::TopicQuery("numbers"));
55  EXPECT_EQ(42, it0->instantiate<std_msgs::Int32>()->data);
57  it1 = it0;
58  EXPECT_EQ(it0, it1);
59  EXPECT_EQ(42, it1->instantiate<std_msgs::Int32>()->data);
60  ++it1;
61  EXPECT_NE(it0, it1);
62  EXPECT_EQ(42, it0->instantiate<std_msgs::Int32>()->data);
63 }
64 
65 TEST(rosbag_storage, iterate_bag)
66 {
67  rosbag::Bag bag;
69 
70  std::vector<std::string> topics;
71  topics.push_back(std::string("chatter"));
72  topics.push_back(std::string("numbers"));
73 
74  rosbag::View view(bag, rosbag::TopicQuery(topics));
75 
76  BOOST_FOREACH(rosbag::MessageInstance const m, view)
77  {
78  std_msgs::String::ConstPtr s = m.instantiate<std_msgs::String>();
79  if (s != NULL)
80  {
81  if(s->data == std::string("foo")) {
82  printf("Successfully checked string foo\n");
83  }
84  else
85  {
86  printf("Failed checked string foo\n");
87  FAIL();
88  }
89  }
90 
91  std_msgs::Int32::ConstPtr i = m.instantiate<std_msgs::Int32>();
92  if (i != NULL)
93  {
94  if (i->data == 42) {
95  printf("Successfully checked value 42\n");
96  }
97  else
98  {
99  printf("Failed checked value 42.\n");
100  FAIL();
101  }
102  }
103  }
104 
105  bag.close();
106 }
107 
108 int main(int argc, char **argv) {
109  ros::Time::init();
111 
112  testing::InitGoogleTest(&argc, argv);
113  return RUN_ALL_TESTS();
114 }
void create_test_bag(const std::string &filename)
void open(std::string const &filename, uint32_t mode=bagmode::Read)
void close()
const char * bag_filename
int main(int argc, char **argv)
static void init()
boost::shared_ptr< T > instantiate() const
iterator const_iterator
TEST(rosbag_storage, iterator_copy_constructor)
iterator begin()
static Time now()
void write(std::string const &topic, ros::MessageEvent< T > const &event)


test_rosbag_storage
Author(s): Dirk Thomas
autogenerated on Mon Nov 2 2020 03:52:23