Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "bondcpp/timeout.h"
00031
00032 #include <algorithm>
00033
00034 namespace bond {
00035
00036 Timeout::Timeout(const ros::Duration &d,
00037 boost::function<void(void)> on_timeout)
00038 : duration_(d.sec, d.nsec), on_timeout_(on_timeout)
00039 {
00040 }
00041
00042 Timeout::Timeout(const ros::WallDuration &d,
00043 boost::function<void(void)> on_timeout)
00044 : duration_(d), on_timeout_(on_timeout)
00045 {
00046 }
00047
00048 Timeout::~Timeout()
00049 {
00050 timer_.stop();
00051 }
00052
00053 void Timeout::setDuration(const ros::Duration &d)
00054 {
00055 duration_ = ros::WallDuration(d.sec, d.nsec);
00056 }
00057
00058 void Timeout::setDuration(const ros::WallDuration &d)
00059 {
00060 duration_ = d;
00061 }
00062
00063
00064 void Timeout::reset()
00065 {
00066 timer_.stop();
00067 timer_ = nh_.createWallTimer(duration_, &Timeout::timerCallback, this, true);
00068 deadline_ = ros::WallTime::now() + duration_;
00069 }
00070
00071 void Timeout::cancel()
00072 {
00073 timer_.stop();
00074 }
00075
00076 ros::WallDuration Timeout::left()
00077 {
00078 return std::max(ros::WallDuration(0.0), deadline_ - ros::WallTime::now());
00079 }
00080
00081 void Timeout::timerCallback(const ros::WallTimerEvent &)
00082 {
00083 if (on_timeout_) {
00084 on_timeout_();
00085 }
00086 }
00087
00088 }