test_publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <gtest/gtest.h>
00036 
00037 #include "rosrt/rosrt.h"
00038 
00039 #include <ros/ros.h>
00040 
00041 #include <std_msgs/UInt32.h>
00042 
00043 #include <boost/thread.hpp>
00044 
00045 using namespace rosrt;
00046 
00047 struct Helper
00048 {
00049   Helper()
00050   : count(0)
00051   {}
00052 
00053   void cb(const std_msgs::UInt32ConstPtr& msg)
00054   {
00055     latest = msg;
00056     ++count;
00057   }
00058 
00059   std_msgs::UInt32ConstPtr latest;
00060   uint32_t count;
00061 };
00062 
00063 TEST(Publisher, singlePublisher)
00064 {
00065   ros::NodeHandle nh;
00066 
00067   Publisher<std_msgs::UInt32> pub(nh.advertise<std_msgs::UInt32>("test", 0), 1, std_msgs::UInt32());
00068 
00069   Helper h;
00070   ros::Subscriber sub = nh.subscribe("test", 0, &Helper::cb, &h);
00071 
00072   resetThreadAllocInfo();
00073 
00074   std_msgs::UInt32Ptr msg = pub.allocate();
00075   msg->data = 5;
00076   pub.publish(msg);
00077 
00078   ASSERT_EQ(getThreadAllocInfo().total_ops, 0ULL);
00079 
00080   while (h.count == 0)
00081   {
00082     ros::WallDuration(0.001).sleep();
00083     ros::spinOnce();
00084   }
00085 
00086   ASSERT_EQ(h.count, 1UL);
00087   ASSERT_EQ(h.latest->data, 5UL);
00088 }
00089 
00090 TEST(Publisher, simpleInitializeCompile)
00091 {
00092   ros::NodeHandle nh;
00093   Publisher<std_msgs::UInt32> pub(nh, "test", 0, 1, std_msgs::UInt32());
00094 }
00095 
00096 TEST(Publisher, multiplePublishers)
00097 {
00098   ros::NodeHandle nh;
00099 
00100   static const uint32_t count = 100;
00101   Publisher<std_msgs::UInt32> pubs[count];
00102 
00103   Helper helpers[count];
00104   ros::Subscriber subs[count];
00105 
00106   for (uint32_t i = 0; i < count; ++i)
00107   {
00108     std::stringstream topic;
00109     topic << "test" << i;
00110     pubs[i].initialize(nh.advertise<std_msgs::UInt32>(topic.str(), 0), 100, std_msgs::UInt32());
00111     subs[i] = nh.subscribe(topic.str(), 0, &Helper::cb, &helpers[i]);
00112   }
00113 
00114   for (uint32_t j = 0; j < 100; ++j)
00115   {
00116     for (uint32_t i = 0; i < count; ++i)
00117     {
00118       std_msgs::UInt32Ptr msg = pubs[i].allocate();
00119       ASSERT_TRUE(msg);
00120       msg->data = j;
00121       ASSERT_TRUE(pubs[i].publish(msg));
00122     }
00123   }
00124 
00125   uint32_t recv_count = 0;
00126   while (recv_count < count * 100)
00127   {
00128     ros::spinOnce();
00129     ros::WallDuration(0.01).sleep();
00130 
00131     recv_count = 0;
00132 
00133     for (uint32_t i = 0; i < count; ++i)
00134     {
00135       recv_count += helpers[i].count;
00136     }
00137   }
00138 
00139   ASSERT_EQ(recv_count, count * 100);
00140 
00141   for (uint32_t i = 0; i < count; ++i)
00142   {
00143     ASSERT_EQ(helpers[i].latest->data, 99UL);
00144   }
00145 }
00146 
00147 void publishThread(Publisher<std_msgs::UInt32>& pub, bool& done)
00148 {
00149   while (!done)
00150   {
00151     std_msgs::UInt32Ptr msg = pub.allocate();
00152     if (msg)
00153     {
00154       pub.publish(msg);
00155     }
00156     else
00157     {
00158       ros::WallDuration(0.0001).sleep();
00159     }
00160   }
00161 }
00162 
00163 TEST(Publisher, multipleThreads)
00164 {
00165   ros::NodeHandle nh;
00166 
00167   static const uint32_t count = 10;
00168   Publisher<std_msgs::UInt32> pubs[count];
00169 
00170   Helper helpers[count];
00171   ros::Subscriber subs[count];
00172 
00173   boost::thread_group tg;
00174   bool done = false;
00175   for (uint32_t i = 0; i < count; ++i)
00176   {
00177     std::stringstream topic;
00178     topic << "test" << i;
00179     pubs[i].initialize(nh.advertise<std_msgs::UInt32>(topic.str(), 0), 100, std_msgs::UInt32());
00180     subs[i] = nh.subscribe(topic.str(), 0, &Helper::cb, &helpers[i]);
00181 
00182     tg.create_thread(boost::bind(publishThread, boost::ref(pubs[i]), boost::ref(done)));
00183   }
00184 
00185   uint32_t recv_count = 0;
00186   while (recv_count < count * 10000)
00187   {
00188     ros::spinOnce();
00189 
00190     recv_count = 0;
00191 
00192     for (uint32_t i = 0; i < count; ++i)
00193     {
00194       recv_count += helpers[i].count;
00195     }
00196 
00197     ros::WallDuration(0.01).sleep();
00198   }
00199 
00200   done = true;
00201   tg.join_all();
00202 
00203   ASSERT_GE(recv_count, count * 10000);
00204 
00205   for (uint32_t i = 0; i < count; ++i)
00206   {
00207     ASSERT_TRUE(helpers[i].latest);
00208   }
00209 }
00210 
00211 int main(int argc, char** argv)
00212 {
00213   ros::init(argc, argv, "test_rt_publisher");
00214   testing::InitGoogleTest(&argc, argv);
00215 
00216   ros::NodeHandle nh;
00217   rosrt::init();
00218 
00219   return RUN_ALL_TESTS();
00220 }


rosrt
Author(s): Josh Faust
autogenerated on Wed Aug 26 2015 16:00:02