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

interactive_markers
Author(s): David Gossow, William Woodall
autogenerated on Fri Oct 27 2023 02:31:54