tester.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions are met:
00006  *   * Redistributions of source code must retain the above copyright notice,
00007  *     this list of conditions and the following disclaimer.
00008  *   * Redistributions in binary form must reproduce the above copyright
00009  *     notice, this list of conditions and the following disclaimer in the
00010  *     documentation and/or other materials provided with the distribution.
00011  *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
00012  *     contributors may be used to endorse or promote products derived from
00013  *     this software without specific prior written permission.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  */
00027 
00028 #include "ros/ros.h"
00029 #include "std_msgs/String.h"
00030 #include "std_msgs/Int16.h"
00031 #include "test_roslisp/ComplexMessage.h"
00032 #include <gtest/gtest.h>
00033 #include <ros/time.h>
00034 #include <ros/console.h>
00035 
00036 using std::vector;
00037 
00038 int num_messages_received=0;
00039 int service_client_status=0;
00040 int service_client_response=0;
00041 
00042 using test_roslisp::ComplexMessage;
00043 using test_roslisp::Foo;
00044 using test_roslisp::Bar;
00045 
00046 inline bool operator== (const Bar& b, const Bar& b2)
00047 {
00048   return ((b.y == b2.y) && (b.x==b2.x));
00049 }
00050 
00051 inline bool operator== (const Foo& f, const Foo& f2)
00052 {
00053   return ((f.y==f2.y) && (f.z[0]==f2.z[0]) && (f.z[1]==f2.z[1]));
00054 }
00055 
00056 inline
00057 bool operator== (const ComplexMessage& c, const ComplexMessage& c2)
00058 {
00059   unsigned i=0;
00060   
00061   if (c.ind != c2.ind)
00062     return false;
00063   
00064   for ( i=0; i<4; i++)
00065     if (!(c.x[i] == c2.x[i]))
00066       return false;
00067   
00068   for ( i=0; i<c.y.size(); i++)
00069     if (!(c.y[i] == c2.y[i]))
00070       return false;
00071   
00072   for ( i=0; i<c2.z.size(); i++) 
00073     if (!(c.z[i] == c2.z[i]))
00074       return false;
00075 
00076   for ( i=0; i<c.w.size(); i++)
00077     if (!(c.w[i].data == c2.w[i].data))
00078       return false;
00079 
00080   if (c.qux != c2.qux)
00081   {
00082     ROS_INFO ("Qux 1 is %u and qux 2 is %u", c.qux, c2.qux);
00083     return false;
00084   }
00085   
00086   return true;
00087 }
00088 
00089 class ComplexTester
00090 {
00091   // complex talker in lisp sends messages on Ping with ind from 0-7.
00092   // ComplexTester stores msg in vector and republishes on pong
00093   // complex talker in lisp echos pong messages on pang
00094   // ComplexTester counts messages on pang, makes sure they are == equal to vector
00095 public:
00096   ComplexTester() : ok_(true), num_received_(0)
00097   {
00098     ping_sub_ = nh_.subscribe("ping", 1000, &ComplexTester::pingCallback, this);
00099     pong_pub_ = nh_.advertise<ComplexMessage>("pong", 1000);
00100     pang_sub_ = nh_.subscribe("pang", 1000, &ComplexTester::pangCallback, this);
00101     ros::Duration d(3);
00102     d.sleep();
00103     ping_vec_.resize(8);
00104   }
00105   
00106   void pingCallback (const test_roslisp::ComplexMessage::ConstPtr& msg)
00107   {
00108     ping_vec_[msg->ind] = *msg;
00109     pong_pub_.publish(*msg);
00110   }
00111 
00112   void pangCallback (const test_roslisp::ComplexMessage::ConstPtr& msg)
00113   {
00114     ok_ &= (ping_vec_[msg->ind] == *msg);
00115     num_received_++;
00116   }
00117   
00118   bool done()
00119   {
00120     return (num_received_ >= 10);
00121   }
00122 
00123   bool isOk()
00124   {
00125     return ok_;
00126   }
00127   
00128 private:
00129   
00130   vector<ComplexMessage> ping_vec_;
00131   bool ok_;
00132   unsigned num_received_;
00133   ros::Subscriber pang_sub_, ping_sub_;
00134   ros::Publisher pong_pub_;
00135   ros::NodeHandle nh_;
00136 };  
00137 
00138 
00139 
00140 
00141 TEST(Roslisp, ComplexTalkerListener)
00142 {
00143   // checks complex message gets through topics
00144   ros::Duration d(1);
00145   unsigned wait_max = 180;
00146 
00147   ComplexTester t;
00148   
00149   for (unsigned i=0; (i<wait_max) && (!t.done()); i++) {
00150     d.sleep();
00151     ros::spinOnce();
00152   }
00153 
00154   EXPECT_TRUE(t.done());
00155   EXPECT_TRUE(t.isOk());
00156 }
00157 
00158 
00159 
00164 void chatterCallback(const std_msgs::StringConstPtr& msg)
00165 {
00166   num_messages_received++;
00167 }
00168 
00169 
00170 TEST(Roslisp, TalkerListener)
00171 {
00172   // checks > 5 messages received that went from lisp through lisp
00173   ros::NodeHandle n;
00174   ros::Subscriber sub = n.subscribe("chatter_echo", 1000, chatterCallback);
00175   ros::Duration d(1);
00176 
00177   int wait_max=180;
00178 
00179   for (int i=0; i<wait_max && n.ok(); i++) {
00180     d.sleep();
00181     ros::spinOnce();
00182     ROS_INFO_STREAM ("Have waited " << i << " out of " << wait_max << " seconds to receive 5 messages from chatter_echo");
00183     if (num_messages_received>5)
00184       break;
00185   }
00186   EXPECT_TRUE(num_messages_received>5);
00187   ROS_INFO_STREAM ("Successfully received " << num_messages_received << " echoed messages");
00188 }
00189 
00190 TEST(Roslisp, BrokenTalkerListener)
00191 {
00192   // checks error state in node does not kill node 
00193   ros::NodeHandle n;
00194   ros::Publisher chatter_pub = n.advertise<std_msgs::String>("error_chatter", 1000);
00195   ros::Subscriber sub = n.subscribe("error_chatter_echo", 1000, chatterCallback);
00196   ros::Duration d(1);
00197 
00198   std_msgs::String msg;
00199   std::stringstream ss;
00200   ss << "error"; // should trigger error in subscriber callback
00201   msg.data = ss.str();
00202   chatter_pub.publish(msg);
00203   ros::spinOnce();
00204   d.sleep();
00205   
00206   int wait_max=20;
00207   num_messages_received = 0;
00208   for (int i=0; i<wait_max && n.ok(); i++) {
00209     ss << i;
00210     msg.data = ss.str();
00211     chatter_pub.publish(msg);
00212     d.sleep();
00213     ros::spinOnce();
00214     ROS_INFO_STREAM ("Have waited " << i << " out of " << wait_max << " seconds to receive 1 message from chatter_echo");
00215     if (num_messages_received > 0)
00216       break;
00217   }
00218   EXPECT_TRUE(num_messages_received>0);
00219   ROS_INFO_STREAM ("Successfully received " << num_messages_received << " echoed messages");
00220 }
00221 
00222 void clientStatusCallback (const std_msgs::Int16ConstPtr& msg)
00223 {
00224   service_client_response = msg->data;
00225   service_client_status = 1;
00226 }
00227 
00228 TEST(Roslisp, Service)
00229 {
00230   // tests service call got through, service errors did not kill node, service error msg gets through
00231   ros::NodeHandle n;
00232   ros::Subscriber sub=n.subscribe("client_status", 1000, clientStatusCallback);
00233   ros::Duration d(1);
00234   unsigned wait_max=180;
00235   for (unsigned i=0; i<wait_max && n.ok(); i++) {
00236     d.sleep();
00237     ros::spinOnce();
00238     ROS_INFO_STREAM ("Have waited " << i << " out of " << wait_max << " seconds to receive confirmation from service client test");
00239     if (service_client_status)
00240       break;
00241   }
00242 
00243   EXPECT_EQ (66, service_client_response);
00244 }
00245 
00246     
00247 
00248 int main(int argc, char **argv)
00249 {
00250   testing::InitGoogleTest(&argc, argv);
00251   ros::init(argc, argv, "roslisp-tester");
00252   return RUN_ALL_TESTS();
00253 }


test_roslisp
Author(s): Bhaskara Marthi
autogenerated on Wed Apr 23 2014 10:27:57