test_publisher.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #include <gtest/gtest.h>
36 
37 #include "rosrt/rosrt.h"
38 
39 #include <ros/ros.h>
40 
41 #include <std_msgs/UInt32.h>
42 
43 #include <boost/thread.hpp>
44 
45 using namespace rosrt;
46 
47 struct Helper
48 {
50  : count(0)
51  {}
52 
53  void cb(const std_msgs::UInt32ConstPtr& msg)
54  {
55  latest = msg;
56  ++count;
57  }
58 
59  std_msgs::UInt32ConstPtr latest;
60  uint32_t count;
61 };
62 
63 TEST(Publisher, singlePublisher)
64 {
65  ros::NodeHandle nh;
66 
67  Publisher<std_msgs::UInt32> pub(nh.advertise<std_msgs::UInt32>("test", 0), 1, std_msgs::UInt32());
68 
69  Helper h;
70  ros::Subscriber sub = nh.subscribe("test", 0, &Helper::cb, &h);
71 
73 
74  std_msgs::UInt32Ptr msg = pub.allocate();
75  msg->data = 5;
76  pub.publish(msg);
77 
78  ASSERT_EQ(getThreadAllocInfo().total_ops, 0ULL);
79 
80  while (h.count == 0)
81  {
82  ros::WallDuration(0.001).sleep();
83  ros::spinOnce();
84  }
85 
86  ASSERT_EQ(h.count, 1UL);
87  ASSERT_EQ(h.latest->data, 5UL);
88 }
89 
90 TEST(Publisher, simpleInitializeCompile)
91 {
92  ros::NodeHandle nh;
93  Publisher<std_msgs::UInt32> pub(nh, "test", 0, 1, std_msgs::UInt32());
94 }
95 
96 TEST(Publisher, multiplePublishers)
97 {
98  ros::NodeHandle nh;
99 
100  static const uint32_t count = 100;
101  Publisher<std_msgs::UInt32> pubs[count];
102 
103  Helper helpers[count];
104  ros::Subscriber subs[count];
105 
106  for (uint32_t i = 0; i < count; ++i)
107  {
108  std::stringstream topic;
109  topic << "test" << i;
110  pubs[i].initialize(nh.advertise<std_msgs::UInt32>(topic.str(), 0), 100, std_msgs::UInt32());
111  subs[i] = nh.subscribe(topic.str(), 0, &Helper::cb, &helpers[i]);
112  }
113 
114  for (uint32_t j = 0; j < 100; ++j)
115  {
116  for (uint32_t i = 0; i < count; ++i)
117  {
118  std_msgs::UInt32Ptr msg = pubs[i].allocate();
119  ASSERT_TRUE(msg);
120  msg->data = j;
121  ASSERT_TRUE(pubs[i].publish(msg));
122  }
123  }
124 
125  uint32_t recv_count = 0;
126  while (recv_count < count * 100)
127  {
128  ros::spinOnce();
129  ros::WallDuration(0.01).sleep();
130 
131  recv_count = 0;
132 
133  for (uint32_t i = 0; i < count; ++i)
134  {
135  recv_count += helpers[i].count;
136  }
137  }
138 
139  ASSERT_EQ(recv_count, count * 100);
140 
141  for (uint32_t i = 0; i < count; ++i)
142  {
143  ASSERT_EQ(helpers[i].latest->data, 99UL);
144  }
145 }
146 
148 {
149  while (!done)
150  {
151  std_msgs::UInt32Ptr msg = pub.allocate();
152  if (msg)
153  {
154  pub.publish(msg);
155  }
156  else
157  {
158  ros::WallDuration(0.0001).sleep();
159  }
160  }
161 }
162 
163 TEST(Publisher, multipleThreads)
164 {
165  ros::NodeHandle nh;
166 
167  static const uint32_t count = 10;
168  Publisher<std_msgs::UInt32> pubs[count];
169 
170  Helper helpers[count];
171  ros::Subscriber subs[count];
172 
173  boost::thread_group tg;
174  bool done = false;
175  for (uint32_t i = 0; i < count; ++i)
176  {
177  std::stringstream topic;
178  topic << "test" << i;
179  pubs[i].initialize(nh.advertise<std_msgs::UInt32>(topic.str(), 0), 100, std_msgs::UInt32());
180  subs[i] = nh.subscribe(topic.str(), 0, &Helper::cb, &helpers[i]);
181 
182  tg.create_thread(boost::bind(publishThread, boost::ref(pubs[i]), boost::ref(done)));
183  }
184 
185  uint32_t recv_count = 0;
186  while (recv_count < count * 10000)
187  {
188  ros::spinOnce();
189 
190  recv_count = 0;
191 
192  for (uint32_t i = 0; i < count; ++i)
193  {
194  recv_count += helpers[i].count;
195  }
196 
197  ros::WallDuration(0.01).sleep();
198  }
199 
200  done = true;
201  tg.join_all();
202 
203  ASSERT_GE(recv_count, count * 10000);
204 
205  for (uint32_t i = 0; i < count; ++i)
206  {
207  ASSERT_TRUE(helpers[i].latest);
208  }
209 }
210 
211 int main(int argc, char** argv)
212 {
213  ros::init(argc, argv, "test_rt_publisher");
214  testing::InitGoogleTest(&argc, argv);
215 
216  ros::NodeHandle nh;
217  rosrt::init();
218 
219  return RUN_ALL_TESTS();
220 }
AllocInfo getThreadAllocInfo()
Definition: malloc.cpp:142
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Definition: managers.h:38
void publishThread(Publisher< std_msgs::UInt32 > &pub, bool &done)
void init(const InitOptions &ops=InitOptions())
Definition: init.cpp:103
int main(int argc, char **argv)
uint32_t count
bool sleep() const
a realtime-safe ROS publisher
Definition: publisher.h:83
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void cb(const std_msgs::UInt32ConstPtr &msg)
bool publish(const ros::Publisher &pub, const VoidConstPtr &msg, PublishFunc pub_func, CloneFunc clone_func)
Definition: publisher.cpp:50
void resetThreadAllocInfo()
Definition: malloc.cpp:165
std_msgs::UInt32ConstPtr latest
void initialize(const ros::Publisher &pub, uint32_t message_pool_size, const M &tmpl)
initialization function. Only use with the default constructor
Definition: publisher.h:133
ROSCPP_DECL void spinOnce()
boost::shared_ptr< M > allocate()
Allocate a message. The message will have been constructed with the template provided to initialize()...
Definition: publisher.h:165
bool publish(const MConstPtr &msg)
Publish a message.
Definition: publisher.h:156
TEST(Publisher, singlePublisher)


rosrt
Author(s): Josh Faust
autogenerated on Fri Apr 5 2019 02:16:39