service_action_wrappers.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
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
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
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       //compute the name of the service
00149       std::string client_name = prefix_ + arm_name + suffix_;
00150 
00151       //check if the service is already there
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       //new service; wait for it
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       //insert new service in list
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       //and return it
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       // aleeper: Added this to aid with remapping debugging.
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       //we should probably throw something else here
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     //compute the name of the topic
00311     std::string topic_name = prefix_ + arm_name + suffix_;
00312     
00313     //check if the publisher is already there
00314     map_type::iterator it = publishers_.find(topic_name);
00315     if ( it != publishers_.end() ) return it->second;
00316     
00317     //resolve the name if needed
00318     std::string resolved_topic_name = topic_name;
00319     if (resolve_names_) resolved_topic_name = nh_.resolveName(topic_name);
00320     
00321     //insert new publisher in list
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     //and return it
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       //compute the name of the action
00397       std::string client_name = prefix_ + arm_name + suffix_;
00398 
00399       //check if the action client is already there      
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       //create new action client and insert in map
00410       actionlib::SimpleActionClient<ActionDataType>* new_client = 
00411         new actionlib::SimpleActionClient<ActionDataType>(nh_, action_name, spin_thread_ );
00412 
00413       //wait for the server
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       //insert new client in map
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       //and return it
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       //we should probably throw something else here
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 } //namespace object_manipulator
00460 
00461 #endif


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Mon Oct 6 2014 02:59:50