34 #include <gtest/gtest.h> 41 #define DBG_MSG( ... ) printf( __VA_ARGS__ ); printf("\n"); 42 #define DBG_MSG_STREAM( ... ) std::cout << __VA_ARGS__ << std::endl; 65 std::string server_id;
69 std::vector<std::string> expect_reset_calls;
70 std::vector<int> expect_init_seq_num;
71 std::vector<int> expect_update_seq_num;
79 typedef visualization_msgs::InteractiveMarkerInitConstPtr
InitConstPtr;
88 recv_init_msgs.clear();
89 recv_update_msgs.clear();
90 recv_reset_calls.clear();
96 recv_update_msgs.push_back( *msg );
99 void initCb(
const InitConstPtr& msg )
102 recv_init_msgs.push_back( *msg );
106 const std::string& server_id,
107 const std::string& msg )
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 );
117 recv_reset_calls.push_back(server_id);
121 void test( std::vector<Msg> messages )
129 visualization_msgs::InteractiveMarker int_marker;
130 int_marker.pose.orientation.w=1;
132 std::string topic_ns =
"im_client_test";
141 std::map< int, visualization_msgs::InteractiveMarkerInit > sent_init_msgs;
142 std::map< int, visualization_msgs::InteractiveMarkerUpdate > sent_update_msgs;
144 for (
size_t i=0; i<messages.size(); i++ )
148 Msg& msg = messages[i];
150 int_marker.
header.frame_id=msg.frame_id;
151 int_marker.header.stamp=msg.stamp;
152 int_marker.pose.orientation.w = 1.0;
154 std::ostringstream
s;
156 int_marker.name=s.str();
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 );
168 sent_init_msgs[msg.seq_num]=*init_msg_out;
171 case Msg::KEEP_ALIVE:
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;
180 sent_update_msgs[msg.seq_num]=*update_msg_out;
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;
191 update_msg_out->markers.push_back( int_marker );
193 sent_update_msgs[msg.seq_num]=*update_msg_out;
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;
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 );
210 sent_update_msgs[msg.seq_num]=*update_msg_out;
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;
221 update_msg_out->erases.push_back( int_marker.name );
223 sent_update_msgs[msg.seq_num]=*update_msg_out;
232 stf.stamp_=msg.stamp;
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() );
251 for (
size_t u=0; u<msg.expect_update_seq_num.size(); u++ )
253 ASSERT_TRUE( sent_update_msgs.find(msg.expect_update_seq_num[u]) != sent_update_msgs.end() );
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 ];
259 ASSERT_EQ( sent_msg.seq_num, msg.expect_update_seq_num[u] );
262 ASSERT_EQ( recv_msg.seq_num, msg.expect_update_seq_num[u] );
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() );
271 for (
size_t m=0; m<sent_msg.markers.size(); m++ )
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) )
277 ASSERT_EQ(
target_frame, sent_msg.markers[m].header.frame_id );
281 ASSERT_EQ(
target_frame, recv_msg.markers[m].header.frame_id );
284 for (
size_t p=0; p<sent_msg.poses.size(); p++ )
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) )
290 ASSERT_EQ(
target_frame, sent_msg.poses[p].header.frame_id );
294 ASSERT_EQ(
target_frame, recv_msg.poses[p].header.frame_id );
297 for (
size_t e=0; e<sent_msg.erases.size(); e++ )
299 ASSERT_EQ( recv_msg.erases[e], sent_msg.erases[e] );
304 for (
size_t u=0; u<msg.expect_init_seq_num.size(); u++ )
306 ASSERT_TRUE( sent_init_msgs.find(msg.expect_init_seq_num[u]) != sent_init_msgs.end() );
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 ];
312 ASSERT_EQ( sent_msg.seq_num, msg.expect_init_seq_num[u] );
315 ASSERT_EQ( recv_msg.seq_num, msg.expect_init_seq_num[u] );
318 ASSERT_EQ( recv_msg.markers.size(), sent_msg.markers.size() );
322 for (
size_t m=0; m<sent_msg.markers.size(); m++ )
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) )
334 ASSERT_EQ(
target_frame, recv_msg.markers[m].header.frame_id );
346 std::vector<Msg> seq;
350 msg.server_id=
"server1";
354 msg.type=Msg::KEEP_ALIVE;
355 msg.expect_init_seq_num.push_back(0);
366 std::vector<Msg> seq;
370 msg.server_id=
"server1";
374 msg.type=Msg::UPDATE;
375 msg.expect_init_seq_num.push_back(0);
387 std::vector<Msg> seq;
389 for (
int i=0; i<200; i++ )
393 msg.server_id=
"server1";
399 msg.type=Msg::UPDATE;
400 msg.expect_init_seq_num.push_back(msg.seq_num);
405 msg.expect_init_seq_num.clear();
406 msg.expect_update_seq_num.push_back(msg.seq_num);
417 std::vector<Msg> seq;
419 for (
int i=0; i<200; i++ )
421 msg.type=Msg::UPDATE;
423 msg.server_id=
"server1";
430 msg.expect_init_seq_num.push_back(msg.seq_num);
431 for (
int i=191; i<200; i++ )
433 msg.expect_update_seq_num.push_back(i);
445 std::vector<Msg> seq;
449 msg.server_id=
"server1";
450 msg.frame_id=
"invalid_frame";
455 msg.server_id=
"server1";
459 msg.type=Msg::KEEP_ALIVE;
460 msg.expect_init_seq_num.push_back(1);
471 std::vector<Msg> seq;
474 msg.type=Msg::TF_INFO;
475 msg.server_id=
"server1";
476 msg.frame_id=
"wait_frame";
486 msg.type=Msg::KEEP_ALIVE;
490 msg.type=Msg::TF_INFO;
491 msg.expect_init_seq_num.push_back(0);
495 msg.type=Msg::UPDATE;
498 msg.expect_init_seq_num.clear();
502 msg.type=Msg::TF_INFO;
503 msg.expect_update_seq_num.push_back(1);
510 msg.expect_update_seq_num.clear();
514 msg.type=Msg::TF_INFO;
515 msg.expect_update_seq_num.push_back(2);
527 std::vector<Msg> seq;
531 msg.server_id=
"server1";
532 msg.frame_id=
"wait_frame";
537 msg.type=Msg::KEEP_ALIVE;
541 msg.type=Msg::TF_INFO;
543 msg.expect_init_seq_num.push_back(0);
559 std::vector<Msg> seq;
562 msg.type=Msg::TF_INFO;
563 msg.server_id=
"server1";
564 msg.frame_id=
"wait_frame";
578 msg.type=Msg::KEEP_ALIVE;
582 msg.type=Msg::UPDATE;
587 msg.type=Msg::UPDATE;
592 msg.type=Msg::UPDATE;
597 msg.type=Msg::TF_INFO;
601 msg.type=Msg::TF_INFO;
605 msg.type=Msg::TF_INFO;
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);
629 std::vector<Msg> seq;
632 msg.type=Msg::TF_INFO;
633 msg.server_id=
"server1";
634 msg.frame_id=
"wait_frame";
642 msg.type=Msg::KEEP_ALIVE;
644 msg.expect_init_seq_num.push_back(0);
647 msg.expect_init_seq_num.clear();
651 msg.type=Msg::UPDATE;
656 msg.type=Msg::UPDATE;
661 msg.type=Msg::UPDATE;
666 msg.type=Msg::TF_INFO;
670 msg.type=Msg::TF_INFO;
675 msg.type=Msg::TF_INFO;
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);
691 std::vector<Msg> seq;
695 msg.server_id=
"server1";
699 msg.type=Msg::KEEP_ALIVE;
700 msg.expect_init_seq_num.push_back(0);
703 msg.expect_init_seq_num.clear();
705 msg.type=Msg::KEEP_ALIVE;
707 msg.expect_reset_calls.push_back(msg.server_id);
718 std::vector<Msg> seq;
722 msg.server_id=
"server1";
726 msg.type=Msg::KEEP_ALIVE;
727 msg.expect_init_seq_num.push_back(1);
730 msg.expect_init_seq_num.clear();
732 msg.type=Msg::KEEP_ALIVE;
734 msg.expect_reset_calls.push_back(msg.server_id);
745 std::vector<Msg> seq;
749 msg.server_id=
"server1";
753 msg.type=Msg::KEEP_ALIVE;
754 msg.expect_init_seq_num.push_back(1);
757 msg.expect_init_seq_num.clear();
759 msg.type=Msg::UPDATE;
761 msg.expect_reset_calls.push_back(msg.server_id);
772 std::vector<Msg> seq;
776 msg.server_id=
"server1";
780 msg.type=Msg::KEEP_ALIVE;
781 msg.expect_init_seq_num.push_back(1);
784 msg.expect_init_seq_num.clear();
786 msg.type=Msg::UPDATE;
788 msg.expect_update_seq_num.push_back(2);
791 msg.expect_update_seq_num.clear();
793 msg.type=Msg::UPDATE;
795 msg.expect_reset_calls.push_back(msg.server_id);
806 std::vector<Msg> seq;
810 msg.server_id=
"server1";
814 msg.type=Msg::KEEP_ALIVE;
815 msg.expect_init_seq_num.push_back(1);
818 msg.expect_init_seq_num.clear();
820 msg.type=Msg::UPDATE;
822 msg.expect_update_seq_num.push_back(2);
825 msg.expect_update_seq_num.clear();
827 msg.type=Msg::UPDATE;
829 msg.expect_reset_calls.push_back(msg.server_id);
840 std::vector<Msg> seq;
844 msg.server_id=
"server1";
848 msg.type=Msg::KEEP_ALIVE;
849 msg.expect_init_seq_num.push_back(1);
852 msg.expect_init_seq_num.clear();
854 msg.type=Msg::UPDATE;
856 msg.expect_update_seq_num.push_back(2);
859 msg.expect_update_seq_num.clear();
861 msg.type=Msg::UPDATE;
863 msg.expect_reset_calls.push_back(msg.server_id);
875 std::vector<Msg> seq;
879 msg.server_id=
"server1";
883 msg.type=Msg::KEEP_ALIVE;
884 msg.expect_init_seq_num.push_back(0);
887 msg.expect_init_seq_num.clear();
889 msg.type=Msg::UPDATE;
891 msg.expect_update_seq_num.push_back(1);
894 msg.expect_update_seq_num.clear();
898 msg.server_id=
"server2";
902 msg.type=Msg::KEEP_ALIVE;
903 msg.expect_init_seq_num.push_back(0);
906 msg.expect_init_seq_num.clear();
908 msg.type=Msg::UPDATE;
910 msg.expect_update_seq_num.push_back(1);
919 int main(
int argc,
char **argv)
921 testing::InitGoogleTest(&argc, argv);
924 return RUN_ALL_TESTS();
void resetCb(const std::string &server_id)
void initCb(const InitConstPtr &msg)
std::vector< visualization_msgs::InteractiveMarkerUpdate > recv_update_msgs
visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr
void processInit(const InitConstPtr &msg)
void updateCb(const UpdateConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void statusCb(InteractiveMarkerClient::StatusT status, const std::string &server_id, const std::string &msg)
#define DBG_MSG_STREAM(...)
INTERACTIVE_MARKERS_PUBLIC void setInitCb(const InitCallback &cb)
Set callback for init messages.
TEST(InteractiveMarkerClient, init_simple1)
INTERACTIVE_MARKERS_PUBLIC void setResetCb(const ResetCallback &cb)
Set callback for resetting one server connection.
INTERACTIVE_MARKERS_PUBLIC void setUpdateCb(const UpdateCallback &cb)
Set callback for update messages.
INTERACTIVE_MARKERS_PUBLIC void setStatusCb(const StatusCallback &cb)
Set callback for status updates.
void processUpdate(const UpdateConstPtr &msg)
std::vector< std::string > recv_reset_calls
int main(int argc, char **argv)
void test(std::vector< Msg > messages)
INTERACTIVE_MARKERS_PUBLIC void update()
Update tf info, call callbacks.
std::vector< visualization_msgs::InteractiveMarkerInit > recv_init_msgs
visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr