client_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 
31 #include <ros/ros.h>
32 #include <ros/console.h>
33 
34 #include <gtest/gtest.h>
35 
36 #include <tf/transform_listener.h>
37 
38 #include <interactive_markers/interactive_marker_server.h>
39 #include <interactive_markers/interactive_marker_client.h>
40 
41 #define DBG_MSG( ... ) printf( __VA_ARGS__ ); printf("\n");
42 #define DBG_MSG_STREAM( ... ) std::cout << __VA_ARGS__ << std::endl;
43 
44 using namespace interactive_markers;
45 
46 struct Msg
47 {
48  enum {
49  INIT,
50  KEEP_ALIVE,
51  UPDATE,
52  POSE,
53  DELETE,
54  TF_INFO
55  } type;
56 
57  Msg()
58  {
59  type = INIT;
60  seq_num = 0;
61  }
62 
63  uint64_t seq_num;
64 
65  std::string server_id;
66  std::string frame_id;
67  ros::Time stamp;
68 
69  std::vector<std::string> expect_reset_calls;
70  std::vector<int> expect_init_seq_num;
71  std::vector<int> expect_update_seq_num;
72 };
73 
74 std::string target_frame = "target_frame";
75 
76 
77 class SequenceTest
78 {
79  typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr;
80  typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr;
81 
82  std::vector<visualization_msgs::InteractiveMarkerInit> recv_init_msgs;
83  std::vector<visualization_msgs::InteractiveMarkerUpdate> recv_update_msgs;
84  std::vector<std::string> recv_reset_calls;
85 
86  void resetReceivedMsgs()
87  {
88  recv_init_msgs.clear();
89  recv_update_msgs.clear();
90  recv_reset_calls.clear();
91  }
92 
93  void updateCb( const UpdateConstPtr& msg )
94  {
95  DBG_MSG_STREAM( "updateCb called." );
96  recv_update_msgs.push_back( *msg );
97  }
98 
99  void initCb( const InitConstPtr& msg )
100  {
101  DBG_MSG_STREAM( "initCb called." );
102  recv_init_msgs.push_back( *msg );
103  }
104 
105  void statusCb( InteractiveMarkerClient::StatusT status,
106  const std::string& server_id,
107  const std::string& msg )
108  {
109  std::string status_string[]={"INFO","WARN","ERROR"};
110  ASSERT_TRUE( (unsigned)status < 3 );
111  DBG_MSG_STREAM( "(" << status_string[(unsigned)status] << ") " << server_id << ": " << msg );
112  }
113 
114  void resetCb( const std::string& server_id )
115  {
116  DBG_MSG_STREAM( "resetCb called." );
117  recv_reset_calls.push_back(server_id);
118  }
119 
120 public:
121  void test( std::vector<Msg> messages )
122  {
123  tf::Transformer tf;
124 
125  //tf.setTransform();
126 
127  // create an interactive marker server on the topic namespace simple_marker
128 
129  visualization_msgs::InteractiveMarker int_marker;
130  int_marker.pose.orientation.w=1;
131 
132  std::string topic_ns ="im_client_test";
133 
134  interactive_markers::InteractiveMarkerClient client(tf, target_frame, topic_ns );
135 
136  client.setInitCb( boost::bind(&SequenceTest::initCb, this, _1 ) );
137  client.setUpdateCb( boost::bind(&SequenceTest::updateCb, this, _1 ) );
138  client.setResetCb( boost::bind(&SequenceTest::resetCb, this, _1 ) );
139  client.setStatusCb( boost::bind(&SequenceTest::statusCb, this, _1, _2, _3 ) );
140 
141  std::map< int, visualization_msgs::InteractiveMarkerInit > sent_init_msgs;
142  std::map< int, visualization_msgs::InteractiveMarkerUpdate > sent_update_msgs;
143 
144  for ( size_t i=0; i<messages.size(); i++ )
145  {
146  resetReceivedMsgs();
147 
148  Msg& msg = messages[i];
149 
150  int_marker.header.frame_id=msg.frame_id;
151  int_marker.header.stamp=msg.stamp;
152  int_marker.pose.orientation.w = 1.0;
153 
154  std::ostringstream s;
155  s << i;
156  int_marker.name=s.str();
157 
158  switch( msg.type )
159  {
160  case Msg::INIT:
161  {
162  DBG_MSG_STREAM( i << " INIT: seq_num=" << msg.seq_num << " frame=" << msg.frame_id << " stamp=" << msg.stamp );
163  visualization_msgs::InteractiveMarkerInitPtr init_msg_out( new visualization_msgs::InteractiveMarkerInit() );
164  init_msg_out->server_id=msg.server_id;
165  init_msg_out->seq_num=msg.seq_num;
166  init_msg_out->markers.push_back( int_marker );
167  client.processInit( init_msg_out );
168  sent_init_msgs[msg.seq_num]=*init_msg_out;
169  break;
170  }
171  case Msg::KEEP_ALIVE:
172  {
173  DBG_MSG_STREAM( i << " KEEP_ALIVE: seq_num=" << msg.seq_num );
174  visualization_msgs::InteractiveMarkerUpdatePtr update_msg_out( new visualization_msgs::InteractiveMarkerUpdate() );
175  update_msg_out->server_id=msg.server_id;
176  update_msg_out->type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE;
177  update_msg_out->seq_num=msg.seq_num;
178 
179  client.processUpdate( update_msg_out );
180  sent_update_msgs[msg.seq_num]=*update_msg_out;
181  break;
182  }
183  case Msg::UPDATE:
184  {
185  DBG_MSG_STREAM( i << " UPDATE: seq_num=" << msg.seq_num << " frame=" << msg.frame_id << " stamp=" << msg.stamp );
186  visualization_msgs::InteractiveMarkerUpdatePtr update_msg_out( new visualization_msgs::InteractiveMarkerUpdate() );
187  update_msg_out->server_id=msg.server_id;
188  update_msg_out->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
189  update_msg_out->seq_num=msg.seq_num;
190 
191  update_msg_out->markers.push_back( int_marker );
192  client.processUpdate( update_msg_out );
193  sent_update_msgs[msg.seq_num]=*update_msg_out;
194  break;
195  }
196  case Msg::POSE:
197  {
198  DBG_MSG_STREAM( i << " POSE: seq_num=" << msg.seq_num << " frame=" << msg.frame_id << " stamp=" << msg.stamp );
199  visualization_msgs::InteractiveMarkerUpdatePtr update_msg_out( new visualization_msgs::InteractiveMarkerUpdate() );
200  update_msg_out->server_id=msg.server_id;
201  update_msg_out->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
202  update_msg_out->seq_num=msg.seq_num;
203 
204  visualization_msgs::InteractiveMarkerPose pose;
205  pose.header=int_marker.header;
206  pose.name=int_marker.name;
207  pose.pose=int_marker.pose;
208  update_msg_out->poses.push_back( pose );
209  client.processUpdate( update_msg_out );
210  sent_update_msgs[msg.seq_num]=*update_msg_out;
211  break;
212  }
213  case Msg::DELETE:
214  {
215  DBG_MSG_STREAM( i << " DELETE: seq_num=" << msg.seq_num );
216  visualization_msgs::InteractiveMarkerUpdatePtr update_msg_out( new visualization_msgs::InteractiveMarkerUpdate() );
217  update_msg_out->server_id="/im_client_test/test_server";
218  update_msg_out->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
219  update_msg_out->seq_num=msg.seq_num;
220 
221  update_msg_out->erases.push_back( int_marker.name );
222  client.processUpdate( update_msg_out );
223  sent_update_msgs[msg.seq_num]=*update_msg_out;
224  break;
225  }
226  case Msg::TF_INFO:
227  {
228  DBG_MSG_STREAM( i << " TF_INFO: " << msg.frame_id << " -> " << target_frame << " at time " << msg.stamp.toSec() );
229  tf::StampedTransform stf;
230  stf.frame_id_=msg.frame_id;
231  stf.child_frame_id_=target_frame;
232  stf.stamp_=msg.stamp;
233  stf.setIdentity();
234  tf.setTransform( stf, msg.server_id );
235  break;
236  }
237  }
238 
239  /*
240  ASSERT_EQ( 0, recv_update_msgs.size() );
241  ASSERT_EQ( 0, recv_init_msgs.size() );
242  ASSERT_EQ( 0, recv_reset_calls.size() );
243  */
244 
245  client.update();
246 
247  ASSERT_EQ( msg.expect_update_seq_num.size(), recv_update_msgs.size() );
248  ASSERT_EQ( msg.expect_init_seq_num.size(), recv_init_msgs.size() );
249  ASSERT_EQ( msg.expect_reset_calls.size(), recv_reset_calls.size() );
250 
251  for ( size_t u=0; u<msg.expect_update_seq_num.size(); u++ )
252  {
253  ASSERT_TRUE( sent_update_msgs.find(msg.expect_update_seq_num[u]) != sent_update_msgs.end() );
254 
255  visualization_msgs::InteractiveMarkerUpdate sent_msg = sent_update_msgs[msg.expect_update_seq_num[u]];
256  visualization_msgs::InteractiveMarkerUpdate recv_msg = recv_update_msgs[ u ];
257 
258  //sanity check
259  ASSERT_EQ( sent_msg.seq_num, msg.expect_update_seq_num[u] );
260 
261  //chech sequence number
262  ASSERT_EQ( recv_msg.seq_num, msg.expect_update_seq_num[u] );
263 
264  // check sent/received messages for equality
265  ASSERT_EQ( recv_msg.markers.size(), sent_msg.markers.size() );
266  ASSERT_EQ( recv_msg.poses.size(), sent_msg.poses.size() );
267  ASSERT_EQ( recv_msg.erases.size(), sent_msg.erases.size() );
268 
269  // check that messages are equal
270  // and everything is transformed into the target frame
271  for ( size_t m=0; m<sent_msg.markers.size(); m++ )
272  {
273  ASSERT_EQ( recv_msg.markers[m].name, sent_msg.markers[m].name );
274  ASSERT_EQ( recv_msg.markers[m].header.stamp, sent_msg.markers[m].header.stamp );
275  if ( sent_msg.markers[m].header.stamp == ros::Time(0) )
276  {
277  ASSERT_EQ( target_frame, sent_msg.markers[m].header.frame_id );
278  }
279  else
280  {
281  ASSERT_EQ( target_frame, recv_msg.markers[m].header.frame_id );
282  }
283  }
284  for ( size_t p=0; p<sent_msg.poses.size(); p++ )
285  {
286  ASSERT_EQ( recv_msg.poses[p].name, sent_msg.poses[p].name );
287  ASSERT_EQ( recv_msg.poses[p].header.stamp, sent_msg.poses[p].header.stamp );
288  if ( sent_msg.poses[p].header.stamp == ros::Time(0) )
289  {
290  ASSERT_EQ( target_frame, sent_msg.poses[p].header.frame_id );
291  }
292  else
293  {
294  ASSERT_EQ( target_frame, recv_msg.poses[p].header.frame_id );
295  }
296  }
297  for ( size_t e=0; e<sent_msg.erases.size(); e++ )
298  {
299  ASSERT_EQ( recv_msg.erases[e], sent_msg.erases[e] );
300  }
301  }
302 
303 
304  for ( size_t u=0; u<msg.expect_init_seq_num.size(); u++ )
305  {
306  ASSERT_TRUE( sent_init_msgs.find(msg.expect_init_seq_num[u]) != sent_init_msgs.end() );
307 
308  visualization_msgs::InteractiveMarkerInit sent_msg = sent_init_msgs[msg.expect_init_seq_num[u]];
309  visualization_msgs::InteractiveMarkerInit recv_msg = recv_init_msgs[ u ];
310 
311  //sanity check
312  ASSERT_EQ( sent_msg.seq_num, msg.expect_init_seq_num[u] );
313 
314  //chech sequence number
315  ASSERT_EQ( recv_msg.seq_num, msg.expect_init_seq_num[u] );
316 
317  // check sent/received messages for equality
318  ASSERT_EQ( recv_msg.markers.size(), sent_msg.markers.size() );
319 
320  // check that messages are equal
321  // and everything is transformed into the target frame
322  for ( size_t m=0; m<sent_msg.markers.size(); m++ )
323  {
324  ASSERT_EQ( recv_msg.markers[m].name, sent_msg.markers[m].name );
325  ASSERT_EQ( recv_msg.markers[m].header.stamp, sent_msg.markers[m].header.stamp );
326  if ( sent_msg.markers[m].header.stamp == ros::Time(0) )
327  {
328  // check if we can transform frame-locked messages
329  ASSERT_TRUE( tf.canTransform( target_frame, sent_msg.markers[m].header.frame_id, ros::Time(0) ) );
330  }
331  else
332  {
333  // check if non-framelocked messages are being transformed for us
334  ASSERT_EQ( target_frame, recv_msg.markers[m].header.frame_id );
335  }
336  }
337  }
338  }
339  }
340 };
341 
342 TEST(InteractiveMarkerClient, init_simple1)
343 {
344  Msg msg;
345 
346  std::vector<Msg> seq;
347 
348  msg.type=Msg::INIT;
349  msg.seq_num=0;
350  msg.server_id="server1";
351  msg.frame_id=target_frame;
352  seq.push_back(msg);
353 
354  msg.type=Msg::KEEP_ALIVE;
355  msg.expect_init_seq_num.push_back(0);
356  seq.push_back(msg);
357 
358  SequenceTest t;
359  t.test(seq);
360 }
361 
362 TEST(InteractiveMarkerClient, init_simple2)
363 {
364  Msg msg;
365 
366  std::vector<Msg> seq;
367 
368  msg.type=Msg::INIT;
369  msg.seq_num=0;
370  msg.server_id="server1";
371  msg.frame_id=target_frame;
372  seq.push_back(msg);
373 
374  msg.type=Msg::UPDATE;
375  msg.expect_init_seq_num.push_back(0);
376  seq.push_back(msg);
377 
378  SequenceTest t;
379  t.test(seq);
380 }
381 
382 
383 TEST(InteractiveMarkerClient, init_many_inits)
384 {
385  Msg msg;
386 
387  std::vector<Msg> seq;
388 
389  for ( int i=0; i<200; i++ )
390  {
391  msg.type=Msg::INIT;
392  msg.seq_num=i;
393  msg.server_id="server1";
394  msg.frame_id=target_frame;
395  seq.push_back(msg);
396  }
397 
398  // this update should be ommitted
399  msg.type=Msg::UPDATE;
400  msg.expect_init_seq_num.push_back(msg.seq_num);
401  seq.push_back(msg);
402 
403  // this update should go through
404  msg.seq_num++;
405  msg.expect_init_seq_num.clear();
406  msg.expect_update_seq_num.push_back(msg.seq_num);
407  seq.push_back(msg);
408 
409  SequenceTest t;
410  t.test(seq);
411 }
412 
413 TEST(InteractiveMarkerClient, init_many_updates)
414 {
415  Msg msg;
416 
417  std::vector<Msg> seq;
418 
419  for ( int i=0; i<200; i++ )
420  {
421  msg.type=Msg::UPDATE;
422  msg.seq_num=i;
423  msg.server_id="server1";
424  msg.frame_id=target_frame;
425  seq.push_back(msg);
426  }
427 
428  msg.type=Msg::INIT;
429  msg.seq_num=190;
430  msg.expect_init_seq_num.push_back(msg.seq_num);
431  for ( int i=191; i<200; i++ )
432  {
433  msg.expect_update_seq_num.push_back(i);
434  }
435  seq.push_back(msg);
436 
437  SequenceTest t;
438  t.test(seq);
439 }
440 
441 TEST(InteractiveMarkerClient, init_invalid_tf)
442 {
443  Msg msg;
444 
445  std::vector<Msg> seq;
446 
447  msg.type=Msg::INIT;
448  msg.seq_num=0;
449  msg.server_id="server1";
450  msg.frame_id="invalid_frame";
451  seq.push_back(msg);
452 
453  msg.type=Msg::INIT;
454  msg.seq_num=1;
455  msg.server_id="server1";
456  msg.frame_id=target_frame;
457  seq.push_back(msg);
458 
459  msg.type=Msg::KEEP_ALIVE;
460  msg.expect_init_seq_num.push_back(1);
461  seq.push_back(msg);
462 
463  SequenceTest t;
464  t.test(seq);
465 }
466 
467 TEST(InteractiveMarkerClient, init_wait_tf)
468 {
469  Msg msg;
470 
471  std::vector<Msg> seq;
472 
473  // initial tf info needed so wait_frame is in the tf tree
474  msg.type=Msg::TF_INFO;
475  msg.server_id="server1";
476  msg.frame_id="wait_frame";
477  msg.stamp=ros::Time(1.0);
478  seq.push_back(msg);
479 
480  // send init message that lives in the future
481  msg.type=Msg::INIT;
482  msg.seq_num=0;
483  msg.stamp=ros::Time(2.0);
484  seq.push_back(msg);
485 
486  msg.type=Msg::KEEP_ALIVE;
487  seq.push_back(msg);
488 
489  // send tf info -> message should get passed through
490  msg.type=Msg::TF_INFO;
491  msg.expect_init_seq_num.push_back(0);
492  seq.push_back(msg);
493 
494  // send update message that lives in the future
495  msg.type=Msg::UPDATE;
496  msg.seq_num=1;
497  msg.stamp=ros::Time(3.0);
498  msg.expect_init_seq_num.clear();
499  seq.push_back(msg);
500 
501  // send tf info -> message should get passed through
502  msg.type=Msg::TF_INFO;
503  msg.expect_update_seq_num.push_back(1);
504  seq.push_back(msg);
505 
506  // send pose message that lives in the future
507  msg.type=Msg::POSE;
508  msg.seq_num=2;
509  msg.stamp=ros::Time(4.0);
510  msg.expect_update_seq_num.clear();
511  seq.push_back(msg);
512 
513  // send tf info -> message should get passed through
514  msg.type=Msg::TF_INFO;
515  msg.expect_update_seq_num.push_back(2);
516  seq.push_back(msg);
517 
518  SequenceTest t;
519  t.test(seq);
520 }
521 
522 
523 TEST(InteractiveMarkerClient, init_wait_tf_zerotime)
524 {
525  Msg msg;
526 
527  std::vector<Msg> seq;
528 
529  // send init message with zero timestamp and non-existing tf frame
530  msg.type=Msg::INIT;
531  msg.server_id="server1";
532  msg.frame_id="wait_frame";
533  msg.seq_num=0;
534  msg.stamp=ros::Time(0.0);
535  seq.push_back(msg);
536 
537  msg.type=Msg::KEEP_ALIVE;
538  seq.push_back(msg);
539 
540  // send tf info -> message should get passed through
541  msg.type=Msg::TF_INFO;
542  msg.stamp=ros::Time(1.0);
543  msg.expect_init_seq_num.push_back(0);
544  seq.push_back(msg);
545 
546  SequenceTest t;
547  t.test(seq);
548 }
549 
550 
551 TEST(InteractiveMarkerClient, init_wait_tf_inverse)
552 {
553  // send messages with timestamps going backwards
554  // they should still be delivered in the right order
555  // according to their seq_num
556 
557  Msg msg;
558 
559  std::vector<Msg> seq;
560 
561  // initial tf info needed so wait_frame is in the tf tree
562  msg.type=Msg::TF_INFO;
563  msg.server_id="server1";
564  msg.frame_id="wait_frame";
565  msg.stamp=ros::Time(1.0);
566  seq.push_back(msg);
567 
568  msg.type=Msg::INIT;
569  msg.seq_num=0;
570  msg.stamp=ros::Time(6);
571  seq.push_back(msg);
572 
573  msg.type=Msg::INIT;
574  msg.seq_num=1;
575  msg.stamp=ros::Time(5);
576  seq.push_back(msg);
577 
578  msg.type=Msg::KEEP_ALIVE;
579  msg.seq_num=0;
580  seq.push_back(msg);
581 
582  msg.type=Msg::UPDATE;
583  msg.seq_num=1;
584  msg.stamp=ros::Time(5);
585  seq.push_back(msg);
586 
587  msg.type=Msg::UPDATE;
588  msg.seq_num=2;
589  msg.stamp=ros::Time(4);
590  seq.push_back(msg);
591 
592  msg.type=Msg::UPDATE;
593  msg.seq_num=3;
594  msg.stamp=ros::Time(3);
595  seq.push_back(msg);
596 
597  msg.type=Msg::TF_INFO;
598  msg.stamp=ros::Time(2);
599  seq.push_back(msg);
600 
601  msg.type=Msg::TF_INFO;
602  msg.stamp=ros::Time(3);
603  seq.push_back(msg);
604 
605  msg.type=Msg::TF_INFO;
606  msg.stamp=ros::Time(4);
607  seq.push_back(msg);
608 
609  // as soon as tf info for init #1 is there,
610  // all messages should go through
611  msg.stamp=ros::Time(5);
612  msg.expect_init_seq_num.push_back(1);
613  msg.expect_update_seq_num.push_back(2);
614  msg.expect_update_seq_num.push_back(3);
615  seq.push_back(msg);
616 
617  SequenceTest t;
618  t.test(seq);
619 }
620 
621 TEST(InteractiveMarkerClient, wait_tf_inverse)
622 {
623  // send messages with timestamps going backwards
624  // they should still be delivered in the right order
625  // according to their seq_num
626 
627  Msg msg;
628 
629  std::vector<Msg> seq;
630 
631  // initial tf info needed so wait_frame is in the tf tree
632  msg.type=Msg::TF_INFO;
633  msg.server_id="server1";
634  msg.frame_id="wait_frame";
635  msg.stamp=ros::Time(1);
636  seq.push_back(msg);
637 
638  msg.type=Msg::INIT;
639  msg.seq_num=0;
640  seq.push_back(msg);
641 
642  msg.type=Msg::KEEP_ALIVE;
643  msg.seq_num=0;
644  msg.expect_init_seq_num.push_back(0);
645  seq.push_back(msg);
646 
647  msg.expect_init_seq_num.clear();
648 
649  // init complete
650 
651  msg.type=Msg::UPDATE;
652  msg.seq_num=1;
653  msg.stamp=ros::Time(5);
654  seq.push_back(msg);
655 
656  msg.type=Msg::UPDATE;
657  msg.seq_num=2;
658  msg.stamp=ros::Time(4);
659  seq.push_back(msg);
660 
661  msg.type=Msg::UPDATE;
662  msg.seq_num=3;
663  msg.stamp=ros::Time(3);
664  seq.push_back(msg);
665 
666  msg.type=Msg::TF_INFO;
667  msg.stamp=ros::Time(3);
668  seq.push_back(msg);
669 
670  msg.type=Msg::TF_INFO;
671  msg.stamp=ros::Time(4);
672  seq.push_back(msg);
673 
674  // all messages should go through in the right order
675  msg.type=Msg::TF_INFO;
676  msg.stamp=ros::Time(5);
677  msg.expect_update_seq_num.push_back(1);
678  msg.expect_update_seq_num.push_back(2);
679  msg.expect_update_seq_num.push_back(3);
680  seq.push_back(msg);
681 
682  SequenceTest t;
683  t.test(seq);
684 }
685 
686 
687 TEST(InteractiveMarkerClient, wrong_seq_num1)
688 {
689  Msg msg;
690 
691  std::vector<Msg> seq;
692 
693  msg.type=Msg::INIT;
694  msg.seq_num=0;
695  msg.server_id="server1";
696  msg.frame_id=target_frame;
697  seq.push_back(msg);
698 
699  msg.type=Msg::KEEP_ALIVE;
700  msg.expect_init_seq_num.push_back(0);
701  seq.push_back(msg);
702 
703  msg.expect_init_seq_num.clear();
704 
705  msg.type=Msg::KEEP_ALIVE;
706  msg.seq_num=1;
707  msg.expect_reset_calls.push_back(msg.server_id);
708  seq.push_back(msg);
709 
710  SequenceTest t;
711  t.test(seq);
712 }
713 
714 TEST(InteractiveMarkerClient, wrong_seq_num2)
715 {
716  Msg msg;
717 
718  std::vector<Msg> seq;
719 
720  msg.type=Msg::INIT;
721  msg.seq_num=1;
722  msg.server_id="server1";
723  msg.frame_id=target_frame;
724  seq.push_back(msg);
725 
726  msg.type=Msg::KEEP_ALIVE;
727  msg.expect_init_seq_num.push_back(1);
728  seq.push_back(msg);
729 
730  msg.expect_init_seq_num.clear();
731 
732  msg.type=Msg::KEEP_ALIVE;
733  msg.seq_num=0;
734  msg.expect_reset_calls.push_back(msg.server_id);
735  seq.push_back(msg);
736 
737  SequenceTest t;
738  t.test(seq);
739 }
740 
741 TEST(InteractiveMarkerClient, wrong_seq_num3)
742 {
743  Msg msg;
744 
745  std::vector<Msg> seq;
746 
747  msg.type=Msg::INIT;
748  msg.seq_num=1;
749  msg.server_id="server1";
750  msg.frame_id=target_frame;
751  seq.push_back(msg);
752 
753  msg.type=Msg::KEEP_ALIVE;
754  msg.expect_init_seq_num.push_back(1);
755  seq.push_back(msg);
756 
757  msg.expect_init_seq_num.clear();
758 
759  msg.type=Msg::UPDATE;
760  msg.seq_num=1;
761  msg.expect_reset_calls.push_back(msg.server_id);
762  seq.push_back(msg);
763 
764  SequenceTest t;
765  t.test(seq);
766 }
767 
768 TEST(InteractiveMarkerClient, wrong_seq_num4)
769 {
770  Msg msg;
771 
772  std::vector<Msg> seq;
773 
774  msg.type=Msg::INIT;
775  msg.seq_num=1;
776  msg.server_id="server1";
777  msg.frame_id=target_frame;
778  seq.push_back(msg);
779 
780  msg.type=Msg::KEEP_ALIVE;
781  msg.expect_init_seq_num.push_back(1);
782  seq.push_back(msg);
783 
784  msg.expect_init_seq_num.clear();
785 
786  msg.type=Msg::UPDATE;
787  msg.seq_num=2;
788  msg.expect_update_seq_num.push_back(2);
789  seq.push_back(msg);
790 
791  msg.expect_update_seq_num.clear();
792 
793  msg.type=Msg::UPDATE;
794  msg.seq_num=2;
795  msg.expect_reset_calls.push_back(msg.server_id);
796  seq.push_back(msg);
797 
798  SequenceTest t;
799  t.test(seq);
800 }
801 
802 TEST(InteractiveMarkerClient, wrong_seq_num5)
803 {
804  Msg msg;
805 
806  std::vector<Msg> seq;
807 
808  msg.type=Msg::INIT;
809  msg.seq_num=1;
810  msg.server_id="server1";
811  msg.frame_id=target_frame;
812  seq.push_back(msg);
813 
814  msg.type=Msg::KEEP_ALIVE;
815  msg.expect_init_seq_num.push_back(1);
816  seq.push_back(msg);
817 
818  msg.expect_init_seq_num.clear();
819 
820  msg.type=Msg::UPDATE;
821  msg.seq_num=2;
822  msg.expect_update_seq_num.push_back(2);
823  seq.push_back(msg);
824 
825  msg.expect_update_seq_num.clear();
826 
827  msg.type=Msg::UPDATE;
828  msg.seq_num=1;
829  msg.expect_reset_calls.push_back(msg.server_id);
830  seq.push_back(msg);
831 
832  SequenceTest t;
833  t.test(seq);
834 }
835 
836 TEST(InteractiveMarkerClient, wrong_seq_num6)
837 {
838  Msg msg;
839 
840  std::vector<Msg> seq;
841 
842  msg.type=Msg::INIT;
843  msg.seq_num=1;
844  msg.server_id="server1";
845  msg.frame_id=target_frame;
846  seq.push_back(msg);
847 
848  msg.type=Msg::KEEP_ALIVE;
849  msg.expect_init_seq_num.push_back(1);
850  seq.push_back(msg);
851 
852  msg.expect_init_seq_num.clear();
853 
854  msg.type=Msg::UPDATE;
855  msg.seq_num=2;
856  msg.expect_update_seq_num.push_back(2);
857  seq.push_back(msg);
858 
859  msg.expect_update_seq_num.clear();
860 
861  msg.type=Msg::UPDATE;
862  msg.seq_num=4;
863  msg.expect_reset_calls.push_back(msg.server_id);
864  seq.push_back(msg);
865 
866  SequenceTest t;
867  t.test(seq);
868 }
869 
870 
871 TEST(InteractiveMarkerClient, init_twoservers)
872 {
873  Msg msg;
874 
875  std::vector<Msg> seq;
876 
877  msg.type=Msg::INIT;
878  msg.seq_num=0;
879  msg.server_id="server1";
880  msg.frame_id=target_frame;
881  seq.push_back(msg);
882 
883  msg.type=Msg::KEEP_ALIVE;
884  msg.expect_init_seq_num.push_back(0);
885  seq.push_back(msg);
886 
887  msg.expect_init_seq_num.clear();
888 
889  msg.type=Msg::UPDATE;
890  msg.seq_num=1;
891  msg.expect_update_seq_num.push_back(1);
892  seq.push_back(msg);
893 
894  msg.expect_update_seq_num.clear();
895 
896  msg.type=Msg::INIT;
897  msg.seq_num=0;
898  msg.server_id="server2";
899  msg.frame_id=target_frame;
900  seq.push_back(msg);
901 
902  msg.type=Msg::KEEP_ALIVE;
903  msg.expect_init_seq_num.push_back(0);
904  seq.push_back(msg);
905 
906  msg.expect_init_seq_num.clear();
907 
908  msg.type=Msg::UPDATE;
909  msg.seq_num=1;
910  msg.expect_update_seq_num.push_back(1);
911  seq.push_back(msg);
912 
913  SequenceTest t;
914  t.test(seq);
915 }
916 
917 
918 // Run all the tests that were declared with TEST()
919 int main(int argc, char **argv)
920 {
921  testing::InitGoogleTest(&argc, argv);
922  ros::init(argc, argv, "im_client_test");
923  //ros::NodeHandle nh;
924  return RUN_ALL_TESTS();
925 }
SequenceTest::resetCb
void resetCb(const std::string &server_id)
Definition: client_test.cpp:114
SequenceTest::initCb
void initCb(const InitConstPtr &msg)
Definition: client_test.cpp:99
transform_listener.h
Msg::header
Header header
resetReceivedMsgs
void resetReceivedMsgs()
Definition: server_client_test.cpp:60
interactive_markers::InteractiveMarkerClient::StatusT
StatusT
Definition: interactive_marker_client.h:71
SequenceTest::recv_update_msgs
std::vector< visualization_msgs::InteractiveMarkerUpdate > recv_update_msgs
Definition: client_test.cpp:83
SequenceTest::InitConstPtr
visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr
Definition: client_test.cpp:79
target_frame
std::string target_frame
Definition: client_test.cpp:74
s
XmlRpcServer s
interactive_markers::InteractiveMarkerClient::processInit
void processInit(const InitConstPtr &msg)
Definition: interactive_marker_client.cpp:208
SequenceTest::updateCb
void updateCb(const UpdateConstPtr &msg)
Definition: client_test.cpp:93
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
SequenceTest::statusCb
void statusCb(InteractiveMarkerClient::StatusT status, const std::string &server_id, const std::string &msg)
Definition: client_test.cpp:105
DBG_MSG_STREAM
#define DBG_MSG_STREAM(...)
Definition: client_test.cpp:42
tf::Transformer::setTransform
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
interactive_markers::InteractiveMarkerClient::setInitCb
void setInitCb(const InitCallback &cb)
Set callback for init messages.
Definition: interactive_marker_client.cpp:75
interactive_marker_client.h
tf::Transformer::canTransform
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
TEST
TEST(InteractiveMarkerClient, init_simple1)
Definition: client_test.cpp:342
interactive_markers::InteractiveMarkerClient::setResetCb
void setResetCb(const ResetCallback &cb)
Set callback for resetting one server connection.
Definition: interactive_marker_client.cpp:85
interactive_markers::InteractiveMarkerClient::setUpdateCb
void setUpdateCb(const UpdateCallback &cb)
Set callback for update messages.
Definition: interactive_marker_client.cpp:80
interactive_markers::InteractiveMarkerClient::setStatusCb
void setStatusCb(const StatusCallback &cb)
Set callback for status updates.
Definition: interactive_marker_client.cpp:90
SequenceTest::resetReceivedMsgs
void resetReceivedMsgs()
Definition: client_test.cpp:86
interactive_markers::InteractiveMarkerClient::processUpdate
void processUpdate(const UpdateConstPtr &msg)
Definition: interactive_marker_client.cpp:213
SequenceTest::recv_reset_calls
std::vector< std::string > recv_reset_calls
Definition: client_test.cpp:84
interactive_markers
Definition: message_context.h:45
main
int main(int argc, char **argv)
Definition: client_test.cpp:919
SequenceTest::test
void test(std::vector< Msg > messages)
Definition: client_test.cpp:121
interactive_markers::InteractiveMarkerClient
Definition: interactive_marker_client.h:67
interactive_markers::InteractiveMarkerClient::update
void update()
Update tf info, call callbacks.
Definition: interactive_marker_client.cpp:218
interactive_marker_server.h
SequenceTest::recv_init_msgs
std::vector< visualization_msgs::InteractiveMarkerInit > recv_init_msgs
Definition: client_test.cpp:82
SequenceTest::UpdateConstPtr
visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr
Definition: client_test.cpp:80
tf::StampedTransform::frame_id_
std::string frame_id_

interactive_markers
Author(s): David Gossow
autogenerated on Sun Feb 3 2019 03:24:09