interactive_marker_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 
34 
35 #include <boost/bind.hpp>
36 #include <boost/make_shared.hpp>
37 
38 //#define DBG_MSG( ... ) ROS_DEBUG_NAMED( "interactive_markers", __VA_ARGS__ );
39 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ );
40 //#define DBG_MSG( ... ) printf(" "); printf( __VA_ARGS__ ); printf("\n");
41 
42 namespace interactive_markers
43 {
44 
46  tf2_ros::Buffer& tf,
47  const std::string& target_frame,
48  const std::string &topic_ns )
49 : state_("InteractiveMarkerClient",IDLE)
50 , tf_(tf)
51 , last_num_publishers_(0)
52 , enable_autocomplete_transparency_(true)
53 {
55  if ( !topic_ns.empty() )
56  {
57  subscribe( topic_ns );
58  }
59  callbacks_.setStatusCb( boost::bind( &InteractiveMarkerClient::statusCb, this, _1, _2, _3 ) );
60 }
61 
63 {
64  shutdown();
65 }
66 
68 void InteractiveMarkerClient::subscribe( std::string topic_ns )
69 {
70  topic_ns_ = topic_ns;
72  subscribeInit();
73 }
74 
76 {
77  callbacks_.setInitCb( cb );
78 }
79 
81 {
82  callbacks_.setUpdateCb( cb );
83 }
84 
86 {
87  callbacks_.setResetCb( cb );
88 }
89 
91 {
92  status_cb_ = cb;
93 }
94 
96 {
98  DBG_MSG("Target frame is now %s", target_frame_.c_str() );
99 
100  switch ( state_ )
101  {
102  case IDLE:
103  break;
104 
105  case INIT:
106  case RUNNING:
107  shutdown();
108  subscribeUpdate();
109  subscribeInit();
110  break;
111  }
112 }
113 
115 {
116  switch ( state_ )
117  {
118  case IDLE:
119  break;
120 
121  case INIT:
122  case RUNNING:
125  boost::lock_guard<boost::recursive_mutex> lock(publisher_contexts_mutex_);
126  publisher_contexts_.clear();
128  state_=IDLE;
129  break;
130  }
131 }
132 
134 {
135  if ( !topic_ns_.empty() )
136  {
137  try
138  {
140  DBG_MSG( "Subscribed to update topic: %s", (topic_ns_+"/update").c_str() );
141  }
142  catch( ros::Exception& e )
143  {
144  callbacks_.statusCb( ERROR, "General", "Error subscribing: " + std::string(e.what()) );
145  return;
146  }
147  }
148  callbacks_.statusCb( OK, "General", "Waiting for messages.");
149 }
150 
152 {
153  if ( state_ != INIT && !topic_ns_.empty() )
154  {
155  try
156  {
157  init_sub_ = nh_.subscribe( topic_ns_+"/update_full", 100, &InteractiveMarkerClient::processInit, this );
158  DBG_MSG( "Subscribed to init topic: %s", (topic_ns_+"/update_full").c_str() );
159  state_ = INIT;
160  }
161  catch( ros::Exception& e )
162  {
163  callbacks_.statusCb( ERROR, "General", "Error subscribing: " + std::string(e.what()) );
164  }
165  }
166 }
167 
168 template<class MsgConstPtrT>
169 void InteractiveMarkerClient::process( const MsgConstPtrT& msg )
170 {
171  callbacks_.statusCb( OK, "General", "Receiving messages.");
172 
173  // get caller ID of the sending entity
174  if ( msg->server_id.empty() )
175  {
176  callbacks_.statusCb( ERROR, "General", "Received message with empty server_id!");
177  return;
178  }
179 
180  SingleClientPtr client;
181  {
182  boost::lock_guard<boost::recursive_mutex> lock(publisher_contexts_mutex_);
183 
184  M_SingleClient::iterator context_it = publisher_contexts_.find(msg->server_id);
185 
186  // If we haven't seen this publisher before, we need to reset the
187  // display and listen to the init topic, plus of course add this
188  // publisher to our list.
189  if ( context_it == publisher_contexts_.end() )
190  {
191  DBG_MSG( "New publisher detected: %s", msg->server_id.c_str() );
192 
193  SingleClientPtr pc(new SingleClient( msg->server_id, tf_, target_frame_, callbacks_ ));
194  context_it = publisher_contexts_.insert( std::make_pair(msg->server_id,pc) ).first;
195  client = pc;
196 
197  // we need to subscribe to the init topic again
198  subscribeInit();
199  }
200 
201  client = context_it->second;
202  }
203 
204  // forward init/update to respective context
205  client->process( msg, enable_autocomplete_transparency_ );
206 }
207 
209 {
210  process<InitConstPtr>(msg);
211 }
212 
214 {
215  process<UpdateConstPtr>(msg);
216 }
217 
219 {
220  switch ( state_ )
221  {
222  case IDLE:
223  break;
224 
225  case INIT:
226  case RUNNING:
227  {
228  // check if one publisher has gone offline
230  {
231  callbacks_.statusCb( ERROR, "General", "Server is offline. Resetting." );
232  shutdown();
233  subscribeUpdate();
234  subscribeInit();
235  return;
236  }
238 
239  // check if all single clients are finished with the init channels
240  bool initialized = true;
241  boost::lock_guard<boost::recursive_mutex> lock(publisher_contexts_mutex_);
242  M_SingleClient::iterator it;
243  for ( it = publisher_contexts_.begin(); it!=publisher_contexts_.end(); ++it )
244  {
245  // Explicitly reference the pointer to the client here, because the client
246  // might call user code, which might call shutdown(), which will delete
247  // the publisher_contexts_ map...
248 
249  SingleClientPtr single_client = it->second;
250  single_client->update();
251  if ( !single_client->isInitialized() )
252  {
253  initialized = false;
254  }
255 
256  if ( publisher_contexts_.empty() )
257  break; // Yep, someone called shutdown()...
258  }
259  if ( state_ == INIT && initialized )
260  {
262  state_ = RUNNING;
263  }
264  if ( state_ == RUNNING && !initialized )
265  {
266  subscribeInit();
267  }
268  break;
269  }
270  }
271 }
272 
273 void InteractiveMarkerClient::statusCb( StatusT status, const std::string& server_id, const std::string& msg )
274 {
275  switch ( status )
276  {
277  case OK:
278  DBG_MSG( "%s: %s (Status: OK)", server_id.c_str(), msg.c_str() );
279  break;
280  case WARN:
281  DBG_MSG( "%s: %s (Status: WARNING)", server_id.c_str(), msg.c_str() );
282  break;
283  case ERROR:
284  DBG_MSG( "%s: %s (Status: ERROR)", server_id.c_str(), msg.c_str() );
285  break;
286  }
287 
288  if ( status_cb_ )
289  {
290  status_cb_( status, server_id, msg );
291  }
292 }
293 
294 }
interactive_marker_client.h
interactive_markers::InteractiveMarkerClient::tf_
tf2_ros::Buffer & tf_
Definition: interactive_marker_client.h:169
interactive_markers::InteractiveMarkerClient::StatusT
StatusT
Definition: interactive_marker_client.h:73
interactive_markers::InteractiveMarkerClient::subscribeUpdate
void subscribeUpdate()
Definition: interactive_marker_client.cpp:133
interactive_markers::InteractiveMarkerClient::ERROR
@ ERROR
Definition: interactive_marker_client.h:76
boost::shared_ptr
interactive_markers::InteractiveMarkerClient::WARN
@ WARN
Definition: interactive_marker_client.h:75
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
interactive_markers::InteractiveMarkerClient::StatusCallback
boost::function< void(StatusT, const std::string &, const std::string &) > StatusCallback
Definition: interactive_marker_client.h:85
interactive_markers::InteractiveMarkerClient::status_cb_
StatusCallback status_cb_
Definition: interactive_marker_client.h:215
interactive_markers::SingleClient
Definition: single_client.h:62
interactive_markers::InteractiveMarkerClient::CbCollection::statusCb
void statusCb(StatusT s, const std::string &id, const std::string &m) const
Definition: interactive_marker_client.h:182
interactive_markers::InteractiveMarkerClient::InitConstPtr
visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr
Definition: interactive_marker_client.h:80
interactive_markers::InteractiveMarkerClient::enable_autocomplete_transparency_
bool enable_autocomplete_transparency_
Definition: interactive_marker_client.h:221
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
interactive_markers::InteractiveMarkerClient::INIT
@ INIT
Definition: interactive_marker_client.h:145
interactive_markers::InteractiveMarkerClient::UpdateCallback
boost::function< void(const UpdateConstPtr &) > UpdateCallback
Definition: interactive_marker_client.h:82
ros::Subscriber::shutdown
void shutdown()
interactive_markers::InteractiveMarkerClient::CbCollection::setInitCb
void setInitCb(InitCallback init_cb)
Definition: interactive_marker_client.h:185
ros::Exception
DBG_MSG
#define DBG_MSG(...)
Definition: interactive_marker_client.cpp:39
interactive_markers::InteractiveMarkerClient::~InteractiveMarkerClient
INTERACTIVE_MARKERS_PUBLIC ~InteractiveMarkerClient()
Will cause a 'reset' call for all server ids.
Definition: interactive_marker_client.cpp:62
interactive_markers::InteractiveMarkerClient::setTargetFrame
INTERACTIVE_MARKERS_PUBLIC void setTargetFrame(std::string target_frame)
Change the target frame and reset the connection.
Definition: interactive_marker_client.cpp:95
interactive_markers::InteractiveMarkerClient::CbCollection::setStatusCb
void setStatusCb(StatusCallback status_cb)
Definition: interactive_marker_client.h:194
ros::Subscriber::getNumPublishers
uint32_t getNumPublishers() const
interactive_markers::InteractiveMarkerClient::OK
@ OK
Definition: interactive_marker_client.h:74
interactive_markers::InteractiveMarkerClient::subscribe
INTERACTIVE_MARKERS_PUBLIC void subscribe(std::string topic_ns)
Subscribe to the topics topic_ns/update and topic_ns/init.
Definition: interactive_marker_client.cpp:68
interactive_markers::InteractiveMarkerClient::subscribeInit
void subscribeInit()
Definition: interactive_marker_client.cpp:151
interactive_markers::InteractiveMarkerClient::update
INTERACTIVE_MARKERS_PUBLIC void update()
Update tf info, call callbacks.
Definition: interactive_marker_client.cpp:218
interactive_markers::InteractiveMarkerClient::RUNNING
@ RUNNING
Definition: interactive_marker_client.h:146
single_client.h
interactive_markers::InteractiveMarkerClient::publisher_contexts_mutex_
boost::recursive_mutex publisher_contexts_mutex_
Definition: interactive_marker_client.h:167
interactive_markers::InteractiveMarkerClient::ResetCallback
boost::function< void(const std::string &) > ResetCallback
Definition: interactive_marker_client.h:84
interactive_markers::InteractiveMarkerClient::CbCollection::setResetCb
void setResetCb(ResetCallback reset_cb)
Definition: interactive_marker_client.h:191
tf2_ros::Buffer
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
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::statusCb
void statusCb(StatusT status, const std::string &server_id, const std::string &msg)
Definition: interactive_marker_client.cpp:273
interactive_markers::InteractiveMarkerClient::shutdown
INTERACTIVE_MARKERS_PUBLIC void shutdown()
Unsubscribe, clear queues & call reset callbacks.
Definition: interactive_marker_client.cpp:114
interactive_markers::InteractiveMarkerClient::callbacks_
CbCollection callbacks_
Definition: interactive_marker_client.h:212
interactive_markers::InteractiveMarkerClient::topic_ns_
std::string topic_ns_
Definition: interactive_marker_client.h:151
interactive_markers::InteractiveMarkerClient::CbCollection::setUpdateCb
void setUpdateCb(UpdateCallback update_cb)
Definition: interactive_marker_client.h:188
interactive_markers::InteractiveMarkerClient::IDLE
@ IDLE
Definition: interactive_marker_client.h:144
interactive_markers::InteractiveMarkerClient::process
void process(const MsgConstPtrT &msg)
Definition: interactive_marker_client.cpp:169
interactive_markers::InteractiveMarkerClient::InitCallback
boost::function< void(const InitConstPtr &) > InitCallback
Definition: interactive_marker_client.h:83
interactive_markers::InteractiveMarkerClient::InteractiveMarkerClient
INTERACTIVE_MARKERS_PUBLIC InteractiveMarkerClient(tf2_ros::Buffer &tf, const std::string &target_frame="", const std::string &topic_ns="")
Definition: interactive_marker_client.cpp:45
interactive_markers::InteractiveMarkerClient::UpdateConstPtr
visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr
Definition: interactive_marker_client.h:79
interactive_markers::InteractiveMarkerClient::init_sub_
ros::Subscriber init_sub_
Definition: interactive_marker_client.h:154
interactive_markers::InteractiveMarkerClient::update_sub_
ros::Subscriber update_sub_
Definition: interactive_marker_client.h:153
interactive_markers::InteractiveMarkerClient::last_num_publishers_
uint32_t last_num_publishers_
Definition: interactive_marker_client.h:218
target_frame
std::string target_frame
Definition: client_test.cpp:72
interactive_markers
Definition: message_context.h:45
interactive_markers::InteractiveMarkerClient::publisher_contexts_
M_SingleClient publisher_contexts_
Definition: interactive_marker_client.h:166
interactive_markers::InteractiveMarkerClient::target_frame_
std::string target_frame_
Definition: interactive_marker_client.h:170
interactive_markers::InteractiveMarkerClient::state_
StateMachine< StateT > state_
Definition: interactive_marker_client.h:149
interactive_markers::InteractiveMarkerClient::nh_
ros::NodeHandle nh_
Definition: interactive_marker_client.h:140
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