service_server.h
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2015, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
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 Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from 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 SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 #ifndef SWRI_ROSCPP_SERVICE_SERVER_H_
00030 #define SWRI_ROSCPP_SERVICE_SERVER_H_
00031 
00032 #include <ros/node_handle.h>
00033 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00034 #include <swri_roscpp/service_server_impl.h>
00035 #include <swri_roscpp/service_server_statistics.h>
00036 
00037 namespace swri
00038 {
00039 class ServiceServer
00040 {
00041  private:
00042   boost::shared_ptr<ServiceServerImpl> impl_;
00043 
00044  public:
00045   ServiceServer();
00046 
00047   template<class MReq, class MRes, class T>
00048   ServiceServer(ros::NodeHandle &nh,
00049                 const std::string &service,
00050                 bool(T::*srv_func)(MReq &, MRes &),
00051                 T *obj);
00052 
00053   template<class MReq, class MRes, class T>
00054   ServiceServer(ros::NodeHandle &nh,
00055                 const std::string &service,
00056                 bool(T::*srv_func)(ros::ServiceEvent< MReq, MRes > &),                
00057                 T *obj);
00058 
00059   template<class MReq, class MRes, class T>
00060   ServiceServer(ros::NodeHandle &nh,
00061                 const std::string &service,
00062                 bool(T::*srv_func)(const std::string &, const MReq &, MRes &),
00063                 T *obj);
00064   
00065   ServiceServer& operator=(const ServiceServer &other);
00066 
00067   // Reset all statistics, including message and timeout counts.
00068   void resetStatistics();
00069   
00070   // Returns the unmapped service name that was provided when the
00071   // service was created.
00072   const std::string& unmappedService() const;
00073   // Returns the fully mapped service name that the server is
00074   // listening on.
00075   const std::string& mappedService() const;
00076 
00077   // Retrieve the statitics for all service calls handled by the
00078   // server.
00079   const ServiceServerStatistics& statistics() const;  
00080 
00081   // The service statistics can be broken down to individual clients
00082   // if desired.
00083   void setInstrumentPerClient(bool enable);
00084   bool instrumentPerClient() const;
00085   std::vector<std::string> clientNames() const;
00086   ServiceServerStatistics clientStatistics(const std::string &name) const;
00087 
00088   // The service server can output a console log message when the
00089   // service is called if desired.
00090   void setLogCalls(bool enable);
00091   bool logCalls() const;
00092   
00093   // These flags determine which values are added to a diagnostic
00094   // status by the appendDiagnostics method.
00095   enum DIAGNOSTIC_FLAGS {
00096     DIAG_CONNECTION = 1 << 0,  // Include service names
00097     DIAG_SERVINGS   = 1 << 1,   // Servings counts/success rates
00098     DIAG_TIMING     = 1 << 2,  // Include service time
00099     DIAG_CLIENTS    = 1 << 3,  // Include client list, if applicable.
00100     
00101     DIAG_ALL        = ~0,       // Abbreviation to include all information
00102     DIAG_MOST       = DIAG_ALL ^ DIAG_CONNECTION
00103     // Abbreviation to include everything except connection info.
00104   };
00105   
00106   // This is a convenience method to add information about this
00107   // server to a diagnostic status in a standard way.
00108   //
00109   // The status is merged with a warning if service calls have failed.
00110   // The status is merged with an error if the most recent service
00111   // call failed.
00112   //
00113   // The flags parameter determines which values are added to the
00114   // status' key/value pairs.  This should be a bitwise combination of
00115   // the values defined in DIAGNOSTIC_FLAGS.
00116   void appendDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &status,
00117                          const std::string &name,
00118                          const int flags);
00119 };  // class ServiceServer
00120 
00121 inline
00122 ServiceServer::ServiceServer()
00123 {
00124   // Setup an empty implementation so that we can assume impl_ is
00125   // non-null and avoid a lot of unnecessary NULL checks.
00126   impl_ = boost::make_shared<ServiceServerImpl>();
00127 }
00128 
00129 template<class MReq, class MRes, class T>
00130 inline
00131 ServiceServer::ServiceServer(ros::NodeHandle &nh,
00132                              const std::string &service,
00133                              bool(T::*srv_func)(MReq &, MRes &),
00134                              T *obj)
00135 {
00136   impl_ = boost::shared_ptr<ServiceServerImpl>(
00137     new TypedServiceServerImpl<MReq, MRes, T>(
00138       nh, service, srv_func, obj));
00139 }
00140 
00141 template<class MReq, class MRes, class T>
00142 inline
00143 ServiceServer::ServiceServer(ros::NodeHandle &nh,
00144                              const std::string &service,
00145                              bool(T::*srv_func)(ros::ServiceEvent< MReq, MRes > &),                
00146                              T *obj)
00147 {
00148   impl_ = boost::shared_ptr<ServiceServerImpl>(
00149     new TypedServiceServerImpl<MReq, MRes, T>(
00150       nh, service, srv_func, obj));
00151 }
00152 
00153 template<class MReq, class MRes, class T>
00154 inline
00155 ServiceServer::ServiceServer(ros::NodeHandle &nh,
00156                              const std::string &service,
00157                              bool(T::*srv_func)(const std::string &, const MReq &, MRes &),
00158                              T *obj)
00159 {
00160   impl_ = boost::shared_ptr<ServiceServerImpl>(
00161     new TypedServiceServerImpl<MReq, MRes, T>(
00162       nh, service, srv_func, obj));
00163 }
00164 
00165 inline
00166 ServiceServer& ServiceServer::operator=(const ServiceServer &other)
00167 {
00168   bool instrument_per_client = impl_->instrumentPerClient();
00169   bool log_calls = impl_->logCalls();
00170   impl_ = other.impl_;
00171   impl_->setInstrumentPerClient(instrument_per_client);
00172   impl_->setLogCalls(log_calls);
00173   return *this;
00174 }
00175 
00176 inline
00177 void ServiceServer::resetStatistics()
00178 {
00179   impl_->resetStatistics();
00180 }
00181 
00182 inline
00183 const std::string& ServiceServer::unmappedService() const
00184 {
00185   return impl_->unmappedService();
00186 }
00187 
00188 inline
00189 const std::string& ServiceServer::mappedService() const
00190 {
00191   return impl_->mappedService();
00192 }
00193 
00194 inline
00195 const ServiceServerStatistics& ServiceServer::statistics() const
00196 {
00197   return impl_->totalStats();
00198 }
00199    
00200 inline
00201 void ServiceServer::setInstrumentPerClient(bool enable)
00202 {
00203   impl_->setInstrumentPerClient(enable);
00204 }
00205 
00206 inline
00207 bool ServiceServer::instrumentPerClient() const
00208 {
00209   return impl_->instrumentPerClient();
00210 }
00211 
00212 inline
00213 std::vector<std::string> ServiceServer::clientNames() const
00214 {
00215   return impl_->clientNames();
00216 }
00217 
00218 inline
00219 ServiceServerStatistics ServiceServer::clientStatistics(
00220   const std::string &name) const
00221 {
00222   return impl_->clientStatistics(name);
00223 }
00224 
00225 inline
00226 void ServiceServer::setLogCalls(bool enable)
00227 {
00228   impl_->setLogCalls(enable);
00229 }
00230 
00231 inline
00232 bool ServiceServer::logCalls() const
00233 {
00234   return impl_->logCalls();
00235 }
00236 
00237 inline
00238 void ServiceServer::appendDiagnostics(
00239   diagnostic_updater::DiagnosticStatusWrapper &status,
00240   const std::string &name,
00241   const int flags)
00242 {
00243   const ServiceServerStatistics& stats = statistics();
00244 
00245   // Alias a type for easier access to DiagnosticStatus enumerations.
00246   typedef diagnostic_msgs::DiagnosticStatus DS;
00247 
00248   if (stats.lastFailed()) {
00249     status.mergeSummaryf(DS::ERROR, "Last %s service called failed", name.c_str());
00250   } else if (stats.failed()) {
00251     status.mergeSummaryf(DS::ERROR, "%s service calls have failed.", name.c_str());
00252   }
00253 
00254   if (flags & DIAG_CONNECTION) {
00255     if (mappedService() == unmappedService()) {
00256       status.addf(name + "service name", "%s", mappedService().c_str());
00257     } else {
00258       status.addf(name + "service name", "%s -> %s",
00259                   unmappedService().c_str(),
00260                   mappedService().c_str());
00261     }
00262   }
00263 
00264   if (flags & DIAG_SERVINGS) {
00265     status.addf(name + "service calls", "%d failed / %d calls",
00266                 stats.failed(), stats.servings());                
00267   }
00268 
00269   if (flags & DIAG_TIMING) {
00270     status.addf(name + "service call time", "%.2f [s] (%.2f [s] - %.2f [s])",
00271                 stats.meanTime().toSec(),
00272                 stats.minTime().toSec(),
00273                 stats.maxTime().toSec());
00274   }
00275 
00276   if (flags & DIAG_CLIENTS) {
00277     std::vector<std::string> names = clientNames();
00278 
00279     for (size_t i = 0; i < names.size(); i++) {
00280       ServiceServerStatistics client_stats = clientStatistics(names[i]);
00281       
00282       if (flags & DIAG_SERVINGS) {
00283         status.addf(name + " calls from " + names[i], "%d failed / %d calls",
00284                     client_stats.failed(),
00285                     client_stats.servings());
00286       }
00287 
00288       if (flags & DIAG_TIMING) {
00289         status.addf(name + " calls from " + names[i] + " time", "%.2f [s] (%.2f [s] - %.2f [s])",
00290                 client_stats.meanTime().toSec(),
00291                 client_stats.minTime().toSec(),
00292                 client_stats.maxTime().toSec());
00293       }
00294     }
00295   }
00296 }
00297 }  // namespace swri
00298 #endif  // SWRI_ROSCPP_SERVICE_SERVER_H_


swri_roscpp
Author(s):
autogenerated on Thu Jun 6 2019 20:34:47