interactive_marker_server.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 <visualization_msgs/InteractiveMarkerInit.h>
35 
36 #include <boost/bind.hpp>
37 #include <boost/make_shared.hpp>
38 
39 namespace interactive_markers
40 {
41 
42 InteractiveMarkerServer::InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id, bool spin_thread ) :
43  topic_ns_(topic_ns),
44  seq_num_(0)
45 {
46  if ( spin_thread )
47  {
48  // if we're spinning our own thread, we'll also need our own callback queue
50  }
51 
52  if (!server_id.empty())
53  {
54  server_id_ = ros::this_node::getName() + "/" + server_id;
55  }
56  else
57  {
59  }
60 
61  std::string update_topic = topic_ns + "/update";
62  std::string init_topic = update_topic + "_full";
63  std::string feedback_topic = topic_ns + "/feedback";
64 
65  init_pub_ = node_handle_.advertise<visualization_msgs::InteractiveMarkerInit>( init_topic, 100, true );
66  update_pub_ = node_handle_.advertise<visualization_msgs::InteractiveMarkerUpdate>( update_topic, 100 );
68 
70 
71  if ( spin_thread )
72  {
73  need_to_terminate_ = false;
74  spin_thread_.reset( new boost::thread(boost::bind(&InteractiveMarkerServer::spinThread, this)) );
75  }
76 
77  publishInit();
78 }
79 
80 
82 {
83  if (spin_thread_.get())
84  {
85  need_to_terminate_ = true;
86  spin_thread_->join();
87  }
88 
89  if ( node_handle_.ok() )
90  {
91  clear();
92  applyChanges();
93  }
94 }
95 
96 
98 {
99  while (node_handle_.ok())
100  {
101  if (need_to_terminate_)
102  {
103  break;
104  }
106  }
107 }
108 
109 
111 {
112  boost::recursive_mutex::scoped_lock lock( mutex_ );
113 
114  if ( pending_updates_.empty() )
115  {
116  return;
117  }
118 
119  M_UpdateContext::iterator update_it;
120 
121  visualization_msgs::InteractiveMarkerUpdate update;
122  update.type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
123 
124  update.markers.reserve( marker_contexts_.size() );
125  update.poses.reserve( marker_contexts_.size() );
126  update.erases.reserve( marker_contexts_.size() );
127 
128  for ( update_it = pending_updates_.begin(); update_it != pending_updates_.end(); update_it++ )
129  {
130  M_MarkerContext::iterator marker_context_it = marker_contexts_.find( update_it->first );
131 
132  switch ( update_it->second.update_type )
133  {
135  {
136  if ( marker_context_it == marker_contexts_.end() )
137  {
138  ROS_DEBUG("Creating new context for %s", update_it->first.c_str());
139  // create a new int_marker context
140  marker_context_it = marker_contexts_.insert( std::make_pair( update_it->first, MarkerContext() ) ).first;
141  // copy feedback cbs, in case they have been set before the marker context was created
142  marker_context_it->second.default_feedback_cb = update_it->second.default_feedback_cb;
143  marker_context_it->second.feedback_cbs = update_it->second.feedback_cbs;
144  }
145 
146  marker_context_it->second.int_marker = update_it->second.int_marker;
147 
148  update.markers.push_back( marker_context_it->second.int_marker );
149  break;
150  }
151 
153  {
154  if ( marker_context_it == marker_contexts_.end() )
155  {
156  ROS_ERROR( "Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface." );
157  }
158  else
159  {
160  marker_context_it->second.int_marker.pose = update_it->second.int_marker.pose;
161  marker_context_it->second.int_marker.header = update_it->second.int_marker.header;
162 
163  visualization_msgs::InteractiveMarkerPose pose_update;
164  pose_update.header = marker_context_it->second.int_marker.header;
165  pose_update.pose = marker_context_it->second.int_marker.pose;
166  pose_update.name = marker_context_it->second.int_marker.name;
167  update.poses.push_back( pose_update );
168  }
169  break;
170  }
171 
173  {
174  if ( marker_context_it != marker_contexts_.end() )
175  {
176  marker_contexts_.erase( update_it->first );
177  update.erases.push_back( update_it->first );
178  }
179  break;
180  }
181  }
182  }
183 
184  seq_num_++;
185 
186  publish( update );
187  publishInit();
188  pending_updates_.clear();
189 }
190 
191 
192 bool InteractiveMarkerServer::erase( const std::string &name )
193 {
194  boost::recursive_mutex::scoped_lock lock( mutex_ );
195 
196  if (marker_contexts_.end() == marker_contexts_.find(name) &&
197  pending_updates_.end() == pending_updates_.find(name))
198  {
199  return false;
200  }
201  pending_updates_[name].update_type = UpdateContext::ERASE;
202  return true;
203 }
204 
206 {
207  boost::recursive_mutex::scoped_lock lock( mutex_ );
208 
209  // erase all markers
210  pending_updates_.clear();
211  M_MarkerContext::iterator it;
212  for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ )
213  {
214  pending_updates_[it->first].update_type = UpdateContext::ERASE;
215  }
216 }
217 
218 
220 {
221  return marker_contexts_.empty();
222 }
223 
224 
225 std::size_t InteractiveMarkerServer::size() const
226 {
227  return marker_contexts_.size();
228 }
229 
230 
231 bool InteractiveMarkerServer::setPose( const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header )
232 {
233  boost::recursive_mutex::scoped_lock lock( mutex_ );
234 
235  M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name );
236  M_UpdateContext::iterator update_it = pending_updates_.find( name );
237 
238  // if there's no marker and no pending addition for it, we can't update the pose
239  if ( marker_context_it == marker_contexts_.end() &&
240  ( update_it == pending_updates_.end() || update_it->second.update_type != UpdateContext::FULL_UPDATE ) )
241  {
242  return false;
243  }
244 
245  // keep the old header
246  if ( header.frame_id.empty() )
247  {
248  if ( marker_context_it != marker_contexts_.end() )
249  {
250  doSetPose( update_it, name, pose, marker_context_it->second.int_marker.header );
251  }
252  else if ( update_it != pending_updates_.end() )
253  {
254  doSetPose( update_it, name, pose, update_it->second.int_marker.header );
255  }
256  else
257  {
258  BOOST_ASSERT_MSG(false, "Marker does not exist and there is no pending creation.");
259  return false;
260  }
261  }
262  else
263  {
264  doSetPose( update_it, name, pose, header );
265  }
266  return true;
267 }
268 
269 bool InteractiveMarkerServer::setCallback( const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type )
270 {
271  boost::recursive_mutex::scoped_lock lock( mutex_ );
272 
273  M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name );
274  M_UpdateContext::iterator update_it = pending_updates_.find( name );
275 
276  if ( marker_context_it == marker_contexts_.end() && update_it == pending_updates_.end() )
277  {
278  return false;
279  }
280 
281  // we need to overwrite both the callbacks for the actual marker
282  // and the update, if there's any
283 
284  if ( marker_context_it != marker_contexts_.end() )
285  {
286  // the marker exists, so we can just overwrite the existing callbacks
287  if ( feedback_type == DEFAULT_FEEDBACK_CB )
288  {
289  marker_context_it->second.default_feedback_cb = feedback_cb;
290  }
291  else
292  {
293  if ( feedback_cb )
294  {
295  marker_context_it->second.feedback_cbs[feedback_type] = feedback_cb;
296  }
297  else
298  {
299  marker_context_it->second.feedback_cbs.erase( feedback_type );
300  }
301  }
302  }
303 
304  if ( update_it != pending_updates_.end() )
305  {
306  if ( feedback_type == DEFAULT_FEEDBACK_CB )
307  {
308  update_it->second.default_feedback_cb = feedback_cb;
309  }
310  else
311  {
312  if ( feedback_cb )
313  {
314  update_it->second.feedback_cbs[feedback_type] = feedback_cb;
315  }
316  else
317  {
318  update_it->second.feedback_cbs.erase( feedback_type );
319  }
320  }
321  }
322  return true;
323 }
324 
325 void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker )
326 {
327  boost::recursive_mutex::scoped_lock lock( mutex_ );
328 
329  M_UpdateContext::iterator update_it = pending_updates_.find( int_marker.name );
330  if ( update_it == pending_updates_.end() )
331  {
332  update_it = pending_updates_.insert( std::make_pair( int_marker.name, UpdateContext() ) ).first;
333  }
334 
335  update_it->second.update_type = UpdateContext::FULL_UPDATE;
336  update_it->second.int_marker = int_marker;
337 }
338 
339 void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker,
340  FeedbackCallback feedback_cb, uint8_t feedback_type)
341 {
342  insert( int_marker );
343 
344  setCallback( int_marker.name, feedback_cb, feedback_type );
345 }
346 
347 bool InteractiveMarkerServer::get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const
348 {
349  boost::recursive_mutex::scoped_lock lock( mutex_ );
350 
351  M_UpdateContext::const_iterator update_it = pending_updates_.find( name );
352 
353  if ( update_it == pending_updates_.end() )
354  {
355  M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name );
356  if ( marker_context_it == marker_contexts_.end() )
357  {
358  return false;
359  }
360 
361  int_marker = marker_context_it->second.int_marker;
362  return true;
363  }
364 
365  // if there's an update pending, we'll have to account for that
366  switch ( update_it->second.update_type )
367  {
369  return false;
370 
372  {
373  M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name );
374  if ( marker_context_it == marker_contexts_.end() )
375  {
376  return false;
377  }
378  int_marker = marker_context_it->second.int_marker;
379  int_marker.pose = update_it->second.int_marker.pose;
380  return true;
381  }
382 
384  int_marker = update_it->second.int_marker;
385  return true;
386  }
387 
388  return false;
389 }
390 
392 {
393  boost::recursive_mutex::scoped_lock lock( mutex_ );
394 
395  visualization_msgs::InteractiveMarkerInit init;
396  init.server_id = server_id_;
397  init.seq_num = seq_num_;
398  init.markers.reserve( marker_contexts_.size() );
399 
400  M_MarkerContext::iterator it;
401  for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ )
402  {
403  ROS_DEBUG( "Publishing %s", it->second.int_marker.name.c_str() );
404  init.markers.push_back( it->second.int_marker );
405  }
406 
407  init_pub_.publish( init );
408 }
409 
411 {
412  boost::recursive_mutex::scoped_lock lock( mutex_ );
413 
414  M_MarkerContext::iterator marker_context_it = marker_contexts_.find( feedback->marker_name );
415 
416  // ignore feedback for non-existing markers
417  if ( marker_context_it == marker_contexts_.end() )
418  {
419  return;
420  }
421 
422  MarkerContext &marker_context = marker_context_it->second;
423 
424  // if two callers try to modify the same marker, reject (timeout= 1 sec)
425  if ( marker_context.last_client_id != feedback->client_id &&
426  (ros::Time::now() - marker_context.last_feedback).toSec() < 1.0 )
427  {
428  ROS_DEBUG( "Rejecting feedback for %s: conflicting feedback from separate clients.", feedback->marker_name.c_str() );
429  return;
430  }
431 
432  marker_context.last_feedback = ros::Time::now();
433  marker_context.last_client_id = feedback->client_id;
434 
435  if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE )
436  {
437  if ( marker_context.int_marker.header.stamp == ros::Time(0) )
438  {
439  // keep the old header
440  doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, marker_context.int_marker.header );
441  }
442  else
443  {
444  doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, feedback->header );
445  }
446  }
447 
448  // call feedback handler
449  boost::unordered_map<uint8_t,FeedbackCallback>::iterator feedback_cb_it = marker_context.feedback_cbs.find( feedback->event_type );
450  if ( feedback_cb_it != marker_context.feedback_cbs.end() && feedback_cb_it->second )
451  {
452  // call type-specific callback
453  feedback_cb_it->second( feedback );
454  }
455  else if ( marker_context.default_feedback_cb )
456  {
457  // call default callback
458  marker_context.default_feedback_cb( feedback );
459  }
460 }
461 
462 
464 {
465  visualization_msgs::InteractiveMarkerUpdate empty_update;
466  empty_update.type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE;
467  publish( empty_update );
468 }
469 
470 
471 void InteractiveMarkerServer::publish( visualization_msgs::InteractiveMarkerUpdate &update )
472 {
473  update.server_id = server_id_;
474  update.seq_num = seq_num_;
475  update_pub_.publish( update );
476 }
477 
478 
479 void InteractiveMarkerServer::doSetPose( M_UpdateContext::iterator update_it, const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header )
480 {
481  if ( update_it == pending_updates_.end() )
482  {
483  update_it = pending_updates_.insert( std::make_pair( name, UpdateContext() ) ).first;
484  update_it->second.update_type = UpdateContext::POSE_UPDATE;
485  }
486  else if ( update_it->second.update_type != UpdateContext::FULL_UPDATE )
487  {
488  update_it->second.update_type = UpdateContext::POSE_UPDATE;
489  }
490 
491  update_it->second.int_marker.pose = pose;
492  update_it->second.int_marker.header = header;
493  ROS_DEBUG( "Marker '%s' is now at %f, %f, %f", update_it->first.c_str(), pose.position.x, pose.position.y, pose.position.z );
494 }
495 
496 
497 }
void publish(const boost::shared_ptr< M > &message) const
f
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
visualization_msgs::InteractiveMarkerFeedbackConstPtr FeedbackConstPtr
boost::unordered_map< uint8_t, FeedbackCallback > feedback_cbs
void processFeedback(const FeedbackConstPtr &feedback)
ROSCPP_DECL const std::string & getName()
boost::function< void(const FeedbackConstPtr &) > FeedbackCallback
INTERACTIVE_MARKERS_PUBLIC ~InteractiveMarkerServer()
Destruction of the interface will lead to all managed markers being cleared.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void setCallbackQueue(CallbackQueueInterface *queue)
INTERACTIVE_MARKERS_PUBLIC InteractiveMarkerServer(const std::string &topic_ns, const std::string &server_id="", bool spin_thread=false)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INTERACTIVE_MARKERS_PUBLIC bool get(std::string name, visualization_msgs::InteractiveMarker &int_marker) const
void doSetPose(M_UpdateContext::iterator update_it, const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header)
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
void publish(visualization_msgs::InteractiveMarkerUpdate &update)
INTERACTIVE_MARKERS_PUBLIC bool setCallback(const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type=DEFAULT_FEEDBACK_CB)
static Time now()
bool ok() const
INTERACTIVE_MARKERS_PUBLIC bool setPose(const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header=std_msgs::Header())
#define ROS_ERROR(...)
INTERACTIVE_MARKERS_PUBLIC bool erase(const std::string &name)
INTERACTIVE_MARKERS_PUBLIC bool empty() const
INTERACTIVE_MARKERS_PUBLIC std::size_t size() const
#define ROS_DEBUG(...)


interactive_markers
Author(s): David Gossow
autogenerated on Wed Apr 8 2020 04:01:49