topic_service_client.h
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2018, 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_TOPIC_SERVICE_CLIENT_H_
00030 #define SWRI_ROSCPP_TOPIC_SERVICE_CLIENT_H_
00031 
00032 #include <ros/node_handle.h>
00033 
00034 #include <boost/thread/mutex.hpp>
00035 
00036 #include <map>
00037 
00038 namespace swri
00039 {
00040 
00041 template<class MReq, class MRes>
00042 class TopicServiceClientRaw
00043 {
00044 private:
00045   typedef
00046   boost::mutex request_lock_;
00047   ros::Subscriber response_sub_;
00048   ros::Publisher request_pub_;
00049   boost::shared_ptr<MRes> response_;
00050 
00051   ros::Duration timeout_;
00052   std::string name_;
00053   std::string service_name_;
00054 
00055   int sequence_;
00056 
00057 public:
00058   TopicServiceClientRaw() : sequence_(0), timeout_(ros::Duration(4.0))
00059   {
00060 
00061   }
00062 
00063   void initialize(ros::NodeHandle &nh,
00064                 const std::string &service,
00065                 const std::string &client_name = "")
00066   {
00067     name_ = client_name.length() ? client_name : ros::this_node::getName();
00068     std::string rservice = nh.resolveName(service);
00069     service_name_ = rservice;
00070 
00071     request_pub_ = nh.advertise<MReq>(rservice + "/request", 10);
00072     response_sub_ = nh.subscribe(rservice + "/response", 10, &TopicServiceClientRaw<MReq, MRes>::response_callback, this);
00073   }
00074 
00075   bool call(MReq& request, MRes& response)
00076   {
00077     boost::mutex::scoped_lock scoped_lock(request_lock_);
00078 
00079     // block for response
00080     request.srv_header.stamp = ros::Time::now();
00081     request.srv_header.sequence = sequence_;
00082     request.srv_header.sender = name_;
00083 
00084     // Wait until we get a subscriber and publisher
00085     while (request_pub_.getNumSubscribers() == 0 || response_sub_.getNumPublishers() == 0)
00086     {
00087       ros::Duration(0.002).sleep();
00088       ros::spinOnce();
00089 
00090       if (ros::Time::now() - request.srv_header.stamp > timeout_)
00091       {
00092         return false;
00093       }
00094     }
00095     request_pub_.publish(request);
00096 
00097     // Wait until we get a response
00098     while (!response_ && ros::Time::now() - request.srv_header.stamp < timeout_)
00099     {
00100       ros::Duration(0.002).sleep();
00101       ros::spinOnce();
00102     }
00103 
00104     sequence_++;
00105     if (response_)
00106     {
00107       response = *response_;
00108       response_.reset();
00109       return response.srv_header.result;
00110     }
00111     return false;
00112   }
00113 
00114   std::string getService()
00115   {
00116     return service_name_;
00117   }
00118 
00119   bool exists()
00120   {
00121     return request_pub_.getNumSubscribers() > 0 && response_sub_.getNumPublishers() > 0;
00122   }
00123 
00124   // The service server can output a console log message when the
00125   // service is called if desired.
00126   void setLogCalls(bool enable);
00127   bool logCalls() const;
00128 private:
00129 
00130   void response_callback(const boost::shared_ptr<MRes>& message)
00131   {
00132     ROS_DEBUG("Got response for %s with sequence %i",
00133              message->srv_header.sender.c_str(), message->srv_header.sequence);
00134 
00135     if (message->srv_header.sender != name_)
00136     {
00137       ROS_DEBUG("Got response from another client, ignoring..");
00138       return;
00139     }
00140 
00141     if (message->srv_header.sequence != sequence_)
00142     {
00143       ROS_WARN("Got wrong sequence number, ignoring..");
00144       return;
00145     }
00146 
00147     response_ = message;
00148   }
00149 };  // class TopicServiceClientRaw
00150 
00151 template<class MReq>
00152 class TopicServiceClient: public TopicServiceClientRaw<typename MReq:: Request, typename MReq:: Response>
00153 {
00154 public:
00155   bool call(MReq& req)
00156   {
00157     return TopicServiceClientRaw<typename MReq:: Request, typename MReq:: Response>::call(req.request, req.response);
00158   }
00159 
00160 };  // class TopicServiceClient
00161 
00162 
00163 }  // namespace swri
00164 #endif  // SWRI_ROSCPP_TOPIC_SERVICE_SERVER_H_


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