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 #include <gtest/gtest.h>
00031 #include <ros/ros.h>
00032 #include <rviz/default_plugin/interactive_markers/interactive_marker_client.h>
00033
00034 class MockReceiver: public rviz::InteractiveMarkerReceiver
00035 {
00036 public:
00037 std::map<std::string, double> marks_;
00038
00039 std::string calls_;
00040
00041 virtual void processMarkerChanges( const std::vector<visualization_msgs::InteractiveMarker>* markers = NULL,
00042 const std::vector<visualization_msgs::InteractiveMarkerPose>* poses = NULL,
00043 const std::vector<std::string>* erases = NULL )
00044 {
00045 if( markers != NULL )
00046 {
00047 std::vector<visualization_msgs::InteractiveMarker>::const_iterator marker_it;
00048 for( marker_it = markers->begin(); marker_it != markers->end(); marker_it++ )
00049 {
00050 marks_[ marker_it->name ] = marker_it->pose.position.x;
00051 }
00052 }
00053
00054 if( poses != NULL )
00055 {
00056 std::vector<visualization_msgs::InteractiveMarkerPose>::const_iterator pose_it;
00057 for( pose_it = poses->begin(); pose_it != poses->end(); pose_it++ )
00058 {
00059 marks_[ pose_it->name ] = pose_it->pose.position.x;
00060 }
00061 }
00062
00063 if( erases != NULL )
00064 {
00065 std::vector<std::string>::const_iterator erase_it;
00066 for( erase_it = erases->begin(); erase_it != erases->end(); erase_it++ )
00067 {
00068 std::map<std::string, double>::iterator find_it = marks_.find( *erase_it );
00069 if( find_it != marks_.end() )
00070 {
00071 marks_.erase( find_it );
00072 }
00073 }
00074 }
00075
00076 calls_ += "processMarkerChanges ";
00077 }
00078
00079 virtual void clearMarkers()
00080 {
00081 marks_.clear();
00082 calls_ += "clearMarkers ";
00083 }
00084
00085 virtual bool subscribeToInit()
00086 {
00087 calls_ += "subscribeToInit ";
00088 return true;
00089 }
00090
00091 virtual void unsubscribeFromInit()
00092 {
00093 calls_ += "unsubscribeFromInit ";
00094 }
00095
00096 virtual void setStatusOk(const std::string& name, const std::string& text)
00097 {
00098 }
00099
00100 virtual void setStatusWarn(const std::string& name, const std::string& text)
00101 {
00102 }
00103
00104 virtual void setStatusError(const std::string& name, const std::string& text)
00105 {
00106 }
00107
00108 void clearCalls()
00109 {
00110 calls_ = "";
00111 }
00112 };
00113
00114 visualization_msgs::InteractiveMarkerInit::Ptr makeInit( const std::string& server_name,
00115 uint64_t seq_num,
00116 const std::string& marker_name,
00117 double x_value )
00118 {
00119 visualization_msgs::InteractiveMarkerInit::Ptr msg( new visualization_msgs::InteractiveMarkerInit() );
00120 msg->server_id = server_name;
00121 msg->seq_num = seq_num;
00122 msg->markers.resize( 1 );
00123 msg->markers[0].name = marker_name;
00124 msg->markers[0].pose.position.x = x_value;
00125 return msg;
00126 }
00127
00128 visualization_msgs::InteractiveMarkerUpdate::Ptr makeUpdateMarker( const std::string& server_name,
00129 uint64_t seq_num,
00130 const std::string& marker_name,
00131 double x_value )
00132 {
00133 visualization_msgs::InteractiveMarkerUpdate::Ptr msg( new visualization_msgs::InteractiveMarkerUpdate() );
00134 msg->server_id = server_name;
00135 msg->seq_num = seq_num;
00136 msg->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
00137 msg->markers.resize( 1 );
00138 msg->markers[0].name = marker_name;
00139 msg->markers[0].pose.position.x = x_value;
00140 return msg;
00141 }
00142
00143 visualization_msgs::InteractiveMarkerUpdate::Ptr makeUpdatePose( const std::string& server_name,
00144 uint64_t seq_num,
00145 const std::string& marker_name,
00146 double x_value )
00147 {
00148 visualization_msgs::InteractiveMarkerUpdate::Ptr msg( new visualization_msgs::InteractiveMarkerUpdate() );
00149 msg->server_id = server_name;
00150 msg->seq_num = seq_num;
00151 msg->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
00152 msg->poses.resize( 1 );
00153 msg->poses[0].name = marker_name;
00154 msg->poses[0].pose.position.x = x_value;
00155 return msg;
00156 }
00157
00158 visualization_msgs::InteractiveMarkerUpdate::Ptr makeKeepAlive( const std::string& server_name,
00159 uint64_t seq_num )
00160 {
00161 visualization_msgs::InteractiveMarkerUpdate::Ptr msg( new visualization_msgs::InteractiveMarkerUpdate() );
00162 msg->server_id = server_name;
00163 msg->seq_num = seq_num;
00164 msg->type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE;
00165 return msg;
00166 }
00167
00168 TEST(InteractiveMarkerClient, normal_startup)
00169 {
00170 MockReceiver rcvr;
00171 rviz::InteractiveMarkerClient client( &rcvr );
00172
00173 client.clear();
00174 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00175 rcvr.clearCalls();
00176
00177 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00178
00179 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00180 EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00181 rcvr.clearCalls();
00182
00183 client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00184
00185 EXPECT_EQ( "unsubscribeFromInit processMarkerChanges ", rcvr.calls_ );
00186 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00187 rcvr.clearCalls();
00188
00189 client.processMarkerUpdate( makeUpdatePose( "s1", 3, "m1", 1.3 ));
00190
00191 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00192 EXPECT_EQ( 1.3, rcvr.marks_["m1"] );
00193 rcvr.clearCalls();
00194 }
00195
00196 TEST(InteractiveMarkerClient, normal_keepalive)
00197 {
00198 MockReceiver rcvr;
00199 rviz::InteractiveMarkerClient client( &rcvr );
00200
00201 client.clear();
00202 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00203 rcvr.clearCalls();
00204
00205 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00206
00207 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00208 EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00209 rcvr.clearCalls();
00210
00211 client.processMarkerUpdate( makeKeepAlive( "s1", 1 ));
00212
00213 EXPECT_EQ( "unsubscribeFromInit ", rcvr.calls_ );
00214 EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00215 rcvr.clearCalls();
00216
00217 client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00218
00219 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00220 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00221 rcvr.clearCalls();
00222 }
00223
00224 TEST(InteractiveMarkerClient, update_first)
00225 {
00226 MockReceiver rcvr;
00227 rviz::InteractiveMarkerClient client( &rcvr );
00228
00229 client.clear();
00230 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00231 rcvr.clearCalls();
00232
00233 client.processMarkerUpdate( makeUpdatePose( "s1", 1, "m1", 1.1 ));
00234
00235 EXPECT_EQ( "", rcvr.calls_ );
00236 EXPECT_EQ( 0, rcvr.marks_["m1"] );
00237 rcvr.clearCalls();
00238
00239 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.2 ));
00240
00241 EXPECT_EQ( "processMarkerChanges unsubscribeFromInit ", rcvr.calls_ );
00242 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00243 rcvr.clearCalls();
00244 }
00245
00246 TEST(InteractiveMarkerClient, good_keepalive_first)
00247 {
00248 MockReceiver rcvr;
00249 rviz::InteractiveMarkerClient client( &rcvr );
00250
00251 client.clear();
00252 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00253 rcvr.clearCalls();
00254
00255 client.processMarkerUpdate( makeKeepAlive( "s1", 1 ));
00256
00257 EXPECT_EQ( "", rcvr.calls_ );
00258 rcvr.clearCalls();
00259
00260 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.2 ));
00261
00262 EXPECT_EQ( "processMarkerChanges unsubscribeFromInit ", rcvr.calls_ );
00263 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00264 rcvr.clearCalls();
00265 }
00266
00267 TEST(InteractiveMarkerClient, later_keepalive_first)
00268 {
00269 MockReceiver rcvr;
00270 rviz::InteractiveMarkerClient client( &rcvr );
00271
00272 client.clear();
00273 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00274 rcvr.clearCalls();
00275
00276 client.processMarkerUpdate( makeKeepAlive( "s1", 2 ));
00277
00278 EXPECT_EQ( "", rcvr.calls_ );
00279 rcvr.clearCalls();
00280
00281 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00282
00283 EXPECT_EQ( "", rcvr.calls_ );
00284 EXPECT_EQ( 0, rcvr.marks_["m1"] );
00285 rcvr.clearCalls();
00286
00287 client.processMarkerInit( makeInit( "s1", 2, "m1", 1.2 ));
00288
00289 EXPECT_EQ( "processMarkerChanges unsubscribeFromInit ", rcvr.calls_ );
00290 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00291 rcvr.clearCalls();
00292 }
00293
00294
00295
00296
00297 TEST(InteractiveMarkerClient, queued_updates_second_init)
00298 {
00299 MockReceiver rcvr;
00300 rviz::InteractiveMarkerClient client( &rcvr );
00301
00302 client.clear();
00303 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00304 rcvr.clearCalls();
00305
00306 client.processMarkerUpdate( makeUpdatePose( "s1", 3, "m3", 3.1 ));
00307
00308 EXPECT_EQ( "", rcvr.calls_ );
00309 EXPECT_EQ( 0, rcvr.marks_["m3"] );
00310 rcvr.clearCalls();
00311
00312 client.processMarkerUpdate( makeUpdatePose( "s1", 4, "m4", 4.1 ));
00313
00314 EXPECT_EQ( "", rcvr.calls_ );
00315 EXPECT_EQ( 0, rcvr.marks_["m3"] );
00316 EXPECT_EQ( 0, rcvr.marks_["m4"] );
00317 rcvr.clearCalls();
00318
00319 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00320
00321 EXPECT_EQ( "", rcvr.calls_ );
00322 EXPECT_EQ( 0, rcvr.marks_["m1"] );
00323 EXPECT_EQ( 0, rcvr.marks_["m3"] );
00324 EXPECT_EQ( 0, rcvr.marks_["m4"] );
00325 rcvr.clearCalls();
00326
00327 client.processMarkerInit( makeInit( "s1", 2, "m2", 2.1 ));
00328
00329 EXPECT_EQ( "processMarkerChanges unsubscribeFromInit processMarkerChanges processMarkerChanges ", rcvr.calls_ );
00330 EXPECT_EQ( 0, rcvr.marks_["m1"] );
00331 EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00332 EXPECT_EQ( 3.1, rcvr.marks_["m3"] );
00333 EXPECT_EQ( 4.1, rcvr.marks_["m4"] );
00334 rcvr.clearCalls();
00335 }
00336
00337 TEST(InteractiveMarkerClient, normal_two_server)
00338 {
00339 MockReceiver rcvr;
00340 rviz::InteractiveMarkerClient client( &rcvr );
00341
00342 client.clear();
00343 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00344 rcvr.clearCalls();
00345
00346 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00347
00348 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00349 EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00350 rcvr.clearCalls();
00351
00352 client.processMarkerInit( makeInit( "s2", 1, "m2", 2.1 ));
00353
00354 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00355 EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00356 EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00357 rcvr.clearCalls();
00358
00359 client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00360
00361 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00362 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00363 EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00364 rcvr.clearCalls();
00365
00366 client.processMarkerUpdate( makeUpdatePose( "s2", 2, "m2", 2.2 ));
00367
00368 EXPECT_EQ( "unsubscribeFromInit processMarkerChanges ", rcvr.calls_ );
00369 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00370 EXPECT_EQ( 2.2, rcvr.marks_["m2"] );
00371 rcvr.clearCalls();
00372 }
00373
00374 TEST(InteractiveMarkerClient, two_server_one_slow)
00375 {
00376 MockReceiver rcvr;
00377 rviz::InteractiveMarkerClient client( &rcvr );
00378
00379 client.clear();
00380 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00381 rcvr.clearCalls();
00382
00383 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00384
00385 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00386 EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00387 rcvr.clearCalls();
00388
00389 client.processMarkerInit( makeInit( "s2", 1, "m2", 2.1 ));
00390
00391 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00392 EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00393 EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00394 rcvr.clearCalls();
00395
00396 client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00397
00398 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00399 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00400 EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00401 rcvr.clearCalls();
00402
00403
00404
00405 client.processMarkerInit( makeInit( "s1", 2, "m1", 1.2 ));
00406
00407 EXPECT_EQ( "", rcvr.calls_ );
00408 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00409 EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00410 rcvr.clearCalls();
00411
00412 client.processMarkerUpdate( makeUpdatePose( "s2", 2, "m2", 2.2 ));
00413
00414 EXPECT_EQ( "unsubscribeFromInit processMarkerChanges ", rcvr.calls_ );
00415 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00416 EXPECT_EQ( 2.2, rcvr.marks_["m2"] );
00417 rcvr.clearCalls();
00418 }
00419
00420 TEST(InteractiveMarkerClient, init_init_update)
00421 {
00422 MockReceiver rcvr;
00423 rviz::InteractiveMarkerClient client( &rcvr );
00424
00425 client.clear();
00426 EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00427 rcvr.clearCalls();
00428
00429 client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00430
00431 EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00432 EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00433 rcvr.clearCalls();
00434
00435 client.processMarkerInit( makeInit( "s1", 2, "m1", 1.2 ));
00436
00437 EXPECT_EQ( "clearMarkers processMarkerChanges ", rcvr.calls_ );
00438 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00439 rcvr.clearCalls();
00440
00441 client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00442
00443 EXPECT_EQ( "unsubscribeFromInit ", rcvr.calls_ );
00444 EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00445 rcvr.clearCalls();
00446 }
00447
00448
00449 int main(int argc, char **argv){
00450 testing::InitGoogleTest(&argc, argv);
00451 ros::Time::init();
00452 return RUN_ALL_TESTS();
00453 }