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     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   // get caller ID of the sending entity
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   // If we haven't seen this publisher before, we need to reset the
00182   // display and listen to the init topic, plus of course add this
00183   // publisher to our list.
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     // we need to subscribe to the init topic again
00192     subscribeInit();
00193   }
00194 
00195   // forward init/update to respective context
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     // check if one publisher has gone offline
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     // check if all single clients are finished with the init channels
00231     bool initialized = true;
00232     M_SingleClient::iterator it;
00233     for ( it = publisher_contexts_.begin(); it!=publisher_contexts_.end(); ++it )
00234     {
00235       // Explicitly reference the pointer to the client here, because the client
00236       // might call user code, which might call shutdown(), which will delete
00237       // the publisher_contexts_ map...
00238 
00239       SingleClientPtr single_client = it->second;
00240       single_client->update();
00241       if ( !single_client->isInitialized() )
00242       {
00243         initialized = false;
00244       }
00245 
00246       if ( publisher_contexts_.empty() )
00247         break; // Yep, someone called shutdown()...
00248     }
00249     if ( state_ == INIT && initialized )
00250     {
00251       init_sub_.shutdown();
00252       state_ = RUNNING;
00253     }
00254     if ( state_ == RUNNING && !initialized )
00255     {
00256       subscribeInit();
00257     }
00258     break;
00259   }
00260   }
00261 }
00262 
00263 void InteractiveMarkerClient::statusCb( StatusT status, const std::string& server_id, const std::string& msg )
00264 {
00265   switch ( status )
00266   {
00267   case OK:
00268     DBG_MSG( "%s: %s (Status: OK)", server_id.c_str(), msg.c_str() );
00269     break;
00270   case WARN:
00271     DBG_MSG( "%s: %s (Status: WARNING)", server_id.c_str(), msg.c_str() );
00272     break;
00273   case ERROR:
00274     DBG_MSG( "%s: %s (Status: ERROR)", server_id.c_str(), msg.c_str() );
00275     break;
00276   }
00277 
00278   if ( status_cb_ )
00279   {
00280     status_cb_( status, server_id, msg );
00281   }
00282 }
00283 
00284 }


interactive_markers
Author(s): David Gossow
autogenerated on Sat Aug 27 2016 03:37:45