bond.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Stuart Glaser
31 
32 #include <bondcpp/bond.h>
33 #include <boost/thread/thread_time.hpp>
34 #include <boost/date_time/posix_time/posix_time_types.hpp>
35 
36 #ifdef _WIN32
37 #include <Rpc.h>
38 #else
39 #include <uuid/uuid.h>
40 #endif
41 
42 #include <algorithm>
43 #include <string>
44 #include <vector>
45 
46 namespace bond {
47 
48 static std::string makeUUID()
49 {
50 #ifdef _WIN32
51  UUID uuid;
52  UuidCreate(&uuid);
53  unsigned char *str;
54  UuidToStringA(&uuid, &str);
55  std::string return_string(reinterpret_cast<char *>(str));
56  RpcStringFreeA(&str);
57  return return_string;
58 #else
59  uuid_t uuid;
60  uuid_generate_random(uuid);
61  char uuid_str[40];
62  uuid_unparse(uuid, uuid_str);
63  return std::string(uuid_str);
64 #endif
65 }
66 
67 Bond::Bond(const std::string &topic, const std::string &id,
68  boost::function<void(void)> on_broken,
69  boost::function<void(void)> on_formed)
70  :
71 
72  bondsm_(new BondSM(this)),
73  sm_(*bondsm_),
74  topic_(topic),
75  id_(id),
76  instance_id_(makeUUID()),
77  on_broken_(on_broken),
78  on_formed_(on_formed),
79  sisterDiedFirst_(false),
80  started_(false),
81 
82  connect_timer_(ros::WallDuration(), boost::bind(&Bond::onConnectTimeout, this)),
83  heartbeat_timer_(ros::WallDuration(), boost::bind(&Bond::onHeartbeatTimeout, this)),
84  disconnect_timer_(ros::WallDuration(), boost::bind(&Bond::onDisconnectTimeout, this))
85 {
86  setConnectTimeout(bond::Constants::DEFAULT_CONNECT_TIMEOUT);
87  setDisconnectTimeout(bond::Constants::DEFAULT_DISCONNECT_TIMEOUT);
88  setHeartbeatTimeout(bond::Constants::DEFAULT_HEARTBEAT_TIMEOUT);
89  setHeartbeatPeriod(bond::Constants::DEFAULT_HEARTBEAT_PERIOD);
90 }
91 
93 {
94  if (!started_) {
95  return;
96  }
97 
98  breakBond();
99  if (!waitUntilBroken(ros::Duration(1.0))) {
100  ROS_DEBUG("Bond failed to break on destruction %s (%s)", id_.c_str(), instance_id_.c_str());
101  }
102 
103  // Must destroy the subscription before locking mutex_: shutdown()
104  // will block until the status callback completes, and the status
105  // callback locks mutex_ (in flushPendingCallbacks).
106  sub_.shutdown();
107 
108  // Stops the timers before locking the mutex. Makes sure none of
109  // the callbacks are running when we aquire the mutex.
114 
115  boost::mutex::scoped_lock lock(mutex_);
116  pub_.shutdown();
117 }
118 
119 
120 void Bond::setConnectTimeout(double dur)
121 {
122  if (started_) {
123  ROS_ERROR("Cannot set timeouts after calling start()");
124  return;
125  }
126 
127  connect_timeout_ = dur;
129 }
130 
132 {
133  if (started_) {
134  ROS_ERROR("Cannot set timeouts after calling start()");
135  return;
136  }
137 
138  disconnect_timeout_ = dur;
140 }
141 
143 {
144  if (started_) {
145  ROS_ERROR("Cannot set timeouts after calling start()");
146  return;
147  }
148 
149  heartbeat_timeout_ = dur;
151 }
152 
153 void Bond::setHeartbeatPeriod(double dur)
154 {
155  if (started_) {
156  ROS_ERROR("Cannot set timeouts after calling start()");
157  return;
158  }
159 
160  heartbeat_period_ = dur;
161 }
162 
164 {
165  if (started_) {
166  ROS_ERROR("Cannot set callback queue after calling start()");
167  return;
168  }
169 
170  nh_.setCallbackQueue(queue);
171 }
172 
173 
175 {
176  boost::mutex::scoped_lock lock(mutex_);
178  pub_ = nh_.advertise<bond::Status>(topic_, 5);
179  sub_ = nh_.subscribe<bond::Status>(topic_, 30, boost::bind(&Bond::bondStatusCB, this, _1));
180 
183  started_ = true;
184 }
185 
186 void Bond::setFormedCallback(boost::function<void(void)> on_formed)
187 {
188  boost::mutex::scoped_lock lock(mutex_);
189  on_formed_ = on_formed;
190 }
191 
192 void Bond::setBrokenCallback(boost::function<void(void)> on_broken)
193 {
194  boost::mutex::scoped_lock lock(mutex_);
195  on_broken_ = on_broken;
196 }
197 
199 {
200  return waitUntilFormed(ros::WallDuration(timeout.sec, timeout.nsec));
201 }
203 {
204  boost::mutex::scoped_lock lock(mutex_);
205  ros::SteadyTime deadline(ros::SteadyTime::now() + timeout);
206 
207  while (sm_.getState().getId() == SM::WaitingForSister.getId()) {
208  if (!ros::ok()) {
209  break;
210  }
211 
212  ros::WallDuration wait_time = ros::WallDuration(0.1);
213  if (timeout >= ros::WallDuration(0.0)) {
214  wait_time = std::min(wait_time, deadline - ros::SteadyTime::now());
215  }
216 
217  if (wait_time <= ros::WallDuration(0.0)) {
218  break; // The deadline has expired
219  }
220 
221  condition_.timed_wait(mutex_, boost::posix_time::milliseconds(
222  static_cast<int64_t>(wait_time.toSec() * 1000.0f)));
223  }
224  return sm_.getState().getId() != SM::WaitingForSister.getId();
225 }
226 
228 {
229  return waitUntilBroken(ros::WallDuration(timeout.sec, timeout.nsec));
230 }
232 {
233  boost::mutex::scoped_lock lock(mutex_);
234  ros::SteadyTime deadline(ros::SteadyTime::now() + timeout);
235 
236  while (sm_.getState().getId() != SM::Dead.getId()) {
237  if (!ros::ok()) {
238  break;
239  }
240 
241  ros::WallDuration wait_time = ros::WallDuration(0.1);
242  if (timeout >= ros::WallDuration(0.0)) {
243  wait_time = std::min(wait_time, deadline - ros::SteadyTime::now());
244  }
245 
246  if (wait_time <= ros::WallDuration(0.0)) {
247  break; // The deadline has expired
248  }
249 
250  condition_.timed_wait(mutex_, boost::posix_time::milliseconds(
251  static_cast<int64_t>(wait_time.toSec() * 1000.0f)));
252  }
253  return sm_.getState().getId() == SM::Dead.getId();
254 }
255 
257 {
258  boost::mutex::scoped_lock lock(mutex_);
259  return sm_.getState().getId() == SM::Dead.getId();
260 }
261 
263 {
264  {
265  boost::mutex::scoped_lock lock(mutex_);
266  if (sm_.getState().getId() != SM::Dead.getId()) {
267  sm_.Die();
268  publishStatus(false);
269  }
270  }
272 }
273 
274 
276 {
277  {
278  boost::mutex::scoped_lock lock(mutex_);
280  }
282 }
284 {
285  // Checks that heartbeat timeouts haven't been disabled globally.
286  bool disable_heartbeat_timeout;
287  nh_.param(bond::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, disable_heartbeat_timeout, false);
288  if (disable_heartbeat_timeout) {
289  ROS_WARN("Heartbeat timeout is disabled. Not breaking bond (topic: %s, id: %s)",
290  topic_.c_str(), id_.c_str());
291  return;
292  }
293 
294  {
295  boost::mutex::scoped_lock lock(mutex_);
297  }
299 }
301 {
302  {
303  boost::mutex::scoped_lock lock(mutex_);
305  }
307 }
308 
309 void Bond::bondStatusCB(const bond::Status::ConstPtr &msg)
310 {
311  // Filters out messages from other bonds and messages from ourself
312  if (msg->id == id_ && msg->instance_id != instance_id_) {
313  {
314  boost::mutex::scoped_lock lock(mutex_);
315 
316  if (sister_instance_id_.empty()) {
317  sister_instance_id_ = msg->instance_id;
318  }
319  if (sister_instance_id_ != msg->instance_id) {
320  ROS_ERROR(
321  "More than two locations are trying to use a single bond (topic: %s, id: %s). "
322  "You should only instantiate at most two bond instances for each (topic, id) pair.",
323  topic_.c_str(), id_.c_str());
324  return;
325  }
326 
327  if (msg->active) {
328  sm_.SisterAlive();
329  } else {
330  sm_.SisterDead();
331 
332  // Immediate ack for sister's death notification
333  if (sisterDiedFirst_) {
334  publishStatus(false);
335  }
336  }
337  }
339  }
340 }
341 
343 {
344  boost::mutex::scoped_lock lock(mutex_);
346  sm_.getState().getId() == SM::Alive.getId()) {
347  publishStatus(true);
348  } else if (sm_.getState().getId() == SM::AwaitSisterDeath.getId()) {
349  publishStatus(false);
350  } else {
352  }
353 }
354 
355 void Bond::publishStatus(bool active)
356 {
357  bond::Status::Ptr msg(new bond::Status);
358  msg->header.stamp = ros::Time::now();
359  msg->id = id_;
360  msg->instance_id = instance_id_;
361  msg->active = active;
362  msg->heartbeat_timeout = heartbeat_timeout_;
363  msg->heartbeat_period = heartbeat_period_;
364  pub_.publish(msg);
365 }
366 
367 
369 {
370  std::vector<boost::function<void(void)> > callbacks;
371  {
372  boost::mutex::scoped_lock lock(mutex_);
373  callbacks = pending_callbacks_;
374  pending_callbacks_.clear();
375  }
376 
377  for (size_t i = 0; i < callbacks.size(); ++i) {
378  callbacks[i]();
379  }
380 }
381 
382 } // namespace bond
383 
384 
386 {
388  b->condition_.notify_all();
389  if (b->on_formed_) {
390  b->pending_callbacks_.push_back(b->on_formed_);
391  }
392 }
393 
395 {
396  b->sisterDiedFirst_ = true;
397 }
398 
400 {
401  b->condition_.notify_all();
404  if (b->on_broken_) {
405  b->pending_callbacks_.push_back(b->on_broken_);
406  }
407 }
408 
410 {
412 }
413 
415 {
418  b->publishingTimer_.setPeriod(ros::WallDuration(bond::Constants::DEAD_PUBLISH_PERIOD));
419 }
bond.h
bond::Bond::doPublishing
void doPublishing(const ros::SteadyTimerEvent &e)
Definition: bond.cpp:342
bond::Bond::sm_
BondSMContext sm_
Definition: bond.h:156
BondSMContext::DisconnectTimeout
void DisconnectTimeout()
Definition: BondSM_sm.h:171
BondSMContext::SisterAlive
void SisterAlive()
Definition: BondSM_sm.h:181
ros
BondSMContext::Die
void Die()
Definition: BondSM_sm.h:166
bond::Timeout::setDuration
void setDuration(const ros::Duration &d)
Definition: timeout.cpp:53
bond::Bond::sisterDiedFirst_
bool sisterDiedFirst_
Definition: bond.h:164
bond::Timeout::reset
void reset()
Definition: timeout.cpp:64
ros::SteadyTimer::setPeriod
void setPeriod(const WallDuration &period, bool reset=true)
bond::Bond::setHeartbeatTimeout
void setHeartbeatTimeout(double dur)
Definition: bond.cpp:142
bond::Bond::start
void start()
Starts the bond and connects to the sister process.
Definition: bond.cpp:174
BondSM::Heartbeat
void Heartbeat()
Definition: bond.cpp:409
bond::Bond::pending_callbacks_
std::vector< boost::function< void(void)> > pending_callbacks_
Definition: bond.h:192
bond::Bond::breakBond
void breakBond()
Breaks the bond, notifying the other process.
Definition: bond.cpp:262
BondSMContext::ConnectTimeout
void ConnectTimeout()
Definition: BondSM_sm.h:161
BondSM::SisterDied
void SisterDied()
Definition: bond.cpp:394
DurationBase< Duration >::nsec
int32_t nsec
ros::Subscriber::shutdown
void shutdown()
boost
bond::Bond::setCallbackQueue
void setCallbackQueue(ros::CallbackQueueInterface *queue)
Definition: bond.cpp:163
bond::Bond::publishStatus
void publishStatus(bool active)
Definition: bond.cpp:355
bond::Bond::disconnect_timeout_
double disconnect_timeout_
Definition: bond.h:172
SM::AwaitSisterDeath
static SM_AwaitSisterDeath AwaitSisterDeath
Definition: BondSM_sm.h:54
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::ok
ROSCPP_DECL bool ok()
bond::Bond::on_broken_
boost::function< void(void)> on_broken_
Definition: bond.h:162
bond::Bond::setHeartbeatPeriod
void setHeartbeatPeriod(double dur)
Definition: bond.cpp:153
bond::Bond::connect_timer_
Timeout connect_timer_
Definition: bond.h:175
bond::makeUUID
static std::string makeUUID()
Definition: bond.cpp:48
BondSM::Connected
void Connected()
Definition: bond.cpp:385
bond::Bond::~Bond
~Bond()
Destructs the object, breaking the bond if it is still formed.
Definition: bond.cpp:92
bond::Bond::onHeartbeatTimeout
void onHeartbeatTimeout()
Definition: bond.cpp:283
bond::Bond::heartbeat_timeout_
double heartbeat_timeout_
Definition: bond.h:171
SM::WaitingForSister
static SM_WaitingForSister WaitingForSister
Definition: BondSM_sm.h:52
ros::SteadyTime::now
static SteadyTime now()
bond::Bond::setBrokenCallback
void setBrokenCallback(boost::function< void(void)> on_broken)
Sets the broken callback.
Definition: bond.cpp:192
BondSM::b
bond::Bond * b
Definition: bond.h:210
BondSM
Definition: bond.h:200
ros::SteadyTimer::stop
void stop()
bond::Bond
Forms a bond to monitor another process.
Definition: bond.h:68
bond::Bond::setDisconnectTimeout
void setDisconnectTimeout(double dur)
Definition: bond.cpp:131
ros::Publisher::shutdown
void shutdown()
ROS_DEBUG
#define ROS_DEBUG(...)
bond::Bond::mutex_
boost::mutex mutex_
Definition: bond.h:167
bond::Bond::on_formed_
boost::function< void(void)> on_formed_
Definition: bond.h:163
statemap::State::getId
int getId() const
BondSMContext::HeartbeatTimeout
void HeartbeatTimeout()
Definition: BondSM_sm.h:176
ros::SteadyTime
bond::Bond::pub_
ros::Publisher pub_
Definition: bond.h:180
bond::Bond::sub_
ros::Subscriber sub_
Definition: bond.h:179
ROS_WARN
#define ROS_WARN(...)
ros::SteadyTimerEvent
BondSM::StartDying
void StartDying()
Definition: bond.cpp:414
bond::Bond::bondStatusCB
void bondStatusCB(const bond::Status::ConstPtr &msg)
Definition: bond.cpp:309
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
bond
Definition: bond.h:60
bond::Bond::connect_timeout_
double connect_timeout_
Definition: bond.h:170
bond::Bond::onDisconnectTimeout
void onDisconnectTimeout()
Definition: bond.cpp:300
bond::Bond::flushPendingCallbacks
void flushPendingCallbacks()
Definition: bond.cpp:368
bond::Bond::heartbeat_period_
double heartbeat_period_
Definition: bond.h:173
bond::Bond::isBroken
bool isBroken()
Indicates if the bond is broken.
Definition: bond.cpp:256
ros::NodeHandle::setCallbackQueue
void setCallbackQueue(CallbackQueueInterface *queue)
SM::Dead
static SM_Dead Dead
Definition: BondSM_sm.h:55
bond::Bond::nh_
ros::NodeHandle nh_
Definition: bond.h:154
SM::Alive
static SM_Alive Alive
Definition: BondSM_sm.h:53
bond::Bond::setFormedCallback
void setFormedCallback(boost::function< void(void)> on_formed)
Sets the formed callback.
Definition: bond.cpp:186
bond::Bond::Bond
Bond(const std::string &topic, const std::string &id, boost::function< void(void)> on_broken=boost::function< void(void)>(), boost::function< void(void)> on_formed=boost::function< void(void)>())
Constructs a bond, but does not connect.
Definition: bond.cpp:67
bond::Bond::started_
bool started_
Definition: bond.h:165
ros::NodeHandle::createSteadyTimer
SteadyTimer createSteadyTimer(SteadyTimerOptions &ops) const
bond::Bond::waitUntilFormed
bool waitUntilFormed(ros::Duration timeout=ros::Duration(-1))
Blocks until the bond is formed for at most 'duration'.
Definition: bond.cpp:198
bond::Bond::topic_
std::string topic_
Definition: bond.h:158
ROS_ERROR
#define ROS_ERROR(...)
bond::Bond::id_
std::string id_
Definition: bond.h:159
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
bond::Bond::disconnect_timer_
Timeout disconnect_timer_
Definition: bond.h:177
BondSMContext::SisterDead
void SisterDead()
Definition: BondSM_sm.h:186
bond::Bond::sister_instance_id_
std::string sister_instance_id_
Definition: bond.h:161
DurationBase< WallDuration >::toSec
double toSec() const
BondSM::Death
void Death()
Definition: bond.cpp:399
ros::WallDuration
bond::Bond::waitUntilBroken
bool waitUntilBroken(ros::Duration timeout=ros::Duration(-1))
Blocks until the bond is broken for at most 'duration'.
Definition: bond.cpp:227
bond::Bond::instance_id_
std::string instance_id_
Definition: bond.h:160
BondSMContext::getState
BondSMState & getState() const
Definition: BondSM_sm.h:151
bond::Bond::heartbeat_timer_
Timeout heartbeat_timer_
Definition: bond.h:176
ros::Duration
bond::Bond::condition_
boost::condition condition_
Definition: bond.h:168
bond::Bond::publishingTimer_
ros::SteadyTimer publishingTimer_
Definition: bond.h:181
bond::Bond::setConnectTimeout
void setConnectTimeout(double dur)
Definition: bond.cpp:120
DurationBase< Duration >::sec
int32_t sec
ros::CallbackQueueInterface
bond::Bond::onConnectTimeout
void onConnectTimeout()
Definition: bond.cpp:275
ros::Time::now
static Time now()
bond::Timeout::cancel
void cancel()
Definition: timeout.cpp:71


bondcpp
Author(s): Stuart Glaser
autogenerated on Tue Mar 1 2022 23:53:37