00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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
00039 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ );
00040
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
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
00187
00188
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
00198 subscribeInit();
00199 }
00200
00201 client = context_it->second;
00202 }
00203
00204
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
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
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
00246
00247
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;
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 }