00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
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
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
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
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
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
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 }