single_client.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  * Author: David Gossow
30  */
31 
33 
34 #include <boost/bind.hpp>
35 #include <boost/make_shared.hpp>
36 
37 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ );
38 //#define DBG_MSG( ... ) printf(" "); printf( __VA_ARGS__ ); printf("\n");
39 
40 namespace interactive_markers
41 {
42 
44  const std::string& server_id,
46  const std::string& target_frame,
48 )
49 : state_(server_id,INIT)
50 , first_update_seq_num_(-1)
51 , last_update_seq_num_(-1)
52 , tf_(tf)
53 , target_frame_(target_frame)
54 , callbacks_(callbacks)
55 , server_id_(server_id)
56 , warn_keepalive_(false)
57 {
58  callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Waiting for init message." );
59 }
60 
62 {
64 }
65 
66 void SingleClient::process(const visualization_msgs::InteractiveMarkerInit::ConstPtr& msg, bool enable_autocomplete_transparency)
67 {
68  DBG_MSG( "%s: received init #%lu", server_id_.c_str(), msg->seq_num );
69 
70  switch (state_)
71  {
72  case INIT:
73  if ( init_queue_.size() > 5 )
74  {
75  DBG_MSG( "Init queue too large. Erasing init message with id %lu.", init_queue_.begin()->msg->seq_num );
76  init_queue_.pop_back();
77  }
78  init_queue_.push_front( InitMessageContext(tf_, target_frame_, msg, enable_autocomplete_transparency ) );
79  callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Init message received." );
80  break;
81 
82  case RECEIVING:
83  case TF_ERROR:
84  break;
85  }
86 }
87 
88 void SingleClient::process(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& msg, bool enable_autocomplete_transparency)
89 {
90  if ( first_update_seq_num_ == (uint64_t)-1 )
91  {
92  first_update_seq_num_ = msg->seq_num;
93  }
94 
96 
97  if ( msg->type == msg->KEEP_ALIVE )
98  {
99  DBG_MSG( "%s: received keep-alive #%lu", server_id_.c_str(), msg->seq_num );
100  if (last_update_seq_num_ != (uint64_t)-1 && msg->seq_num != last_update_seq_num_ )
101  {
102  std::ostringstream s;
103  s << "Sequence number of update is out of order. Expected: " << last_update_seq_num_ << " Received: " << msg->seq_num;
104  errorReset( s.str() );
105  return;
106  }
107  last_update_seq_num_ = msg->seq_num;
108  return;
109  }
110  else
111  {
112  DBG_MSG( "%s: received update #%lu", server_id_.c_str(), msg->seq_num );
113  if (last_update_seq_num_ != (uint64_t)-1 && msg->seq_num != last_update_seq_num_+1 )
114  {
115  std::ostringstream s;
116  s << "Sequence number of update is out of order. Expected: " << last_update_seq_num_+1 << " Received: " << msg->seq_num;
117  errorReset( s.str() );
118  return;
119  }
120  last_update_seq_num_ = msg->seq_num;
121  }
122 
123  switch (state_)
124  {
125  case INIT:
126  if ( update_queue_.size() > 100 )
127  {
128  DBG_MSG( "Update queue too large. Erasing update message with id %lu.", update_queue_.begin()->msg->seq_num );
129  update_queue_.pop_back();
130  }
131  update_queue_.push_front( UpdateMessageContext(tf_, target_frame_, msg, enable_autocomplete_transparency) );
132  break;
133 
134  case RECEIVING:
135  update_queue_.push_front( UpdateMessageContext(tf_, target_frame_, msg, enable_autocomplete_transparency) );
136  break;
137 
138  case TF_ERROR:
139  break;
140  }
141 }
142 
144 {
145  switch (state_)
146  {
147  case INIT:
151  break;
152 
153  case RECEIVING:
155  pushUpdates();
156  checkKeepAlive();
157  if ( update_queue_.size() > 100 )
158  {
159  errorReset( "Update queue overflow. Resetting connection." );
160  }
161  break;
162 
163  case TF_ERROR:
164  if ( state_.getDuration().toSec() > 1.0 )
165  {
166  callbacks_.statusCb( InteractiveMarkerClient::ERROR, server_id_, "1 second has passed. Re-initializing." );
167  state_ = INIT;
168  }
169  break;
170  }
171 }
172 
174 {
175  double time_since_upd = (ros::Time::now() - last_update_time_).toSec();
176  if ( time_since_upd > 2.0 )
177  {
178  std::ostringstream s;
179  s << "No update received for " << round(time_since_upd) << " seconds.";
181  warn_keepalive_ = true;
182  }
183  else if ( warn_keepalive_ )
184  {
185  warn_keepalive_ = false;
187  }
188 }
189 
191 {
192  // check for all init messages received so far if tf info is ready
193  // and the consecutive update exists.
194  // If so, omit all updates with lower sequence number,
195  // switch to RECEIVING mode and treat the init message like a regular update.
196 
197  if (last_update_seq_num_ == (uint64_t)-1)
198  {
199  callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Initialization: Waiting for first update/keep-alive message." );
200  return;
201  }
202 
203  M_InitMessageContext::iterator init_it;
204  for ( init_it = init_queue_.begin(); init_it!=init_queue_.end(); ++init_it )
205  {
206  uint64_t init_seq_num = init_it->msg->seq_num;
207  bool next_up_exists = init_seq_num >= first_update_seq_num_ && init_seq_num <= last_update_seq_num_;
208 
209  if ( !init_it->isReady() )
210  {
211  callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Initialization: Waiting for tf info." );
212  }
213  else if ( next_up_exists )
214  {
215  DBG_MSG( "Init message with seq_id=%lu is ready & in line with updates. Switching to receive mode.", init_seq_num );
216  while ( !update_queue_.empty() && update_queue_.back().msg->seq_num <= init_seq_num )
217  {
218  DBG_MSG( "Omitting update with seq_id=%lu", update_queue_.back().msg->seq_num );
219  update_queue_.pop_back();
220  }
221 
222  callbacks_.initCb( init_it->msg );
223  callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Receiving updates." );
224 
225  init_queue_.clear();
226  state_ = RECEIVING;
227 
228  pushUpdates();
229  break;
230  }
231  }
232 }
233 
235 {
236  M_InitMessageContext::iterator it;
237  for ( it = init_queue_.begin(); it!=init_queue_.end(); )
238  {
239  try
240  {
241  it->getTfTransforms();
242  }
243  catch ( std::runtime_error& e )
244  {
245  // we want to notify the user, but also keep the init message
246  // in case it is the only one we will receive.
247  std::ostringstream s;
248  s << "Cannot get tf info for init message with sequence number " << it->msg->seq_num << ". Error: " << e.what();
250  }
251  ++it;
252  }
253 }
254 
256 {
257  M_UpdateMessageContext::iterator it;
258  for ( it = update_queue_.begin(); it!=update_queue_.end(); ++it )
259  {
260  try
261  {
262  it->getTfTransforms();
263  }
264  catch ( std::runtime_error& e )
265  {
266  std::ostringstream s;
267  s << "Resetting due to tf error: " << e.what();
268  errorReset( s.str() );
269  return;
270  }
271  catch ( ... )
272  {
273  std::ostringstream s;
274  s << "Resetting due to unknown exception";
275  errorReset( s.str() );
276  }
277  }
278 }
279 
280 void SingleClient::errorReset( std::string error_msg )
281 {
282  // if we get an error here, we re-initialize everything
283  state_ = TF_ERROR;
284  update_queue_.clear();
285  init_queue_.clear();
288  warn_keepalive_ = false;
289 
292 }
293 
295 {
296  if( !update_queue_.empty() && update_queue_.back().isReady() )
297  {
299  }
300  while( !update_queue_.empty() && update_queue_.back().isReady() )
301  {
302  DBG_MSG("Pushing out update #%lu.", update_queue_.back().msg->seq_num );
303  callbacks_.updateCb( update_queue_.back().msg );
304  update_queue_.pop_back();
305  }
306 }
307 
309 {
310  return (state_ != INIT);
311 }
312 
313 }
314 
MessageContext< visualization_msgs::InteractiveMarkerInit > InitMessageContext
MessageContext< visualization_msgs::InteractiveMarkerUpdate > UpdateMessageContext
M_UpdateMessageContext update_queue_
std::string target_frame
Definition: client_test.cpp:74
XmlRpcServer s
M_InitMessageContext init_queue_
void errorReset(std::string error_msg)
void process(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr &msg, bool enable_autocomplete_transparency=true)
const InteractiveMarkerClient::CbCollection & callbacks_
#define DBG_MSG(...)
StateMachine< StateT > state_
static Time now()
SingleClient(const std::string &server_id, tf::Transformer &tf, const std::string &target_frame, const InteractiveMarkerClient::CbCollection &callbacks)
void statusCb(StatusT s, const std::string &id, const std::string &m) const


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