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 #ifndef SWRI_ROSCPP_SERVICE_SERVER_IMPL_H_
00030 #define SWRI_ROSCPP_SERVICE_SERVER_IMPL_H_
00031
00032 #include <swri_roscpp/service_server_statistics.h>
00033
00034 namespace swri
00035 {
00036 class ServiceServer;
00037 class ServiceServerImpl
00038 {
00039 protected:
00040 ros::ServiceServer server_;
00041 std::string unmapped_service_;
00042 std::string mapped_service_;
00043
00044 ServiceServerStatistics total_stats_;
00045 bool instrument_per_client_;
00046 std::map<std::string, ServiceServerStatistics> client_stats_;
00047
00048 bool log_calls_;
00049
00050 void processServing(const std::string caller_name,
00051 bool success,
00052 const ros::WallDuration &runtime)
00053 {
00054 total_stats_.merge(success, runtime);
00055 if (instrument_per_client_) {
00056 client_stats_[caller_name].merge(success, runtime);
00057 }
00058 }
00059
00060 public:
00061 ServiceServerImpl()
00062 :
00063 unmapped_service_("uninitialized"),
00064 mapped_service_("initialized"),
00065 instrument_per_client_(false),
00066 log_calls_(false)
00067 {
00068 }
00069
00070 void resetStatistics()
00071 {
00072 total_stats_.reset();
00073 client_stats_.clear();
00074 }
00075
00076 const std::string& unmappedService() const { return unmapped_service_; }
00077 const std::string& mappedService() const { return mapped_service_; }
00078
00079 const ServiceServerStatistics& totalStats() const { return total_stats_; }
00080
00081 void setInstrumentPerClient(bool enable)
00082 {
00083 instrument_per_client_ = enable;
00084 if (!instrument_per_client_) {
00085 client_stats_.clear();
00086 }
00087 }
00088
00089 bool instrumentPerClient() const { return instrument_per_client_; }
00090
00091 std::vector<std::string> clientNames() const
00092 {
00093 std::vector<std::string> names;
00094 names.reserve(client_stats_.size());
00095
00096 std::map<std::string, ServiceServerStatistics>::const_iterator it;
00097 for (it = client_stats_.begin(); it != client_stats_.end(); it++) {
00098 names.push_back(it->first);
00099 }
00100 return names;
00101 }
00102
00103 ServiceServerStatistics clientStatistics(
00104 const std::string &name) const
00105 {
00106 std::map<std::string, ServiceServerStatistics>::const_iterator it;
00107 it = client_stats_.find(name);
00108 if (it == client_stats_.end()) {
00109 return ServiceServerStatistics();
00110 } else {
00111 return it->second;
00112 }
00113 }
00114
00115 void setLogCalls(bool enable)
00116 {
00117 log_calls_ = enable;
00118 }
00119
00120 bool logCalls() const { return log_calls_; }
00121 };
00122
00123 template<class MReq, class MRes, class T>
00124 class TypedServiceServerImpl : public ServiceServerImpl
00125 {
00126 T *obj_;
00127 bool (T::*callback_plain_)(MReq &, MRes &);
00128 bool (T::*callback_with_event_)(ros::ServiceEvent< MReq, MRes > &);
00129 bool (T::*callback_with_name_)(const std::string &, const MReq &, MRes &);
00130
00131 void initialize(ros::NodeHandle &nh,
00132 const std::string &service)
00133 {
00134 unmapped_service_ = service;
00135 mapped_service_ = nh.resolveName(service);
00136
00137 if (unmapped_service_ == mapped_service_) {
00138 ROS_INFO("Serving to '%s'.", mapped_service_.c_str());
00139 } else {
00140 ROS_INFO("Serving to '%s' at '%s'.",
00141 unmapped_service_.c_str(),
00142 mapped_service_.c_str());
00143 }
00144
00145 server_ = nh.advertiseService(mapped_service_,
00146 &TypedServiceServerImpl::handleService,
00147 this);
00148 }
00149
00150 bool handleService(ros::ServiceEvent<MReq, MRes> &event)
00151 {
00152 if (logCalls()) {
00153 ROS_INFO("Service '%s' called by '%s'",
00154 mapped_service_.c_str(),
00155 event.getCallerName().c_str());
00156 }
00157
00158 bool result;
00159 ros::WallTime start = ros::WallTime::now();
00160
00161 try {
00162 if (callback_plain_) {
00163 result = (obj_->*callback_plain_)(
00164 const_cast<MReq&>(event.getRequest()), event.getResponse());
00165 } else if (callback_with_name_) {
00166 result = (obj_->*callback_with_name_)(
00167 event.getCallerName(), event.getRequest(), event.getResponse());
00168 } else {
00169 result = (obj_->*callback_with_event_)(event);
00170 }
00171 } catch (...) {
00172 result = false;
00173 }
00174 ros::WallTime finish = ros::WallTime::now();
00175
00176 processServing(event.getCallerName(), result, finish-start);
00177 return result;
00178 }
00179
00180 public:
00181 TypedServiceServerImpl()
00182 :
00183 obj_(NULL),
00184 callback_plain_(NULL),
00185 callback_with_event_(NULL),
00186 callback_with_name_(NULL)
00187 {
00188 }
00189
00190 TypedServiceServerImpl(ros::NodeHandle &nh,
00191 const std::string &service,
00192 bool(T::*srv_func)(MReq &, MRes &),
00193 T *obj)
00194 {
00195 obj_ = obj;
00196 callback_plain_ = srv_func;
00197 callback_with_event_ = NULL;
00198 callback_with_name_ = NULL;
00199 initialize(nh, service);
00200 }
00201
00202 TypedServiceServerImpl(ros::NodeHandle &nh,
00203 const std::string &service,
00204 bool(T::*srv_func)(ros::ServiceEvent<MReq, MRes> &),
00205 T *obj)
00206 {
00207 obj_ = obj;
00208 callback_plain_ = NULL;
00209 callback_with_event_ = srv_func;
00210 callback_with_name_ = NULL;
00211 initialize(nh, service);
00212 }
00213
00214 TypedServiceServerImpl(ros::NodeHandle &nh,
00215 const std::string &service,
00216 bool(T::*srv_func)(const std::string &, const MReq &, MRes &),
00217 T *obj)
00218 {
00219 obj_ = obj;
00220 callback_plain_ = NULL;
00221 callback_with_event_ = NULL;
00222 callback_with_name_ = srv_func;
00223 initialize(nh, service);
00224 }
00225 };
00226 }
00227 #endif // SWRI_ROSCPP_SERVICE_SERVER_IMPL_H_
00228