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 struct Msg
00047 {
00048 enum {
00049 INIT,
00050 KEEP_ALIVE,
00051 UPDATE,
00052 POSE,
00053 DELETE,
00054 TF_INFO
00055 } type;
00056
00057 Msg()
00058 {
00059 type = INIT;
00060 seq_num = 0;
00061 }
00062
00063 uint64_t seq_num;
00064
00065 std::string server_id;
00066 std::string frame_id;
00067 ros::Time stamp;
00068
00069 std::vector<std::string> expect_reset_calls;
00070 std::vector<int> expect_init_seq_num;
00071 std::vector<int> expect_update_seq_num;
00072 };
00073
00074 std::string target_frame = "target_frame";
00075
00076
00077 class SequenceTest
00078 {
00079 typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr;
00080 typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr;
00081
00082 std::vector<visualization_msgs::InteractiveMarkerInit> recv_init_msgs;
00083 std::vector<visualization_msgs::InteractiveMarkerUpdate> recv_update_msgs;
00084 std::vector<std::string> recv_reset_calls;
00085
00086 void resetReceivedMsgs()
00087 {
00088 recv_init_msgs.clear();
00089 recv_update_msgs.clear();
00090 recv_reset_calls.clear();
00091 }
00092
00093 void updateCb( const UpdateConstPtr& msg )
00094 {
00095 DBG_MSG_STREAM( "updateCb called." );
00096 recv_update_msgs.push_back( *msg );
00097 }
00098
00099 void initCb( const InitConstPtr& msg )
00100 {
00101 DBG_MSG_STREAM( "initCb called." );
00102 recv_init_msgs.push_back( *msg );
00103 }
00104
00105 void statusCb( InteractiveMarkerClient::StatusT status,
00106 const std::string& server_id,
00107 const std::string& msg )
00108 {
00109 std::string status_string[]={"INFO","WARN","ERROR"};
00110 ASSERT_TRUE( (unsigned)status < 3 );
00111 DBG_MSG_STREAM( "(" << status_string[(unsigned)status] << ") " << server_id << ": " << msg );
00112 }
00113
00114 void resetCb( const std::string& server_id )
00115 {
00116 DBG_MSG_STREAM( "resetCb called." );
00117 recv_reset_calls.push_back(server_id);
00118 }
00119
00120 public:
00121 void test( std::vector<Msg> messages )
00122 {
00123 tf::Transformer tf;
00124
00125
00126
00127
00128
00129 visualization_msgs::InteractiveMarker int_marker;
00130 int_marker.pose.orientation.w=1;
00131
00132 std::string topic_ns ="im_client_test";
00133
00134 interactive_markers::InteractiveMarkerClient client(tf, target_frame, topic_ns );
00135
00136 client.setInitCb( boost::bind(&SequenceTest::initCb, this, _1 ) );
00137 client.setUpdateCb( boost::bind(&SequenceTest::updateCb, this, _1 ) );
00138 client.setResetCb( boost::bind(&SequenceTest::resetCb, this, _1 ) );
00139 client.setStatusCb( boost::bind(&SequenceTest::statusCb, this, _1, _2, _3 ) );
00140
00141 std::map< int, visualization_msgs::InteractiveMarkerInit > sent_init_msgs;
00142 std::map< int, visualization_msgs::InteractiveMarkerUpdate > sent_update_msgs;
00143
00144 for ( size_t i=0; i<messages.size(); i++ )
00145 {
00146 resetReceivedMsgs();
00147
00148 Msg& msg = messages[i];
00149
00150 int_marker.header.frame_id=msg.frame_id;
00151 int_marker.header.stamp=msg.stamp;
00152 int_marker.pose.orientation.w = 1.0;
00153
00154 std::ostringstream s;
00155 s << i;
00156 int_marker.name=s.str();
00157
00158 switch( msg.type )
00159 {
00160 case Msg::INIT:
00161 {
00162 DBG_MSG_STREAM( i << " INIT: seq_num=" << msg.seq_num << " frame=" << msg.frame_id << " stamp=" << msg.stamp );
00163 visualization_msgs::InteractiveMarkerInitPtr init_msg_out( new visualization_msgs::InteractiveMarkerInit() );
00164 init_msg_out->server_id=msg.server_id;
00165 init_msg_out->seq_num=msg.seq_num;
00166 init_msg_out->markers.push_back( int_marker );
00167 client.processInit( init_msg_out );
00168 sent_init_msgs[msg.seq_num]=*init_msg_out;
00169 break;
00170 }
00171 case Msg::KEEP_ALIVE:
00172 {
00173 DBG_MSG_STREAM( i << " KEEP_ALIVE: seq_num=" << msg.seq_num );
00174 visualization_msgs::InteractiveMarkerUpdatePtr update_msg_out( new visualization_msgs::InteractiveMarkerUpdate() );
00175 update_msg_out->server_id=msg.server_id;
00176 update_msg_out->type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE;
00177 update_msg_out->seq_num=msg.seq_num;
00178
00179 client.processUpdate( update_msg_out );
00180 sent_update_msgs[msg.seq_num]=*update_msg_out;
00181 break;
00182 }
00183 case Msg::UPDATE:
00184 {
00185 DBG_MSG_STREAM( i << " UPDATE: seq_num=" << msg.seq_num << " frame=" << msg.frame_id << " stamp=" << msg.stamp );
00186 visualization_msgs::InteractiveMarkerUpdatePtr update_msg_out( new visualization_msgs::InteractiveMarkerUpdate() );
00187 update_msg_out->server_id=msg.server_id;
00188 update_msg_out->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
00189 update_msg_out->seq_num=msg.seq_num;
00190
00191 update_msg_out->markers.push_back( int_marker );
00192 client.processUpdate( update_msg_out );
00193 sent_update_msgs[msg.seq_num]=*update_msg_out;
00194 break;
00195 }
00196 case Msg::POSE:
00197 {
00198 DBG_MSG_STREAM( i << " POSE: seq_num=" << msg.seq_num << " frame=" << msg.frame_id << " stamp=" << msg.stamp );
00199 visualization_msgs::InteractiveMarkerUpdatePtr update_msg_out( new visualization_msgs::InteractiveMarkerUpdate() );
00200 update_msg_out->server_id=msg.server_id;
00201 update_msg_out->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
00202 update_msg_out->seq_num=msg.seq_num;
00203
00204 visualization_msgs::InteractiveMarkerPose pose;
00205 pose.header=int_marker.header;
00206 pose.name=int_marker.name;
00207 pose.pose=int_marker.pose;
00208 update_msg_out->poses.push_back( pose );
00209 client.processUpdate( update_msg_out );
00210 sent_update_msgs[msg.seq_num]=*update_msg_out;
00211 break;
00212 }
00213 case Msg::DELETE:
00214 {
00215 DBG_MSG_STREAM( i << " DELETE: seq_num=" << msg.seq_num );
00216 visualization_msgs::InteractiveMarkerUpdatePtr update_msg_out( new visualization_msgs::InteractiveMarkerUpdate() );
00217 update_msg_out->server_id="/im_client_test/test_server";
00218 update_msg_out->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
00219 update_msg_out->seq_num=msg.seq_num;
00220
00221 update_msg_out->erases.push_back( int_marker.name );
00222 client.processUpdate( update_msg_out );
00223 sent_update_msgs[msg.seq_num]=*update_msg_out;
00224 break;
00225 }
00226 case Msg::TF_INFO:
00227 {
00228 DBG_MSG_STREAM( i << " TF_INFO: " << msg.frame_id << " -> " << target_frame << " at time " << msg.stamp.toSec() );
00229 tf::StampedTransform stf;
00230 stf.frame_id_=msg.frame_id;
00231 stf.child_frame_id_=target_frame;
00232 stf.stamp_=msg.stamp;
00233 stf.setIdentity();
00234 tf.setTransform( stf, msg.server_id );
00235 break;
00236 }
00237 }
00238
00239
00240
00241
00242
00243
00244
00245 client.update();
00246
00247 ASSERT_EQ( msg.expect_update_seq_num.size(), recv_update_msgs.size() );
00248 ASSERT_EQ( msg.expect_init_seq_num.size(), recv_init_msgs.size() );
00249 ASSERT_EQ( msg.expect_reset_calls.size(), recv_reset_calls.size() );
00250
00251 for ( size_t u=0; u<msg.expect_update_seq_num.size(); u++ )
00252 {
00253 ASSERT_TRUE( sent_update_msgs.find(msg.expect_update_seq_num[u]) != sent_update_msgs.end() );
00254
00255 visualization_msgs::InteractiveMarkerUpdate sent_msg = sent_update_msgs[msg.expect_update_seq_num[u]];
00256 visualization_msgs::InteractiveMarkerUpdate recv_msg = recv_update_msgs[ u ];
00257
00258
00259 ASSERT_EQ( sent_msg.seq_num, msg.expect_update_seq_num[u] );
00260
00261
00262 ASSERT_EQ( recv_msg.seq_num, msg.expect_update_seq_num[u] );
00263
00264
00265 ASSERT_EQ( recv_msg.markers.size(), sent_msg.markers.size() );
00266 ASSERT_EQ( recv_msg.poses.size(), sent_msg.poses.size() );
00267 ASSERT_EQ( recv_msg.erases.size(), sent_msg.erases.size() );
00268
00269
00270
00271 for ( size_t m=0; m<sent_msg.markers.size(); m++ )
00272 {
00273 ASSERT_EQ( recv_msg.markers[m].name, sent_msg.markers[m].name );
00274 ASSERT_EQ( recv_msg.markers[m].header.stamp, sent_msg.markers[m].header.stamp );
00275 if ( sent_msg.markers[m].header.stamp == ros::Time(0) )
00276 {
00277 ASSERT_EQ( target_frame, sent_msg.markers[m].header.frame_id );
00278 }
00279 else
00280 {
00281 ASSERT_EQ( target_frame, recv_msg.markers[m].header.frame_id );
00282 }
00283 }
00284 for ( size_t p=0; p<sent_msg.poses.size(); p++ )
00285 {
00286 ASSERT_EQ( recv_msg.poses[p].name, sent_msg.poses[p].name );
00287 ASSERT_EQ( recv_msg.poses[p].header.stamp, sent_msg.poses[p].header.stamp );
00288 if ( sent_msg.poses[p].header.stamp == ros::Time(0) )
00289 {
00290 ASSERT_EQ( target_frame, sent_msg.poses[p].header.frame_id );
00291 }
00292 else
00293 {
00294 ASSERT_EQ( target_frame, recv_msg.poses[p].header.frame_id );
00295 }
00296 }
00297 for ( size_t e=0; e<sent_msg.erases.size(); e++ )
00298 {
00299 ASSERT_EQ( recv_msg.erases[e], sent_msg.erases[e] );
00300 }
00301 }
00302
00303
00304 for ( size_t u=0; u<msg.expect_init_seq_num.size(); u++ )
00305 {
00306 ASSERT_TRUE( sent_init_msgs.find(msg.expect_init_seq_num[u]) != sent_init_msgs.end() );
00307
00308 visualization_msgs::InteractiveMarkerInit sent_msg = sent_init_msgs[msg.expect_init_seq_num[u]];
00309 visualization_msgs::InteractiveMarkerInit recv_msg = recv_init_msgs[ u ];
00310
00311
00312 ASSERT_EQ( sent_msg.seq_num, msg.expect_init_seq_num[u] );
00313
00314
00315 ASSERT_EQ( recv_msg.seq_num, msg.expect_init_seq_num[u] );
00316
00317
00318 ASSERT_EQ( recv_msg.markers.size(), sent_msg.markers.size() );
00319
00320
00321
00322 for ( size_t m=0; m<sent_msg.markers.size(); m++ )
00323 {
00324 ASSERT_EQ( recv_msg.markers[m].name, sent_msg.markers[m].name );
00325 ASSERT_EQ( recv_msg.markers[m].header.stamp, sent_msg.markers[m].header.stamp );
00326 if ( sent_msg.markers[m].header.stamp == ros::Time(0) )
00327 {
00328
00329 ASSERT_TRUE( tf.canTransform( target_frame, sent_msg.markers[m].header.frame_id, ros::Time(0) ) );
00330 }
00331 else
00332 {
00333
00334 ASSERT_EQ( target_frame, recv_msg.markers[m].header.frame_id );
00335 }
00336 }
00337 }
00338 }
00339 }
00340 };
00341
00342 TEST(InteractiveMarkerClient, init_simple1)
00343 {
00344 Msg msg;
00345
00346 std::vector<Msg> seq;
00347
00348 msg.type=Msg::INIT;
00349 msg.seq_num=0;
00350 msg.server_id="server1";
00351 msg.frame_id=target_frame;
00352 seq.push_back(msg);
00353
00354 msg.type=Msg::KEEP_ALIVE;
00355 msg.expect_init_seq_num.push_back(0);
00356 seq.push_back(msg);
00357
00358 SequenceTest t;
00359 t.test(seq);
00360 }
00361
00362 TEST(InteractiveMarkerClient, init_simple2)
00363 {
00364 Msg msg;
00365
00366 std::vector<Msg> seq;
00367
00368 msg.type=Msg::INIT;
00369 msg.seq_num=0;
00370 msg.server_id="server1";
00371 msg.frame_id=target_frame;
00372 seq.push_back(msg);
00373
00374 msg.type=Msg::UPDATE;
00375 msg.expect_init_seq_num.push_back(0);
00376 seq.push_back(msg);
00377
00378 SequenceTest t;
00379 t.test(seq);
00380 }
00381
00382
00383 TEST(InteractiveMarkerClient, init_many_inits)
00384 {
00385 Msg msg;
00386
00387 std::vector<Msg> seq;
00388
00389 for ( int i=0; i<200; i++ )
00390 {
00391 msg.type=Msg::INIT;
00392 msg.seq_num=i;
00393 msg.server_id="server1";
00394 msg.frame_id=target_frame;
00395 seq.push_back(msg);
00396 }
00397
00398
00399 msg.type=Msg::UPDATE;
00400 msg.expect_init_seq_num.push_back(msg.seq_num);
00401 seq.push_back(msg);
00402
00403
00404 msg.seq_num++;
00405 msg.expect_init_seq_num.clear();
00406 msg.expect_update_seq_num.push_back(msg.seq_num);
00407 seq.push_back(msg);
00408
00409 SequenceTest t;
00410 t.test(seq);
00411 }
00412
00413 TEST(InteractiveMarkerClient, init_many_updates)
00414 {
00415 Msg msg;
00416
00417 std::vector<Msg> seq;
00418
00419 for ( int i=0; i<200; i++ )
00420 {
00421 msg.type=Msg::UPDATE;
00422 msg.seq_num=i;
00423 msg.server_id="server1";
00424 msg.frame_id=target_frame;
00425 seq.push_back(msg);
00426 }
00427
00428 msg.type=Msg::INIT;
00429 msg.seq_num=190;
00430 msg.expect_init_seq_num.push_back(msg.seq_num);
00431 for ( int i=191; i<200; i++ )
00432 {
00433 msg.expect_update_seq_num.push_back(i);
00434 }
00435 seq.push_back(msg);
00436
00437 SequenceTest t;
00438 t.test(seq);
00439 }
00440
00441 TEST(InteractiveMarkerClient, init_invalid_tf)
00442 {
00443 Msg msg;
00444
00445 std::vector<Msg> seq;
00446
00447 msg.type=Msg::INIT;
00448 msg.seq_num=0;
00449 msg.server_id="server1";
00450 msg.frame_id="invalid_frame";
00451 seq.push_back(msg);
00452
00453 msg.type=Msg::INIT;
00454 msg.seq_num=1;
00455 msg.server_id="server1";
00456 msg.frame_id=target_frame;
00457 seq.push_back(msg);
00458
00459 msg.type=Msg::KEEP_ALIVE;
00460 msg.expect_init_seq_num.push_back(1);
00461 seq.push_back(msg);
00462
00463 SequenceTest t;
00464 t.test(seq);
00465 }
00466
00467 TEST(InteractiveMarkerClient, init_wait_tf)
00468 {
00469 Msg msg;
00470
00471 std::vector<Msg> seq;
00472
00473
00474 msg.type=Msg::TF_INFO;
00475 msg.server_id="server1";
00476 msg.frame_id="wait_frame";
00477 msg.stamp=ros::Time(1.0);
00478 seq.push_back(msg);
00479
00480
00481 msg.type=Msg::INIT;
00482 msg.seq_num=0;
00483 msg.stamp=ros::Time(2.0);
00484 seq.push_back(msg);
00485
00486 msg.type=Msg::KEEP_ALIVE;
00487 seq.push_back(msg);
00488
00489
00490 msg.type=Msg::TF_INFO;
00491 msg.expect_init_seq_num.push_back(0);
00492 seq.push_back(msg);
00493
00494
00495 msg.type=Msg::UPDATE;
00496 msg.seq_num=1;
00497 msg.stamp=ros::Time(3.0);
00498 msg.expect_init_seq_num.clear();
00499 seq.push_back(msg);
00500
00501
00502 msg.type=Msg::TF_INFO;
00503 msg.expect_update_seq_num.push_back(1);
00504 seq.push_back(msg);
00505
00506
00507 msg.type=Msg::POSE;
00508 msg.seq_num=2;
00509 msg.stamp=ros::Time(4.0);
00510 msg.expect_update_seq_num.clear();
00511 seq.push_back(msg);
00512
00513
00514 msg.type=Msg::TF_INFO;
00515 msg.expect_update_seq_num.push_back(2);
00516 seq.push_back(msg);
00517
00518 SequenceTest t;
00519 t.test(seq);
00520 }
00521
00522
00523 TEST(InteractiveMarkerClient, init_wait_tf_zerotime)
00524 {
00525 Msg msg;
00526
00527 std::vector<Msg> seq;
00528
00529
00530 msg.type=Msg::INIT;
00531 msg.server_id="server1";
00532 msg.frame_id="wait_frame";
00533 msg.seq_num=0;
00534 msg.stamp=ros::Time(0.0);
00535 seq.push_back(msg);
00536
00537 msg.type=Msg::KEEP_ALIVE;
00538 seq.push_back(msg);
00539
00540
00541 msg.type=Msg::TF_INFO;
00542 msg.stamp=ros::Time(1.0);
00543 msg.expect_init_seq_num.push_back(0);
00544 seq.push_back(msg);
00545
00546 SequenceTest t;
00547 t.test(seq);
00548 }
00549
00550
00551 TEST(InteractiveMarkerClient, init_wait_tf_inverse)
00552 {
00553
00554
00555
00556
00557 Msg msg;
00558
00559 std::vector<Msg> seq;
00560
00561
00562 msg.type=Msg::TF_INFO;
00563 msg.server_id="server1";
00564 msg.frame_id="wait_frame";
00565 msg.stamp=ros::Time(1.0);
00566 seq.push_back(msg);
00567
00568 msg.type=Msg::INIT;
00569 msg.seq_num=0;
00570 msg.stamp=ros::Time(6);
00571 seq.push_back(msg);
00572
00573 msg.type=Msg::INIT;
00574 msg.seq_num=1;
00575 msg.stamp=ros::Time(5);
00576 seq.push_back(msg);
00577
00578 msg.type=Msg::KEEP_ALIVE;
00579 msg.seq_num=0;
00580 seq.push_back(msg);
00581
00582 msg.type=Msg::UPDATE;
00583 msg.seq_num=1;
00584 msg.stamp=ros::Time(5);
00585 seq.push_back(msg);
00586
00587 msg.type=Msg::UPDATE;
00588 msg.seq_num=2;
00589 msg.stamp=ros::Time(4);
00590 seq.push_back(msg);
00591
00592 msg.type=Msg::UPDATE;
00593 msg.seq_num=3;
00594 msg.stamp=ros::Time(3);
00595 seq.push_back(msg);
00596
00597 msg.type=Msg::TF_INFO;
00598 msg.stamp=ros::Time(2);
00599 seq.push_back(msg);
00600
00601 msg.type=Msg::TF_INFO;
00602 msg.stamp=ros::Time(3);
00603 seq.push_back(msg);
00604
00605 msg.type=Msg::TF_INFO;
00606 msg.stamp=ros::Time(4);
00607 seq.push_back(msg);
00608
00609
00610
00611 msg.stamp=ros::Time(5);
00612 msg.expect_init_seq_num.push_back(1);
00613 msg.expect_update_seq_num.push_back(2);
00614 msg.expect_update_seq_num.push_back(3);
00615 seq.push_back(msg);
00616
00617 SequenceTest t;
00618 t.test(seq);
00619 }
00620
00621 TEST(InteractiveMarkerClient, wait_tf_inverse)
00622 {
00623
00624
00625
00626
00627 Msg msg;
00628
00629 std::vector<Msg> seq;
00630
00631
00632 msg.type=Msg::TF_INFO;
00633 msg.server_id="server1";
00634 msg.frame_id="wait_frame";
00635 msg.stamp=ros::Time(1);
00636 seq.push_back(msg);
00637
00638 msg.type=Msg::INIT;
00639 msg.seq_num=0;
00640 seq.push_back(msg);
00641
00642 msg.type=Msg::KEEP_ALIVE;
00643 msg.seq_num=0;
00644 msg.expect_init_seq_num.push_back(0);
00645 seq.push_back(msg);
00646
00647 msg.expect_init_seq_num.clear();
00648
00649
00650
00651 msg.type=Msg::UPDATE;
00652 msg.seq_num=1;
00653 msg.stamp=ros::Time(5);
00654 seq.push_back(msg);
00655
00656 msg.type=Msg::UPDATE;
00657 msg.seq_num=2;
00658 msg.stamp=ros::Time(4);
00659 seq.push_back(msg);
00660
00661 msg.type=Msg::UPDATE;
00662 msg.seq_num=3;
00663 msg.stamp=ros::Time(3);
00664 seq.push_back(msg);
00665
00666 msg.type=Msg::TF_INFO;
00667 msg.stamp=ros::Time(3);
00668 seq.push_back(msg);
00669
00670 msg.type=Msg::TF_INFO;
00671 msg.stamp=ros::Time(4);
00672 seq.push_back(msg);
00673
00674
00675 msg.type=Msg::TF_INFO;
00676 msg.stamp=ros::Time(5);
00677 msg.expect_update_seq_num.push_back(1);
00678 msg.expect_update_seq_num.push_back(2);
00679 msg.expect_update_seq_num.push_back(3);
00680 seq.push_back(msg);
00681
00682 SequenceTest t;
00683 t.test(seq);
00684 }
00685
00686
00687 TEST(InteractiveMarkerClient, wrong_seq_num1)
00688 {
00689 Msg msg;
00690
00691 std::vector<Msg> seq;
00692
00693 msg.type=Msg::INIT;
00694 msg.seq_num=0;
00695 msg.server_id="server1";
00696 msg.frame_id=target_frame;
00697 seq.push_back(msg);
00698
00699 msg.type=Msg::KEEP_ALIVE;
00700 msg.expect_init_seq_num.push_back(0);
00701 seq.push_back(msg);
00702
00703 msg.expect_init_seq_num.clear();
00704
00705 msg.type=Msg::KEEP_ALIVE;
00706 msg.seq_num=1;
00707 msg.expect_reset_calls.push_back(msg.server_id);
00708 seq.push_back(msg);
00709
00710 SequenceTest t;
00711 t.test(seq);
00712 }
00713
00714 TEST(InteractiveMarkerClient, wrong_seq_num2)
00715 {
00716 Msg msg;
00717
00718 std::vector<Msg> seq;
00719
00720 msg.type=Msg::INIT;
00721 msg.seq_num=1;
00722 msg.server_id="server1";
00723 msg.frame_id=target_frame;
00724 seq.push_back(msg);
00725
00726 msg.type=Msg::KEEP_ALIVE;
00727 msg.expect_init_seq_num.push_back(1);
00728 seq.push_back(msg);
00729
00730 msg.expect_init_seq_num.clear();
00731
00732 msg.type=Msg::KEEP_ALIVE;
00733 msg.seq_num=0;
00734 msg.expect_reset_calls.push_back(msg.server_id);
00735 seq.push_back(msg);
00736
00737 SequenceTest t;
00738 t.test(seq);
00739 }
00740
00741 TEST(InteractiveMarkerClient, wrong_seq_num3)
00742 {
00743 Msg msg;
00744
00745 std::vector<Msg> seq;
00746
00747 msg.type=Msg::INIT;
00748 msg.seq_num=1;
00749 msg.server_id="server1";
00750 msg.frame_id=target_frame;
00751 seq.push_back(msg);
00752
00753 msg.type=Msg::KEEP_ALIVE;
00754 msg.expect_init_seq_num.push_back(1);
00755 seq.push_back(msg);
00756
00757 msg.expect_init_seq_num.clear();
00758
00759 msg.type=Msg::UPDATE;
00760 msg.seq_num=1;
00761 msg.expect_reset_calls.push_back(msg.server_id);
00762 seq.push_back(msg);
00763
00764 SequenceTest t;
00765 t.test(seq);
00766 }
00767
00768 TEST(InteractiveMarkerClient, wrong_seq_num4)
00769 {
00770 Msg msg;
00771
00772 std::vector<Msg> seq;
00773
00774 msg.type=Msg::INIT;
00775 msg.seq_num=1;
00776 msg.server_id="server1";
00777 msg.frame_id=target_frame;
00778 seq.push_back(msg);
00779
00780 msg.type=Msg::KEEP_ALIVE;
00781 msg.expect_init_seq_num.push_back(1);
00782 seq.push_back(msg);
00783
00784 msg.expect_init_seq_num.clear();
00785
00786 msg.type=Msg::UPDATE;
00787 msg.seq_num=2;
00788 msg.expect_update_seq_num.push_back(2);
00789 seq.push_back(msg);
00790
00791 msg.expect_update_seq_num.clear();
00792
00793 msg.type=Msg::UPDATE;
00794 msg.seq_num=2;
00795 msg.expect_reset_calls.push_back(msg.server_id);
00796 seq.push_back(msg);
00797
00798 SequenceTest t;
00799 t.test(seq);
00800 }
00801
00802 TEST(InteractiveMarkerClient, wrong_seq_num5)
00803 {
00804 Msg msg;
00805
00806 std::vector<Msg> seq;
00807
00808 msg.type=Msg::INIT;
00809 msg.seq_num=1;
00810 msg.server_id="server1";
00811 msg.frame_id=target_frame;
00812 seq.push_back(msg);
00813
00814 msg.type=Msg::KEEP_ALIVE;
00815 msg.expect_init_seq_num.push_back(1);
00816 seq.push_back(msg);
00817
00818 msg.expect_init_seq_num.clear();
00819
00820 msg.type=Msg::UPDATE;
00821 msg.seq_num=2;
00822 msg.expect_update_seq_num.push_back(2);
00823 seq.push_back(msg);
00824
00825 msg.expect_update_seq_num.clear();
00826
00827 msg.type=Msg::UPDATE;
00828 msg.seq_num=1;
00829 msg.expect_reset_calls.push_back(msg.server_id);
00830 seq.push_back(msg);
00831
00832 SequenceTest t;
00833 t.test(seq);
00834 }
00835
00836 TEST(InteractiveMarkerClient, wrong_seq_num6)
00837 {
00838 Msg msg;
00839
00840 std::vector<Msg> seq;
00841
00842 msg.type=Msg::INIT;
00843 msg.seq_num=1;
00844 msg.server_id="server1";
00845 msg.frame_id=target_frame;
00846 seq.push_back(msg);
00847
00848 msg.type=Msg::KEEP_ALIVE;
00849 msg.expect_init_seq_num.push_back(1);
00850 seq.push_back(msg);
00851
00852 msg.expect_init_seq_num.clear();
00853
00854 msg.type=Msg::UPDATE;
00855 msg.seq_num=2;
00856 msg.expect_update_seq_num.push_back(2);
00857 seq.push_back(msg);
00858
00859 msg.expect_update_seq_num.clear();
00860
00861 msg.type=Msg::UPDATE;
00862 msg.seq_num=4;
00863 msg.expect_reset_calls.push_back(msg.server_id);
00864 seq.push_back(msg);
00865
00866 SequenceTest t;
00867 t.test(seq);
00868 }
00869
00870
00871 TEST(InteractiveMarkerClient, init_twoservers)
00872 {
00873 Msg msg;
00874
00875 std::vector<Msg> seq;
00876
00877 msg.type=Msg::INIT;
00878 msg.seq_num=0;
00879 msg.server_id="server1";
00880 msg.frame_id=target_frame;
00881 seq.push_back(msg);
00882
00883 msg.type=Msg::KEEP_ALIVE;
00884 msg.expect_init_seq_num.push_back(0);
00885 seq.push_back(msg);
00886
00887 msg.expect_init_seq_num.clear();
00888
00889 msg.type=Msg::UPDATE;
00890 msg.seq_num=1;
00891 msg.expect_update_seq_num.push_back(1);
00892 seq.push_back(msg);
00893
00894 msg.expect_update_seq_num.clear();
00895
00896 msg.type=Msg::INIT;
00897 msg.seq_num=0;
00898 msg.server_id="server2";
00899 msg.frame_id=target_frame;
00900 seq.push_back(msg);
00901
00902 msg.type=Msg::KEEP_ALIVE;
00903 msg.expect_init_seq_num.push_back(0);
00904 seq.push_back(msg);
00905
00906 msg.expect_init_seq_num.clear();
00907
00908 msg.type=Msg::UPDATE;
00909 msg.seq_num=1;
00910 msg.expect_update_seq_num.push_back(1);
00911 seq.push_back(msg);
00912
00913 SequenceTest t;
00914 t.test(seq);
00915 }
00916
00917
00918
00919 int main(int argc, char **argv)
00920 {
00921 testing::InitGoogleTest(&argc, argv);
00922 ros::init(argc, argv, "im_client_test");
00923
00924 return RUN_ALL_TESTS();
00925 }