server_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 
40 
41 #include <chrono>
42 #include <thread>
43 
44 #define DBG_MSG( ... ) printf( __VA_ARGS__ ); printf("\n");
45 #define DBG_MSG_STREAM( ... ) std::cout << __VA_ARGS__ << std::endl;
46 
47 using namespace interactive_markers;
48 
53 
54 
55 std::string reset_server_id;
56 
57 typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr;
58 typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr;
59 
62 
64 {
65  update_calls = 0;
66  init_calls = 0;
67  reset_calls = 0;
68  status_calls = 0;
69  reset_server_id = "";
70  init_msg.reset();
71  update_msg.reset();
72 }
73 
74 void updateCb( const UpdateConstPtr& msg )
75 {
76  DBG_MSG("updateCb called");
77  update_calls++;
78  update_msg = msg;
79 }
80 
81 void initCb( const InitConstPtr& msg )
82 {
83  DBG_MSG("initCb called");
84  init_calls++;
85  init_msg = msg;
86 }
87 
89  const std::string& server_id,
90  const std::string& msg )
91 {
92  DBG_MSG("statusCb called");
93  status_calls++;
94  DBG_MSG_STREAM( (int)status << " " << server_id << ": " << msg );
95 }
96 
97 void resetCb( const std::string& server_id )
98 {
99  DBG_MSG("resetCb( %s ) called", server_id.c_str() );
100  reset_calls++;
101  reset_server_id = server_id;
102 }
103 
104 void waitMsg()
105 {
106  for(int i=0;i<10;i++)
107  {
108  ros::spinOnce();
109  std::this_thread::sleep_for(std::chrono::microseconds(1000));
110  }
111 }
112 
113 TEST(InteractiveMarkerServerAndClient, connect_tf_error)
114 {
116 
117  // create an interactive marker server on the topic namespace simple_marker
118  interactive_markers::InteractiveMarkerServer server("im_server_client_test","test_server",false);
119  visualization_msgs::InteractiveMarker int_marker;
120  int_marker.name = "marker1";
121  int_marker.header.frame_id = "valid_frame";
122 
123  waitMsg();
124 
126 
127  interactive_markers::InteractiveMarkerClient client(tf, "valid_frame", "im_server_client_test");
128  client.setInitCb( &initCb );
129  client.setStatusCb( &statusCb );
130  client.setResetCb( &resetCb );
131  client.setUpdateCb( &updateCb );
132 
133  // Add one marker -> init should get called once
134  DBG_MSG("----------------------------------------");
135 
136  server.insert(int_marker);
137  server.applyChanges();
138  waitMsg();
139  client.update();
140 
141  ASSERT_EQ( 0, update_calls );
142  ASSERT_EQ( 1, init_calls );
143  ASSERT_EQ( 0, reset_calls );
144  ASSERT_TRUE( init_msg );
145  ASSERT_EQ( 1, init_msg->markers.size() );
146  ASSERT_EQ( "marker1", init_msg->markers[0].name );
147  ASSERT_EQ( "valid_frame", init_msg->markers[0].header.frame_id );
148 
149  // Add another marker -> update should get called once
150  DBG_MSG("----------------------------------------");
151 
153 
154  int_marker.name = "marker2";
155 
156  server.insert(int_marker);
157  server.applyChanges();
158  waitMsg();
159  client.update();
160 
161  ASSERT_EQ( 1, update_calls );
162  ASSERT_EQ( 0, init_calls );
163  ASSERT_EQ( 0, reset_calls );
164  ASSERT_TRUE( update_msg );
165  ASSERT_EQ( 1, update_msg->markers.size() );
166  ASSERT_EQ( 0, update_msg->poses.size() );
167  ASSERT_EQ( 0, update_msg->erases.size() );
168  ASSERT_EQ( "marker2", update_msg->markers[0].name );
169 
170  // Make marker tf info invalid -> connection should be reset
171  DBG_MSG("----------------------------------------");
172 
174 
175  int_marker.header.frame_id = "invalid_frame";
176 
177  server.insert(int_marker);
178  server.applyChanges();
179  waitMsg();
180  client.update();
181 
182  ASSERT_EQ( 0, update_calls );
183  ASSERT_EQ( 0, init_calls );
184  ASSERT_EQ( 1, reset_calls );
185  ASSERT_TRUE( reset_server_id.find("/test_server") != std::string::npos );
186 
187  // Make marker tf info valid again -> connection should be successfully initialized again
188  DBG_MSG("----------------------------------------");
189 
190  std::this_thread::sleep_for(std::chrono::microseconds(2000000));
191  waitMsg();
192  client.update();
193 
195 
196  int_marker.header.frame_id = "valid_frame";
197 
198  server.insert(int_marker);
199  server.applyChanges();
200  waitMsg();
201  client.update();
202 
203  ASSERT_EQ( 0, update_calls );
204  ASSERT_EQ( 1, init_calls );
205  ASSERT_EQ( 0, reset_calls );
206 
207  // Erase marker
208  DBG_MSG("----------------------------------------");
209 
211 
212  server.erase("marker1");
213  server.applyChanges();
214  waitMsg();
215  client.update();
216 
217  ASSERT_EQ( 1, update_calls );
218  ASSERT_EQ( 0, init_calls );
219  ASSERT_EQ( 0, reset_calls );
220  ASSERT_TRUE( update_msg );
221  ASSERT_EQ( 0, update_msg->markers.size() );
222  ASSERT_EQ( 0, update_msg->poses.size() );
223  ASSERT_EQ( 1, update_msg->erases.size() );
224  ASSERT_EQ( "marker1", update_msg->erases[0] );
225 
226  // Update pose
227  DBG_MSG("----------------------------------------");
228 
230 
231  server.setPose( "marker2", int_marker.pose, int_marker.header );
232  server.applyChanges();
233  waitMsg();
234  client.update();
235 
236  ASSERT_EQ( 1, update_calls );
237  ASSERT_EQ( 0, init_calls );
238  ASSERT_EQ( 0, reset_calls );
239  ASSERT_TRUE( update_msg );
240  ASSERT_EQ( 0, update_msg->markers.size() );
241  ASSERT_EQ( 1, update_msg->poses.size() );
242  ASSERT_EQ( 0, update_msg->erases.size() );
243  ASSERT_EQ( "marker2", update_msg->poses[0].name );
244 }
245 
246 
247 // Run all the tests that were declared with TEST()
248 int main(int argc, char **argv)
249 {
250  ros::init(argc, argv, "im_server_client_test");
251  testing::InitGoogleTest(&argc, argv);
252  return RUN_ALL_TESTS();
253 }
int status_calls
int main(int argc, char **argv)
UpdateConstPtr update_msg
void resetReceivedMsgs()
void statusCb(InteractiveMarkerClient::StatusT status, const std::string &server_id, const std::string &msg)
int reset_calls
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server
Definition: bursty_tf.cpp:43
INTERACTIVE_MARKERS_PUBLIC void setInitCb(const InitCallback &cb)
Set callback for init messages.
std::string reset_server_id
TEST(InteractiveMarkerServerAndClient, connect_tf_error)
int init_calls
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.
visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr
void waitMsg()
void initCb(const InitConstPtr &msg)
InitConstPtr init_msg
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
INTERACTIVE_MARKERS_PUBLIC void update()
Update tf info, call callbacks.
void resetCb(const std::string &server_id)
int update_calls
INTERACTIVE_MARKERS_PUBLIC bool setPose(const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header=std_msgs::Header())
ROSCPP_DECL void spinOnce()
#define DBG_MSG(...)
INTERACTIVE_MARKERS_PUBLIC bool erase(const std::string &name)
void updateCb(const UpdateConstPtr &msg)
#define DBG_MSG_STREAM(...)
visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr


interactive_markers
Author(s): David Gossow, William Woodall
autogenerated on Mon Feb 28 2022 22:33:22