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


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