connection_monitor.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 the Willow Garage 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 
35 
37 #include <ros/ros.h>
38 
39 #include <map>
40 #include <sstream>
41 #include <string>
42 
43 // using namespace actionlib;
44 
45 
46 #define CONNECTION_DEBUG(fmt, ...) \
47  ROS_DEBUG_NAMED("ConnectionMonitor", fmt, ## __VA_ARGS__)
48 
49 #define CONNECTION_WARN(fmt, ...) \
50  ROS_WARN_NAMED("ConnectionMonitor", fmt, ## __VA_ARGS__)
51 
52 
54  ros::Subscriber & result_sub)
55 : feedback_sub_(feedback_sub), result_sub_(result_sub)
56 {
57  status_received_ = false;
58 }
59 
60 // ********* Goal Connections *********
61 
63 {
64  boost::recursive_mutex::scoped_lock lock(data_mutex_);
65 
66  // Check if it's not in the list
67  if (goalSubscribers_.find(pub.getSubscriberName()) == goalSubscribers_.end()) {
68  CONNECTION_DEBUG("goalConnectCallback: Adding [%s] to goalSubscribers",
69  pub.getSubscriberName().c_str());
71  } else {
73  "goalConnectCallback: Trying to add [%s] to goalSubscribers, but it is already in the goalSubscribers list",
74  pub.getSubscriberName().c_str());
76  }
77  CONNECTION_DEBUG("%s", goalSubscribersString().c_str());
78 
79  check_connection_condition_.notify_all();
80 }
81 
83 {
84  boost::recursive_mutex::scoped_lock lock(data_mutex_);
85 
86  std::map<std::string, size_t>::iterator it;
87  it = goalSubscribers_.find(pub.getSubscriberName());
88 
89  if (it == goalSubscribers_.end()) {
91  "goalDisconnectCallback: Trying to remove [%s] to goalSubscribers, but it is not in the goalSubscribers list",
92  pub.getSubscriberName().c_str());
93  } else {
94  CONNECTION_DEBUG("goalDisconnectCallback: Removing [%s] from goalSubscribers",
95  pub.getSubscriberName().c_str());
97  if (goalSubscribers_[pub.getSubscriberName()] == 0) {
98  goalSubscribers_.erase(it);
99  }
100  }
101  CONNECTION_DEBUG("%s", goalSubscribersString().c_str());
102 }
103 
105 {
106  boost::recursive_mutex::scoped_lock lock(data_mutex_);
107  std::ostringstream ss;
108  ss << "Goal Subscribers (" << goalSubscribers_.size() << " total)";
109  for (std::map<std::string, size_t>::iterator it = goalSubscribers_.begin();
110  it != goalSubscribers_.end(); it++)
111  {
112  ss << "\n - " << it->first;
113  }
114  return ss.str();
115 }
116 
117 // ********* Cancel Connections *********
118 
120 {
121  boost::recursive_mutex::scoped_lock lock(data_mutex_);
122 
123  // Check if it's not in the list
124  if (cancelSubscribers_.find(pub.getSubscriberName()) == cancelSubscribers_.end()) {
125  CONNECTION_DEBUG("cancelConnectCallback: Adding [%s] to cancelSubscribers",
126  pub.getSubscriberName().c_str());
128  } else {
130  "cancelConnectCallback: Trying to add [%s] to cancelSubscribers, but it is already in the cancelSubscribers list",
131  pub.getSubscriberName().c_str());
133  }
134  CONNECTION_DEBUG("%s", cancelSubscribersString().c_str());
135 
136  check_connection_condition_.notify_all();
137 }
138 
140  const ros::SingleSubscriberPublisher & pub)
141 {
142  boost::recursive_mutex::scoped_lock lock(data_mutex_);
143 
144  std::map<std::string, size_t>::iterator it;
145  it = cancelSubscribers_.find(pub.getSubscriberName());
146 
147  if (it == cancelSubscribers_.end()) {
149  "cancelDisconnectCallback: Trying to remove [%s] to cancelSubscribers, but it is not in the cancelSubscribers list",
150  pub.getSubscriberName().c_str());
151  } else {
152  CONNECTION_DEBUG("cancelDisconnectCallback: Removing [%s] from cancelSubscribers",
153  pub.getSubscriberName().c_str());
155  if (cancelSubscribers_[pub.getSubscriberName()] == 0) {
156  cancelSubscribers_.erase(it);
157  }
158  }
159  CONNECTION_DEBUG("%s", cancelSubscribersString().c_str());
160 }
161 
163 {
164  boost::recursive_mutex::scoped_lock lock(data_mutex_);
165 
166  std::ostringstream ss;
167  ss << "cancel Subscribers (" << cancelSubscribers_.size() << " total)";
168  for (std::map<std::string, size_t>::iterator it = cancelSubscribers_.begin();
169  it != cancelSubscribers_.end(); it++)
170  {
171  ss << "\n - " << it->first;
172  }
173  return ss.str();
174 }
175 
176 // ********* GoalStatus Connections *********
178  const actionlib_msgs::GoalStatusArrayConstPtr & status, const std::string & cur_status_caller_id)
179 {
180  boost::recursive_mutex::scoped_lock lock(data_mutex_);
181 
182  if (status_received_) {
183  if (status_caller_id_ != cur_status_caller_id) {
185  "processStatus: Previously received status from [%s], but we now received status from [%s]. Did the ActionServer change?",
186  status_caller_id_.c_str(), cur_status_caller_id.c_str());
187  status_caller_id_ = cur_status_caller_id;
188  }
189  latest_status_time_ = status->header.stamp;
190  } else {
192  "processStatus: Just got our first status message from the ActionServer at node [%s]",
193  cur_status_caller_id.c_str());
194  status_received_ = true;
195  status_caller_id_ = cur_status_caller_id;
196  latest_status_time_ = status->header.stamp;
197  }
198 
199  check_connection_condition_.notify_all();
200 }
201 
202 // ********* Connection logic *********
204 {
205  boost::recursive_mutex::scoped_lock lock(data_mutex_);
206 
207  if (!status_received_) {
208  CONNECTION_DEBUG("isServerConnected: Didn't receive status yet, so not connected yet");
209  return false;
210  }
211 
214  "isServerConnected: Server [%s] has not yet subscribed to the goal topic, so not connected yet",
215  status_caller_id_.c_str());
216  CONNECTION_DEBUG("%s", goalSubscribersString().c_str());
217  return false;
218  }
219 
222  "isServerConnected: Server [%s] has not yet subscribed to the cancel topic, so not connected yet",
223  status_caller_id_.c_str());
224  CONNECTION_DEBUG("%s", cancelSubscribersString().c_str());
225  return false;
226  }
227 
228  if (feedback_sub_.getNumPublishers() == 0) {
230  "isServerConnected: Client has not yet connected to feedback topic of server [%s]",
231  status_caller_id_.c_str());
232  return false;
233  }
234 
235  if (result_sub_.getNumPublishers() == 0) {
237  "isServerConnected: Client has not yet connected to result topic of server [%s]",
238  status_caller_id_.c_str());
239  return false;
240  }
241 
242  CONNECTION_DEBUG("isServerConnected: Server [%s] is fully connected", status_caller_id_.c_str());
243  return true;
244 }
245 
247  const ros::NodeHandle & nh)
248 {
249  if (timeout < ros::Duration(0, 0)) {
250  ROS_ERROR_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
251  }
252 
253  ros::Time timeout_time = ros::Time::now() + timeout;
254 
255  boost::recursive_mutex::scoped_lock lock(data_mutex_);
256 
257  if (isServerConnected()) {
258  return true;
259  }
260 
261  // Hardcode how often we check for node.ok()
262  ros::Duration loop_period = ros::Duration().fromSec(.5);
263 
264  while (nh.ok() && !isServerConnected()) {
265  // Determine how long we should wait
266  ros::Duration time_left = timeout_time - ros::Time::now();
267 
268  // Check if we're past the timeout time
269  if (timeout != ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) {
270  break;
271  }
272 
273  // Truncate the time left
274  if (time_left > loop_period || timeout == ros::Duration()) {
275  time_left = loop_period;
276  }
277 
278  check_connection_condition_.timed_wait(lock,
279  boost::posix_time::milliseconds(static_cast<int64_t>(time_left.toSec() * 1000.0f)));
280  }
281 
282  return isServerConnected();
283 }
void goalConnectCallback(const ros::SingleSubscriberPublisher &pub)
void goalDisconnectCallback(const ros::SingleSubscriberPublisher &pub)
#define CONNECTION_DEBUG(fmt,...)
void cancelConnectCallback(const ros::SingleSubscriberPublisher &pub)
std::map< std::string, size_t > goalSubscribers_
Duration & fromSec(double t)
boost::recursive_mutex data_mutex_
void cancelDisconnectCallback(const ros::SingleSubscriberPublisher &pub)
#define CONNECTION_WARN(fmt,...)
std::string getSubscriberName() const
uint32_t getNumPublishers() const
std::map< std::string, size_t > cancelSubscribers_
ConnectionMonitor(ros::Subscriber &feedback_sub, ros::Subscriber &result_sub)
bool waitForActionServerToStart(const ros::Duration &timeout=ros::Duration(0, 0), const ros::NodeHandle &nh=ros::NodeHandle())
void processStatus(const actionlib_msgs::GoalStatusArrayConstPtr &status, const std::string &cur_status_caller_id)
#define ROS_ERROR_NAMED(name,...)
static Time now()
bool ok() const
boost::condition check_connection_condition_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Mon Aug 24 2020 03:40:47