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
00033 #ifndef _SERVICE_ACTION_WRAPPERS_H_
00034 #define _SERVICE_ACTION_WRAPPERS_H_
00035
00036 #include <ros/ros.h>
00037 #include <actionlib/client/simple_action_client.h>
00038
00039 #include <string>
00040 #include <map>
00041
00042 #include "object_manipulator/tools/exceptions.h"
00043
00044 namespace object_manipulator {
00045
00047
00050 template <class ServiceDataType>
00051 class ServiceWrapper
00052 {
00053 private:
00055 bool initialized_;
00057 std::string service_name_;
00059 ros::NodeHandle nh_;
00061 ros::ServiceClient client_;
00063 boost::function<bool()> interrupt_function_;
00064 public:
00065 ServiceWrapper(std::string service_name) : initialized_(false),
00066 service_name_(service_name),
00067 nh_("")
00068 {}
00069
00071 void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}
00072
00074 ros::ServiceClient& client(ros::Duration timeout = ros::Duration(5.0))
00075 {
00076 if (!initialized_)
00077 {
00078 ros::Duration ping_time = ros::Duration(1.0);
00079 if (timeout > ros::Duration(0) && ping_time > timeout) ping_time = timeout;
00080 ros::Time start_time = ros::Time::now();
00081 while (1)
00082 {
00083 if (ros::service::waitForService(service_name_, ping_time)) break;
00084 ROS_INFO_STREAM("Waiting for service " << service_name_);
00085 if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00086 if (!ros::ok()) throw ServiceNotFoundException(service_name_);
00087 ros::Time current_time = ros::Time::now();
00088 if (timeout > ros::Duration(0) && current_time - start_time >= timeout)
00089 throw ServiceNotFoundException(service_name_);
00090 }
00091 client_ = nh_.serviceClient<ServiceDataType>(service_name_);
00092 initialized_ = true;
00093 }
00094 return client_;
00095 }
00096
00097 bool isInitialized() const {return initialized_;}
00098 };
00099
00101
00110 template <class ServiceDataType>
00111 class MultiArmServiceWrapper
00112 {
00113 private:
00115 ros::NodeHandle nh_;
00117 std::string prefix_;
00119 std::string suffix_;
00120
00121 typedef std::map<std::string, ros::ServiceClient> map_type;
00122
00124 map_type clients_;
00126
00127 bool resolve_names_;
00128
00130 boost::function<bool()> interrupt_function_;
00131
00132 public:
00134 MultiArmServiceWrapper(std::string prefix, std::string suffix, bool resolve_names) :
00135 nh_(""), prefix_(prefix), suffix_(suffix), resolve_names_(resolve_names)
00136 {}
00137
00139 void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}
00140
00142
00146 ros::ServiceClient& client(std::string arm_name, ros::Duration timeout = ros::Duration(5.0))
00147 {
00148
00149 std::string client_name = prefix_ + arm_name + suffix_;
00150
00151
00152 map_type::iterator it = clients_.find(client_name);
00153 if ( it != clients_.end() )
00154 {
00155 return it->second;
00156 }
00157
00158 std::string service_name = client_name;
00159 if (resolve_names_) service_name = nh_.resolveName(client_name);
00160
00161
00162 ros::Duration ping_time = ros::Duration(1.0);
00163 if (timeout > ros::Duration(0) && ping_time > timeout) ping_time = timeout;
00164 ros::Time start_time = ros::Time::now();
00165 while(1)
00166 {
00167 if (ros::service::waitForService(service_name, ping_time)) break;
00168 if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00169 if (!ros::ok()) throw ServiceNotFoundException(client_name + " remapped to " + service_name);
00170 ros::Time current_time = ros::Time::now();
00171 if (timeout > ros::Duration(0) && current_time - start_time >= timeout)
00172 throw ServiceNotFoundException(client_name + " remapped to " + service_name);
00173 ROS_INFO_STREAM("Waiting for service " << client_name << " remapped to " << service_name);
00174 }
00175
00176
00177 std::pair<map_type::iterator, bool> new_pair;
00178 new_pair = clients_.insert(std::pair<std::string, ros::ServiceClient>
00179 (client_name, nh_.serviceClient<ServiceDataType>(service_name) ) );
00180
00181
00182 return new_pair.first->second;
00183 }
00184 };
00185
00187 template <class ActionDataType>
00188 class ScopedGoalCancel
00189 {
00190 private:
00191 actionlib::SimpleActionClient<ActionDataType> *client_;
00192 public:
00193 ScopedGoalCancel(actionlib::SimpleActionClient<ActionDataType> *client) : client_(client) {}
00194 void setClient(actionlib::SimpleActionClient<ActionDataType> *client){client_ = client;}
00195 ~ScopedGoalCancel(){if (client_) client_->cancelAllGoals();}
00196 };
00197
00199 template <class ActionDataType>
00200 class ActionWrapper
00201 {
00202 private:
00204 bool initialized_;
00206 std::string action_name_;
00208 std::string remapped_name_;
00210 ros::NodeHandle nh_;
00212 actionlib::SimpleActionClient<ActionDataType> client_;
00214 boost::function<bool()> interrupt_function_;
00215 public:
00216 ActionWrapper(std::string action_name, bool spin_thread) : initialized_(false),
00217 action_name_(action_name),
00218 remapped_name_("*name failed to remap!*"),
00219 nh_(""),
00220 client_(nh_, action_name, spin_thread) {}
00221
00223 void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}
00224
00225 actionlib::SimpleActionClient<ActionDataType>& client(ros::Duration timeout = ros::Duration(5.0))
00226 {
00227 if (!initialized_)
00228 {
00229
00230 remapped_name_ = nh_.resolveName(action_name_, true);
00231 ros::Duration ping_time = ros::Duration(1.0);
00232 if (timeout > ros::Duration(0) && ping_time > timeout) ping_time = timeout;
00233 ros::Time start_time = ros::Time::now();
00234 while (1)
00235 {
00236 if (client_.waitForServer(ping_time)) break;
00237 if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00238 if (!ros::ok()) throw ServiceNotFoundException(action_name_);
00239 ros::Time current_time = ros::Time::now();
00240 if (timeout > ros::Duration(0) && current_time - start_time >= timeout)
00241 throw ServiceNotFoundException(action_name_);
00242 ROS_INFO_STREAM("Waiting for action client: " << action_name_ << " remapped to " << remapped_name_);
00243 }
00244 initialized_ = true;
00245 }
00246 return client_;
00247 }
00248
00249 bool waitForResult(const ros::Duration &timeout=ros::Duration(0,0))
00250 {
00251 ros::Duration ping_time = ros::Duration(5.0);
00252 if (timeout > ros::Duration(0) && ping_time > timeout) ping_time = timeout;
00253 ros::Time start_time = ros::Time::now();
00254 while (1)
00255 {
00256 if (client().waitForResult(ping_time)) return true;
00257 if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00258
00259 if (!ros::ok()) throw ServiceNotFoundException(action_name_);
00260 ros::Time current_time = ros::Time::now();
00261 if (timeout > ros::Duration(0) && current_time - start_time >= timeout) return false;
00262 if (!client().isServerConnected()) return false;
00263 ROS_INFO_STREAM("Waiting for result from action client: " << action_name_ << " remapped to " << remapped_name_);
00264 }
00265 }
00266
00267 bool isInitialized() const {return initialized_;}
00268 };
00269
00270
00272
00275 template <class TopicDataType>
00276 class MultiArmTopicWrapper
00277 {
00278 private:
00280 ros::NodeHandle nh_;
00281
00283 std::string prefix_;
00284
00286 std::string suffix_;
00287
00288 typedef std::map<std::string, ros::Publisher> map_type;
00289
00291 map_type publishers_;
00292
00294
00295 bool resolve_names_;
00296
00297 public:
00299 MultiArmTopicWrapper(std::string prefix, std::string suffix, bool resolve_names) :
00300 nh_(""), prefix_(prefix), suffix_(suffix), resolve_names_(resolve_names)
00301 {}
00302
00304
00308 ros::Publisher& publisher(std::string arm_name, ros::Duration timeout = ros::Duration(5.0))
00309 {
00310
00311 std::string topic_name = prefix_ + arm_name + suffix_;
00312
00313
00314 map_type::iterator it = publishers_.find(topic_name);
00315 if ( it != publishers_.end() ) return it->second;
00316
00317
00318 std::string resolved_topic_name = topic_name;
00319 if (resolve_names_) resolved_topic_name = nh_.resolveName(topic_name);
00320
00321
00322 std::pair<map_type::iterator, bool> new_pair;
00323 new_pair = publishers_.insert(std::pair<std::string, ros::Publisher>
00324 (topic_name, nh_.advertise<TopicDataType>(resolved_topic_name, 100) ) );
00325
00326
00327 return new_pair.first->second;
00328 }
00329 };
00330
00331
00333
00342 template <class ActionDataType>
00343 class MultiArmActionWrapper
00344 {
00345 private:
00347 ros::NodeHandle nh_;
00348
00350 std::string prefix_;
00351
00353 std::string suffix_;
00354
00356 bool spin_thread_;
00357
00359 typedef std::map<std::string, actionlib::SimpleActionClient<ActionDataType>* > map_type;
00360
00362 map_type clients_;
00363
00365
00366 bool resolve_names_;
00367
00369 boost::function<bool()> interrupt_function_;
00370
00371 public:
00373 MultiArmActionWrapper(std::string prefix, std::string suffix, bool spin_thread, bool resolve_names) :
00374 nh_(""), prefix_(prefix), suffix_(suffix), spin_thread_(spin_thread), resolve_names_(resolve_names)
00375 {}
00376
00378 void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}
00379
00380 ~MultiArmActionWrapper()
00381 {
00382 for (typename map_type::iterator it = clients_.begin(); it!=clients_.end(); it++)
00383 {
00384 delete it->second;
00385 }
00386 }
00387
00389
00393 actionlib::SimpleActionClient<ActionDataType>& client(std::string arm_name,
00394 ros::Duration timeout = ros::Duration(5.0))
00395 {
00396
00397 std::string client_name = prefix_ + arm_name + suffix_;
00398
00399
00400 typename map_type::iterator it = clients_.find(client_name);
00401 if ( it != clients_.end() )
00402 {
00403 return *(it->second);
00404 }
00405
00406 std::string action_name = client_name;
00407 if (resolve_names_) action_name = nh_.resolveName(client_name);
00408
00409
00410 actionlib::SimpleActionClient<ActionDataType>* new_client =
00411 new actionlib::SimpleActionClient<ActionDataType>(nh_, action_name, spin_thread_ );
00412
00413
00414 ros::Duration ping_time = ros::Duration(1.0);
00415 if (timeout > ros::Duration(0) && ping_time > timeout) ping_time = timeout;
00416 ros::Time start_time = ros::Time::now();
00417 while (1)
00418 {
00419 if (new_client->waitForServer(ping_time)) break;
00420 if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00421 if (!ros::ok()) throw ServiceNotFoundException(client_name + " remapped to " + action_name);
00422 ros::Time current_time = ros::Time::now();
00423 if (timeout > ros::Duration(0) && current_time - start_time >= timeout)
00424 throw ServiceNotFoundException(client_name + " remapped to " + action_name);
00425 ROS_INFO_STREAM("Waiting for action client " << client_name << ", remapped to " << action_name);
00426 }
00427
00428
00429 std::pair< typename map_type::iterator, bool> new_pair =
00430 clients_.insert( std::pair<std::string,actionlib::SimpleActionClient<ActionDataType>* >
00431 (client_name, new_client ) );
00432
00433
00434 return *(new_pair.first->second);
00435 }
00436
00438 bool waitForResult(std::string arm_name, const ros::Duration &timeout=ros::Duration(0,0))
00439 {
00440 ros::Duration ping_time = ros::Duration(1.0);
00441 if (timeout > ros::Duration(0) && ping_time > timeout) ping_time = timeout;
00442 ros::Time start_time = ros::Time::now();
00443 while (1)
00444 {
00445 if (client(arm_name).waitForResult(ping_time)) return true;
00446 if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00447
00448 if (!ros::ok()) throw ServiceNotFoundException(arm_name);
00449 ros::Time current_time = ros::Time::now();
00450 if (timeout > ros::Duration(0) && current_time - start_time >= timeout) return false;
00451 if (!client(arm_name).isServerConnected()) return false;
00452 ROS_INFO_STREAM("Waiting for result from multi-arm action client on arm " << arm_name);
00453 }
00454 }
00455
00456 };
00457
00458
00459 }
00460
00461 #endif