service_publication.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #include "ros/connection.h"
39 
40 #include <boost/bind.hpp>
41 
42 #include <std_msgs/String.h>
43 
44 namespace ros
45 {
46 
47 ServicePublication::ServicePublication(const std::string& name, const std::string &md5sum, const std::string& data_type, const std::string& request_data_type,
48  const std::string& response_data_type, const ServiceCallbackHelperPtr& helper, CallbackQueueInterface* callback_queue,
49  const VoidConstPtr& tracked_object)
50 : name_(name)
51 , md5sum_(md5sum)
52 , data_type_(data_type)
53 , request_data_type_(request_data_type)
54 , response_data_type_(response_data_type)
55 , helper_(helper)
56 , dropped_(false)
57 , callback_queue_(callback_queue)
58 , has_tracked_object_(false)
59 , tracked_object_(tracked_object)
60 {
61  if (tracked_object)
62  {
63  has_tracked_object_ = true;
64  }
65 }
66 
68 {
69  drop();
70 }
71 
73 {
74  // grab a lock here, to ensure that no subscription callback will
75  // be invoked after we return
76  {
77  boost::mutex::scoped_lock lock(client_links_mutex_);
78  dropped_ = true;
79  }
80 
82 
83  callback_queue_->removeByID((uint64_t)this);
84 }
85 
87 {
88 public:
89  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)
90  : helper_(helper)
91  , buffer_(buf)
92  , num_bytes_(num_bytes)
93  , link_(link)
94  , has_tracked_object_(has_tracked_object)
95  , tracked_object_(tracked_object)
96  {
97  }
98 
99  virtual CallResult call()
100  {
101  if (link_->getConnection()->isDropped())
102  {
103  return Invalid;
104  }
105 
106  VoidConstPtr tracker;
108  {
109  tracker = tracked_object_.lock();
110 
111  if (!tracker)
112  {
113  SerializedMessage res = serialization::serializeServiceResponse<uint32_t>(false, 0);
114  link_->processResponse(false, res);
115  return Invalid;
116  }
117  }
118 
120  params.request = SerializedMessage(buffer_, num_bytes_);
121  params.connection_header = link_->getConnection()->getHeader().getValues();
122  try
123  {
124 
125  bool ok = helper_->call(params);
126  if (ok)
127  {
128  link_->processResponse(true, params.response);
129  }
130  else
131  {
132  SerializedMessage res = serialization::serializeServiceResponse<uint32_t>(false, 0);
133  link_->processResponse(false, res);
134  }
135  }
136  catch (std::exception& e)
137  {
138  ROS_ERROR("Exception thrown while processing service call: %s", e.what());
139  std_msgs::String error_string;
140  error_string.data = e.what();
142  link_->processResponse(false, res);
143  return Invalid;
144  }
145 
146  return Success;
147  }
148 
149 private:
152  uint32_t num_bytes_;
156 };
157 
159 {
160  CallbackInterfacePtr cb(boost::make_shared<ServiceCallback>(helper_, buf, num_bytes, link, has_tracked_object_, tracked_object_));
161  callback_queue_->addCallback(cb, (uint64_t)this);
162 }
163 
165 {
166  boost::mutex::scoped_lock lock(client_links_mutex_);
167 
168  client_links_.push_back(link);
169 }
170 
172 {
173  boost::mutex::scoped_lock lock(client_links_mutex_);
174 
175  V_ServiceClientLink::iterator it = std::find(client_links_.begin(), client_links_.end(), link);
176  if (it != client_links_.end())
177  {
178  client_links_.erase(it);
179  }
180 }
181 
183 {
184  // Swap our client_links_ list with a local one so we can only lock for a short period of time, because a
185  // side effect of our calling drop() on connections can be re-locking the client_links_ mutex
186  V_ServiceClientLink local_links;
187 
188  {
189  boost::mutex::scoped_lock lock(client_links_mutex_);
190 
191  local_links.swap(client_links_);
192  }
193 
194  for (V_ServiceClientLink::iterator i = local_links.begin();
195  i != local_links.end(); ++i)
196  {
197  (*i)->getConnection()->drop(Connection::Destructing);
198  }
199 }
200 
201 } // namespace ros
virtual CallResult call()
Call this callback.
ServicePublication(const std::string &name, const std::string &md5sum, const std::string &data_type, const std::string &request_data_type, const std::string &response_data_type, const ServiceCallbackHelperPtr &helper, CallbackQueueInterface *queue, const VoidConstPtr &tracked_object)
CallResult
Possible results for the call() method.
SerializedMessage serializeServiceResponse(bool ok, const M &message)
boost::shared_ptr< M_string > connection_header
void drop()
Terminate this service server.
Abstract interface for items which can be added to a CallbackQueueInterface.
ServiceCallbackHelperPtr helper_
Abstract interface for a queue used to handle all callbacks within roscpp.
virtual void removeByID(uint64_t owner_id)=0
Remove all callbacks associated with an owner id.
std::vector< ServiceClientLinkPtr > V_ServiceClientLink
ServiceClientLinkPtr link_
CallbackQueueInterface * callback_queue_
void addServiceClientLink(const ServiceClientLinkPtr &link)
Adds a service link for us to manage.
boost::weak_ptr< void const > VoidConstWPtr
Definition: forwards.h:53
ServiceCallbackHelperPtr helper_
ROSCPP_DECL bool ok()
Check whether it&#39;s time to exit.
Definition: init.cpp:589
boost::shared_array< uint8_t > buffer_
virtual void addCallback(const CallbackInterfacePtr &callback, uint64_t owner_id=0)=0
Add a callback, with an optional owner id. The owner id can be used to remove a set of callbacks from...
V_ServiceClientLink client_links_
void removeServiceClientLink(const ServiceClientLinkPtr &link)
Removes a service link from our list.
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)
void processRequest(boost::shared_array< uint8_t > buf, size_t num_bytes, const ServiceClientLinkPtr &link)
Adds a request to the queue if our thread pool size is not 0, otherwise immediately calls the callbac...
#define ROS_ERROR(...)


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim, Dirk Thomas
autogenerated on Mon Feb 28 2022 23:33:27