server_client_test.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 #include <ros/ros.h>
00032 #include <ros/console.h>
00033 
00034 #include <gtest/gtest.h>
00035 
00036 #include <tf/transform_listener.h>
00037 
00038 #include <interactive_markers/interactive_marker_server.h>
00039 #include <interactive_markers/interactive_marker_client.h>
00040 
00041 #define DBG_MSG( ... ) printf( __VA_ARGS__ ); printf("\n");
00042 #define DBG_MSG_STREAM( ... )  std::cout << __VA_ARGS__ << std::endl;
00043 
00044 using namespace interactive_markers;
00045 
00046 int update_calls;
00047 int init_calls;
00048 int reset_calls;
00049 int status_calls;
00050 
00051 
00052 std::string reset_server_id;
00053 
00054 typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr;
00055 typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr;
00056 
00057 InitConstPtr init_msg;
00058 UpdateConstPtr update_msg;
00059 
00060 void resetReceivedMsgs()
00061 {
00062   update_calls = 0;
00063   init_calls = 0;
00064   reset_calls = 0;
00065   status_calls = 0;
00066   reset_server_id = "";
00067   init_msg.reset();
00068   update_msg.reset();
00069 }
00070 
00071 void updateCb( const UpdateConstPtr& msg )
00072 {
00073   DBG_MSG("updateCb called");
00074   update_calls++;
00075   update_msg = msg;
00076 }
00077 
00078 void initCb( const InitConstPtr& msg )
00079 {
00080   DBG_MSG("initCb called");
00081   init_calls++;
00082   init_msg = msg;
00083 }
00084 
00085 void statusCb( InteractiveMarkerClient::StatusT status,
00086     const std::string& server_id,
00087     const std::string& msg )
00088 {
00089   DBG_MSG("statusCb called");
00090   status_calls++;
00091   DBG_MSG_STREAM( (int)status << " " << server_id << ": " << msg );
00092 }
00093 
00094 void resetCb( const std::string& server_id )
00095 {
00096   DBG_MSG("resetCb( %s ) called", server_id.c_str() );
00097   reset_calls++;
00098   reset_server_id = server_id;
00099 }
00100 
00101 void waitMsg()
00102 {
00103   for(int i=0;i<10;i++)
00104   {
00105     ros::spinOnce();
00106     usleep(1000);
00107   }
00108 }
00109 
00110 TEST(InteractiveMarkerServerAndClient, connect_tf_error)
00111 {
00112   tf::TransformListener tf;
00113 
00114   // create an interactive marker server on the topic namespace simple_marker
00115   interactive_markers::InteractiveMarkerServer server("im_server_client_test","test_server",false);
00116   visualization_msgs::InteractiveMarker int_marker;
00117   int_marker.name = "marker1";
00118   int_marker.header.frame_id = "valid_frame";
00119 
00120   waitMsg();
00121 
00122   resetReceivedMsgs();
00123 
00124   interactive_markers::InteractiveMarkerClient client(tf, "valid_frame", "im_server_client_test");
00125   client.setInitCb( &initCb );
00126   client.setStatusCb( &statusCb );
00127   client.setResetCb( &resetCb );
00128   client.setUpdateCb( &updateCb );
00129 
00130   // Add one marker -> init should get called once
00131   DBG_MSG("----------------------------------------");
00132 
00133   server.insert(int_marker);
00134   server.applyChanges();
00135   waitMsg();
00136   client.update();
00137 
00138   ASSERT_EQ( 0, update_calls  );
00139   ASSERT_EQ( 1, init_calls  );
00140   ASSERT_EQ( 0, reset_calls  );
00141   ASSERT_TRUE( init_msg );
00142   ASSERT_EQ( 1, init_msg->markers.size()  );
00143   ASSERT_EQ( "marker1", init_msg->markers[0].name  );
00144   ASSERT_EQ( "valid_frame", init_msg->markers[0].header.frame_id  );
00145 
00146   // Add another marker -> update should get called once
00147   DBG_MSG("----------------------------------------");
00148 
00149   resetReceivedMsgs();
00150 
00151   int_marker.name = "marker2";
00152 
00153   server.insert(int_marker);
00154   server.applyChanges();
00155   waitMsg();
00156   client.update();
00157 
00158   ASSERT_EQ( 1, update_calls  );
00159   ASSERT_EQ( 0, init_calls  );
00160   ASSERT_EQ( 0, reset_calls  );
00161   ASSERT_TRUE( update_msg );
00162   ASSERT_EQ( 1, update_msg->markers.size()  );
00163   ASSERT_EQ( 0, update_msg->poses.size()  );
00164   ASSERT_EQ( 0, update_msg->erases.size()  );
00165   ASSERT_EQ( "marker2", update_msg->markers[0].name  );
00166 
00167   // Make marker tf info invalid -> connection should be reset
00168   DBG_MSG("----------------------------------------");
00169 
00170   resetReceivedMsgs();
00171 
00172   int_marker.header.frame_id = "invalid_frame";
00173 
00174   server.insert(int_marker);
00175   server.applyChanges();
00176   waitMsg();
00177   client.update();
00178 
00179   ASSERT_EQ( 0, update_calls  );
00180   ASSERT_EQ( 0, init_calls  );
00181   ASSERT_EQ( 1, reset_calls  );
00182   ASSERT_TRUE( reset_server_id.find("/test_server") != std::string::npos );
00183 
00184   // Make marker tf info valid again -> connection should be successfully initialized again
00185   DBG_MSG("----------------------------------------");
00186 
00187   usleep(2000000);
00188   waitMsg();
00189   client.update();
00190 
00191   resetReceivedMsgs();
00192 
00193   int_marker.header.frame_id = "valid_frame";
00194 
00195   server.insert(int_marker);
00196   server.applyChanges();
00197   waitMsg();
00198   client.update();
00199 
00200   ASSERT_EQ( 0, update_calls  );
00201   ASSERT_EQ( 1, init_calls  );
00202   ASSERT_EQ( 0, reset_calls  );
00203 
00204   // Erase marker
00205   DBG_MSG("----------------------------------------");
00206 
00207   resetReceivedMsgs();
00208 
00209   server.erase("marker1");
00210   server.applyChanges();
00211   waitMsg();
00212   client.update();
00213 
00214   ASSERT_EQ( 1, update_calls  );
00215   ASSERT_EQ( 0, init_calls  );
00216   ASSERT_EQ( 0, reset_calls  );
00217   ASSERT_TRUE( update_msg );
00218   ASSERT_EQ( 0, update_msg->markers.size()  );
00219   ASSERT_EQ( 0, update_msg->poses.size()  );
00220   ASSERT_EQ( 1, update_msg->erases.size()  );
00221   ASSERT_EQ( "marker1", update_msg->erases[0]  );
00222 
00223   // Update pose
00224   DBG_MSG("----------------------------------------");
00225 
00226   resetReceivedMsgs();
00227 
00228   server.setPose( "marker2", int_marker.pose, int_marker.header );
00229   server.applyChanges();
00230   waitMsg();
00231   client.update();
00232 
00233   ASSERT_EQ( 1, update_calls  );
00234   ASSERT_EQ( 0, init_calls  );
00235   ASSERT_EQ( 0, reset_calls  );
00236   ASSERT_TRUE( update_msg );
00237   ASSERT_EQ( 0, update_msg->markers.size()  );
00238   ASSERT_EQ( 1, update_msg->poses.size()  );
00239   ASSERT_EQ( 0, update_msg->erases.size()  );
00240   ASSERT_EQ( "marker2", update_msg->poses[0].name  );
00241 }
00242 
00243 
00244 // Run all the tests that were declared with TEST()
00245 int main(int argc, char **argv)
00246 {
00247   ros::init(argc, argv, "im_server_client_test");
00248   testing::InitGoogleTest(&argc, argv);
00249   return RUN_ALL_TESTS();
00250 }


interactive_markers
Author(s): David Gossow
autogenerated on Sat Aug 27 2016 03:37:45