service_publication.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include "ros/service_publication.h"
00036 #include "ros/service_client_link.h"
00037 #include "ros/connection.h"
00038 #include "ros/callback_queue_interface.h"
00039 
00040 #include <boost/bind.hpp>
00041 
00042 #include <std_msgs/String.h>
00043 
00044 namespace ros
00045 {
00046 
00047 ServicePublication::ServicePublication(const std::string& name, const std::string &md5sum, const std::string& data_type, const std::string& request_data_type,
00048                              const std::string& response_data_type, const ServiceCallbackHelperPtr& helper, CallbackQueueInterface* callback_queue,
00049                              const VoidConstPtr& tracked_object)
00050 : name_(name)
00051 , md5sum_(md5sum)
00052 , data_type_(data_type)
00053 , request_data_type_(request_data_type)
00054 , response_data_type_(response_data_type)
00055 , helper_(helper)
00056 , dropped_(false)
00057 , callback_queue_(callback_queue)
00058 , has_tracked_object_(false)
00059 , tracked_object_(tracked_object)
00060 {
00061   if (tracked_object)
00062   {
00063     has_tracked_object_ = true;
00064   }
00065 }
00066 
00067 ServicePublication::~ServicePublication()
00068 {
00069   drop();
00070 }
00071 
00072 void ServicePublication::drop()
00073 {
00074   // grab a lock here, to ensure that no subscription callback will
00075   // be invoked after we return
00076   {
00077     boost::mutex::scoped_lock lock(client_links_mutex_);
00078     dropped_ = true;
00079   }
00080 
00081   dropAllConnections();
00082 
00083   callback_queue_->removeByID((uint64_t)this);
00084 }
00085 
00086 class ServiceCallback : public CallbackInterface
00087 {
00088 public:
00089   ServiceCallback(const ServiceCallbackHelperPtr& helper, const boost::shared_array<uint8_t>& buf, size_t num_bytes, const ServiceClientLinkPtr& link, bool has_tracked_object, const VoidConstWPtr& tracked_object)
00090   : helper_(helper)
00091   , buffer_(buf)
00092   , num_bytes_(num_bytes)
00093   , link_(link)
00094   , has_tracked_object_(has_tracked_object)
00095   , tracked_object_(tracked_object)
00096   {
00097   }
00098 
00099   virtual CallResult call()
00100   {
00101     if (link_->getConnection()->isDropped())
00102     {
00103       return Invalid;
00104     }
00105 
00106     VoidConstPtr tracker;
00107     if (has_tracked_object_)
00108     {
00109       tracker = tracked_object_.lock();
00110 
00111       if (!tracker)
00112       {
00113         SerializedMessage res = serialization::serializeServiceResponse<uint32_t>(false, 0);
00114         link_->processResponse(false, res);
00115         return Invalid;
00116       }
00117     }
00118 
00119     ServiceCallbackHelperCallParams params;
00120     params.request = SerializedMessage(buffer_, num_bytes_);
00121     params.connection_header = link_->getConnection()->getHeader().getValues();
00122     try
00123     {
00124 
00125       bool ok = helper_->call(params);
00126       if (ok != 0)
00127       {
00128         link_->processResponse(true, params.response);
00129       }
00130       else
00131       {
00132         SerializedMessage res = serialization::serializeServiceResponse<uint32_t>(false, 0);
00133         link_->processResponse(false, res);
00134       }
00135     }
00136     catch (std::exception& e)
00137     {
00138       ROS_ERROR("Exception thrown while processing service call: %s", e.what());
00139       std_msgs::String error_string;
00140       error_string.data = e.what();
00141       SerializedMessage res = serialization::serializeServiceResponse(false, error_string);
00142       link_->processResponse(false, res);
00143       return Invalid;
00144     }
00145 
00146     return Success;
00147   }
00148 
00149 private:
00150   ServiceCallbackHelperPtr helper_;
00151   boost::shared_array<uint8_t> buffer_;
00152   uint32_t num_bytes_;
00153   ServiceClientLinkPtr link_;
00154   bool has_tracked_object_;
00155   VoidConstWPtr tracked_object_;
00156 };
00157 
00158 void ServicePublication::processRequest(boost::shared_array<uint8_t> buf, size_t num_bytes, const ServiceClientLinkPtr& link)
00159 {
00160   CallbackInterfacePtr cb(new ServiceCallback(helper_, buf, num_bytes, link, has_tracked_object_, tracked_object_));
00161   callback_queue_->addCallback(cb, (uint64_t)this);
00162 }
00163 
00164 void ServicePublication::addServiceClientLink(const ServiceClientLinkPtr& link)
00165 {
00166   boost::mutex::scoped_lock lock(client_links_mutex_);
00167 
00168   client_links_.push_back(link);
00169 }
00170 
00171 void ServicePublication::removeServiceClientLink(const ServiceClientLinkPtr& link)
00172 {
00173   boost::mutex::scoped_lock lock(client_links_mutex_);
00174 
00175   V_ServiceClientLink::iterator it = std::find(client_links_.begin(), client_links_.end(), link);
00176   if (it != client_links_.end())
00177   {
00178     client_links_.erase(it);
00179   }
00180 }
00181 
00182 void ServicePublication::dropAllConnections()
00183 {
00184   // Swap our client_links_ list with a local one so we can only lock for a short period of time, because a
00185   // side effect of our calling drop() on connections can be re-locking the client_links_ mutex
00186   V_ServiceClientLink local_links;
00187 
00188   {
00189     boost::mutex::scoped_lock lock(client_links_mutex_);
00190 
00191     local_links.swap(client_links_);
00192   }
00193 
00194   for (V_ServiceClientLink::iterator i = local_links.begin();
00195            i != local_links.end(); ++i)
00196   {
00197     (*i)->getConnection()->drop(Connection::Destructing);
00198   }
00199 }
00200 
00201 } // namespace ros


roscpp
Author(s): Morgan Quigley mquigley@cs.stanford.edu, Josh Faust jfaust@willowgarage.com, Brian Gerkey gerkey@willowgarage.com, Troy Straszheim straszheim@willowgarage.com
autogenerated on Sat Dec 28 2013 17:35:52