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 publisher_contexts_.clear();
00124 init_sub_.shutdown();
00125 update_sub_.shutdown();
00126 last_num_publishers_=0;
00127 state_=IDLE;
00128 break;
00129 }
00130 }
00131
00132 void InteractiveMarkerClient::subscribeUpdate()
00133 {
00134 if ( !topic_ns_.empty() )
00135 {
00136 try
00137 {
00138 update_sub_ = nh_.subscribe( topic_ns_+"/update", 100, &InteractiveMarkerClient::processUpdate, this );
00139 DBG_MSG( "Subscribed to update topic: %s", (topic_ns_+"/update").c_str() );
00140 }
00141 catch( ros::Exception& e )
00142 {
00143 callbacks_.statusCb( ERROR, "General", "Error subscribing: " + std::string(e.what()) );
00144 return;
00145 }
00146 }
00147 callbacks_.statusCb( OK, "General", "Waiting for messages.");
00148 }
00149
00150 void InteractiveMarkerClient::subscribeInit()
00151 {
00152 if ( state_ != INIT && !topic_ns_.empty() )
00153 {
00154 try
00155 {
00156 init_sub_ = nh_.subscribe( topic_ns_+"/update_full", 100, &InteractiveMarkerClient::processInit, this );
00157 DBG_MSG( "Subscribed to init topic: %s", (topic_ns_+"/update_full").c_str() );
00158 state_ = INIT;
00159 }
00160 catch( ros::Exception& e )
00161 {
00162 callbacks_.statusCb( ERROR, "General", "Error subscribing: " + std::string(e.what()) );
00163 }
00164 }
00165 }
00166
00167 template<class MsgConstPtrT>
00168 void InteractiveMarkerClient::process( const MsgConstPtrT& msg )
00169 {
00170 callbacks_.statusCb( OK, "General", "Receiving messages.");
00171
00172
00173 if ( msg->server_id.empty() )
00174 {
00175 callbacks_.statusCb( ERROR, "General", "Received message with empty server_id!");
00176 return;
00177 }
00178
00179 M_SingleClient::iterator context_it = publisher_contexts_.find(msg->server_id);
00180
00181
00182
00183
00184 if ( context_it == publisher_contexts_.end() )
00185 {
00186 DBG_MSG( "New publisher detected: %s", msg->server_id.c_str() );
00187
00188 SingleClientPtr pc(new SingleClient( msg->server_id, tf_, target_frame_, callbacks_ ));
00189 context_it = publisher_contexts_.insert( std::make_pair(msg->server_id,pc) ).first;
00190
00191
00192 subscribeInit();
00193 }
00194
00195
00196 context_it->second->process( msg, enable_autocomplete_transparency_ );
00197 }
00198
00199 void InteractiveMarkerClient::processInit( const InitConstPtr& msg )
00200 {
00201 process<InitConstPtr>(msg);
00202 }
00203
00204 void InteractiveMarkerClient::processUpdate( const UpdateConstPtr& msg )
00205 {
00206 process<UpdateConstPtr>(msg);
00207 }
00208
00209 void InteractiveMarkerClient::update()
00210 {
00211 switch ( state_ )
00212 {
00213 case IDLE:
00214 break;
00215
00216 case INIT:
00217 case RUNNING:
00218 {
00219
00220 if ( update_sub_.getNumPublishers() < last_num_publishers_ )
00221 {
00222 callbacks_.statusCb( ERROR, "General", "Server is offline. Resetting." );
00223 shutdown();
00224 subscribeUpdate();
00225 subscribeInit();
00226 return;
00227 }
00228 last_num_publishers_ = update_sub_.getNumPublishers();
00229
00230
00231 bool initialized = true;
00232 M_SingleClient::iterator it;
00233 for ( it = publisher_contexts_.begin(); it!=publisher_contexts_.end(); ++it )
00234 {
00235 it->second->update();
00236 if ( !it->second->isInitialized() )
00237 {
00238 initialized = false;
00239 }
00240 }
00241 if ( state_ == INIT && initialized )
00242 {
00243 init_sub_.shutdown();
00244 state_ = RUNNING;
00245 }
00246 if ( state_ == RUNNING && !initialized )
00247 {
00248 subscribeInit();
00249 }
00250 break;
00251 }
00252 }
00253 }
00254
00255 void InteractiveMarkerClient::statusCb( StatusT status, const std::string& server_id, const std::string& msg )
00256 {
00257 switch ( status )
00258 {
00259 case OK:
00260 DBG_MSG( "%s: %s (Status: OK)", server_id.c_str(), msg.c_str() );
00261 break;
00262 case WARN:
00263 DBG_MSG( "%s: %s (Status: WARNING)", server_id.c_str(), msg.c_str() );
00264 break;
00265 case ERROR:
00266 DBG_MSG( "%s: %s (Status: ERROR)", server_id.c_str(), msg.c_str() );
00267 break;
00268 }
00269
00270 if ( status_cb_ )
00271 {
00272 status_cb_( status, server_id, msg );
00273 }
00274 }
00275
00276 }