interactive_marker_client.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  * 
00029  * Author: David Gossow
00030  */
00031 
00032 #include "interactive_markers/interactive_marker_client.h"
00033 #include "interactive_markers/detail/single_client.h"
00034 
00035 #include <boost/bind.hpp>
00036 #include <boost/make_shared.hpp>
00037 
00038 //#define DBG_MSG( ... ) ROS_DEBUG_NAMED( "interactive_markers", __VA_ARGS__ );
00039 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ );
00040 //#define DBG_MSG( ... ) printf("   "); printf( __VA_ARGS__ ); printf("\n");
00041 
00042 namespace interactive_markers
00043 {
00044 
00045 InteractiveMarkerClient::InteractiveMarkerClient(
00046     tf::Transformer& tf,
00047     const std::string& target_frame,
00048     const std::string &topic_ns )
00049 : state_("InteractiveMarkerClient",IDLE)
00050 , tf_(tf)
00051 , last_num_publishers_(0)
00052 , enable_autocomplete_transparency_(true)
00053 {
00054   target_frame_ = target_frame;
00055   if ( !topic_ns.empty() )
00056   {
00057     subscribe( topic_ns );
00058   }
00059   callbacks_.setStatusCb( boost::bind( &InteractiveMarkerClient::statusCb, this, _1, _2, _3 ) );
00060 }
00061 
00062 InteractiveMarkerClient::~InteractiveMarkerClient()
00063 {
00064   shutdown();
00065 }
00066 
00068 void InteractiveMarkerClient::subscribe( std::string topic_ns )
00069 {
00070   topic_ns_ = topic_ns;
00071   subscribeUpdate();
00072   subscribeInit();
00073 }
00074 
00075 void InteractiveMarkerClient::setInitCb( const InitCallback& cb )
00076 {
00077   callbacks_.setInitCb( cb );
00078 }
00079 
00080 void InteractiveMarkerClient::setUpdateCb( const UpdateCallback& cb )
00081 {
00082   callbacks_.setUpdateCb( cb );
00083 }
00084 
00085 void InteractiveMarkerClient::setResetCb( const ResetCallback& cb )
00086 {
00087   callbacks_.setResetCb( cb );
00088 }
00089 
00090 void InteractiveMarkerClient::setStatusCb( const StatusCallback& cb )
00091 {
00092   status_cb_ = cb;
00093 }
00094 
00095 void InteractiveMarkerClient::setTargetFrame( std::string target_frame )
00096 {
00097   target_frame_ = target_frame;
00098   DBG_MSG("Target frame is now %s", target_frame_.c_str() );
00099 
00100   switch ( state_ )
00101   {
00102   case IDLE:
00103     break;
00104 
00105   case INIT:
00106   case RUNNING:
00107     shutdown();
00108     subscribeUpdate();
00109     subscribeInit();
00110     break;
00111   }
00112 }
00113 
00114 void InteractiveMarkerClient::shutdown()
00115 {
00116   switch ( state_ )
00117   {
00118   case IDLE:
00119     break;
00120 
00121   case INIT:
00122   case RUNNING:
00123     init_sub_.shutdown();
00124     update_sub_.shutdown();
00125     boost::lock_guard<boost::mutex> lock(publisher_contexts_mutex_);
00126     publisher_contexts_.clear();
00127     last_num_publishers_=0;
00128     state_=IDLE;
00129     break;
00130   }
00131 }
00132 
00133 void InteractiveMarkerClient::subscribeUpdate()
00134 {
00135   if ( !topic_ns_.empty() )
00136   {
00137     try
00138     {
00139       update_sub_ = nh_.subscribe( topic_ns_+"/update", 100, &InteractiveMarkerClient::processUpdate, this );
00140       DBG_MSG( "Subscribed to update topic: %s", (topic_ns_+"/update").c_str() );
00141     }
00142     catch( ros::Exception& e )
00143     {
00144       callbacks_.statusCb( ERROR, "General", "Error subscribing: " + std::string(e.what()) );
00145       return;
00146     }
00147   }
00148   callbacks_.statusCb( OK, "General", "Waiting for messages.");
00149 }
00150 
00151 void InteractiveMarkerClient::subscribeInit()
00152 {
00153   if ( state_ != INIT && !topic_ns_.empty() )
00154   {
00155     try
00156     {
00157       init_sub_ = nh_.subscribe( topic_ns_+"/update_full", 100, &InteractiveMarkerClient::processInit, this );
00158       DBG_MSG( "Subscribed to init topic: %s", (topic_ns_+"/update_full").c_str() );
00159       state_ = INIT;
00160     }
00161     catch( ros::Exception& e )
00162     {
00163       callbacks_.statusCb( ERROR, "General", "Error subscribing: " + std::string(e.what()) );
00164     }
00165   }
00166 }
00167 
00168 template<class MsgConstPtrT>
00169 void InteractiveMarkerClient::process( const MsgConstPtrT& msg )
00170 {
00171   callbacks_.statusCb( OK, "General", "Receiving messages.");
00172 
00173   // get caller ID of the sending entity
00174   if ( msg->server_id.empty() )
00175   {
00176     callbacks_.statusCb( ERROR, "General", "Received message with empty server_id!");
00177     return;
00178   }
00179 
00180   SingleClientPtr client;
00181   {
00182     boost::lock_guard<boost::mutex> lock(publisher_contexts_mutex_);
00183 
00184     M_SingleClient::iterator context_it = publisher_contexts_.find(msg->server_id);
00185 
00186     // If we haven't seen this publisher before, we need to reset the
00187     // display and listen to the init topic, plus of course add this
00188     // publisher to our list.
00189     if ( context_it == publisher_contexts_.end() )
00190     {
00191       DBG_MSG( "New publisher detected: %s", msg->server_id.c_str() );
00192 
00193       SingleClientPtr pc(new SingleClient( msg->server_id, tf_, target_frame_, callbacks_ ));
00194       context_it = publisher_contexts_.insert( std::make_pair(msg->server_id,pc) ).first;
00195       client = pc;
00196 
00197       // we need to subscribe to the init topic again
00198       subscribeInit();
00199     }
00200 
00201     client = context_it->second;
00202   }
00203 
00204   // forward init/update to respective context
00205   client->process( msg, enable_autocomplete_transparency_ );
00206 }
00207 
00208 void InteractiveMarkerClient::processInit( const InitConstPtr& msg )
00209 {
00210   process<InitConstPtr>(msg);
00211 }
00212 
00213 void InteractiveMarkerClient::processUpdate( const UpdateConstPtr& msg )
00214 {
00215   process<UpdateConstPtr>(msg);
00216 }
00217 
00218 void InteractiveMarkerClient::update()
00219 {
00220   switch ( state_ )
00221   {
00222   case IDLE:
00223     break;
00224 
00225   case INIT:
00226   case RUNNING:
00227   {
00228     // check if one publisher has gone offline
00229     if ( update_sub_.getNumPublishers() < last_num_publishers_ )
00230     {
00231       callbacks_.statusCb( ERROR, "General", "Server is offline. Resetting." );
00232       shutdown();
00233       subscribeUpdate();
00234       subscribeInit();
00235       return;
00236     }
00237     last_num_publishers_ = update_sub_.getNumPublishers();
00238 
00239     // check if all single clients are finished with the init channels
00240     bool initialized = true;
00241     boost::lock_guard<boost::mutex> lock(publisher_contexts_mutex_);
00242     M_SingleClient::iterator it;
00243     for ( it = publisher_contexts_.begin(); it!=publisher_contexts_.end(); ++it )
00244     {
00245       // Explicitly reference the pointer to the client here, because the client
00246       // might call user code, which might call shutdown(), which will delete
00247       // the publisher_contexts_ map...
00248 
00249       SingleClientPtr single_client = it->second;
00250       single_client->update();
00251       if ( !single_client->isInitialized() )
00252       {
00253         initialized = false;
00254       }
00255 
00256       if ( publisher_contexts_.empty() )
00257         break; // Yep, someone called shutdown()...
00258     }
00259     if ( state_ == INIT && initialized )
00260     {
00261       init_sub_.shutdown();
00262       state_ = RUNNING;
00263     }
00264     if ( state_ == RUNNING && !initialized )
00265     {
00266       subscribeInit();
00267     }
00268     break;
00269   }
00270   }
00271 }
00272 
00273 void InteractiveMarkerClient::statusCb( StatusT status, const std::string& server_id, const std::string& msg )
00274 {
00275   switch ( status )
00276   {
00277   case OK:
00278     DBG_MSG( "%s: %s (Status: OK)", server_id.c_str(), msg.c_str() );
00279     break;
00280   case WARN:
00281     DBG_MSG( "%s: %s (Status: WARNING)", server_id.c_str(), msg.c_str() );
00282     break;
00283   case ERROR:
00284     DBG_MSG( "%s: %s (Status: ERROR)", server_id.c_str(), msg.c_str() );
00285     break;
00286   }
00287 
00288   if ( status_cb_ )
00289   {
00290     status_cb_( status, server_id, msg );
00291   }
00292 }
00293 
00294 }


interactive_markers
Author(s): David Gossow
autogenerated on Sun Feb 3 2019 03:43:04