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
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef REALTIME_PUBLISHER_H
00039 #define REALTIME_PUBLISHER_H
00040
00041 #include <string>
00042 #include <ros/node_handle.h>
00043 #include <boost/utility.hpp>
00044 #include <boost/thread/mutex.hpp>
00045 #include <boost/thread/thread.hpp>
00046 #include <boost/thread/condition.hpp>
00047
00048 namespace realtime_tools {
00049
00050 template <class Msg>
00051 class RealtimePublisher : boost::noncopyable
00052 {
00053
00054 public:
00056 Msg msg_;
00057
00065 RealtimePublisher(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
00066 : topic_(topic), node_(node), is_running_(false), keep_running_(false), turn_(REALTIME)
00067 {
00068 construct(queue_size, latched);
00069 }
00070
00071 RealtimePublisher()
00072 : is_running_(false), keep_running_(false), turn_(REALTIME)
00073 {
00074 }
00075
00077 ~RealtimePublisher()
00078 {
00079 stop();
00080 while (is_running())
00081 usleep(100);
00082
00083 publisher_.shutdown();
00084 }
00085
00086 void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
00087 {
00088 topic_ = topic;
00089 node_ = node;
00090 construct(queue_size, latched);
00091 }
00092
00094 void stop()
00095 {
00096 keep_running_ = false;
00097 #ifdef NON_POLLING
00098 updated_cond_.notify_one();
00099 #endif
00100 }
00101
00108 bool trylock()
00109 {
00110 if (msg_mutex_.try_lock())
00111 {
00112 if (turn_ == REALTIME)
00113 {
00114 return true;
00115 }
00116 else
00117 {
00118 msg_mutex_.unlock();
00119 return false;
00120 }
00121 }
00122 else
00123 {
00124 return false;
00125 }
00126 }
00127
00134 void unlockAndPublish()
00135 {
00136 turn_ = NON_REALTIME;
00137 msg_mutex_.unlock();
00138 #ifdef NON_POLLING
00139 updated_cond_.notify_one();
00140 #endif
00141 }
00142
00149 void lock()
00150 {
00151 #ifdef NON_POLLING
00152 msg_mutex_.lock();
00153 #else
00154
00155 while (!msg_mutex_.try_lock())
00156 usleep(200);
00157 #endif
00158 }
00159
00163 void unlock()
00164 {
00165 msg_mutex_.unlock();
00166 }
00167
00168 private:
00169 void construct(int queue_size, bool latched=false)
00170 {
00171 publisher_ = node_.advertise<Msg>(topic_, queue_size, latched);
00172 keep_running_ = true;
00173 thread_ = boost::thread(&RealtimePublisher::publishingLoop, this);
00174 }
00175
00176
00177 bool is_running() const { return is_running_; }
00178
00179 void publishingLoop()
00180 {
00181 is_running_ = true;
00182 turn_ = REALTIME;
00183
00184 while (keep_running_)
00185 {
00186 Msg outgoing;
00187
00188
00189 lock();
00190 while (turn_ != NON_REALTIME && keep_running_)
00191 {
00192 #ifdef NON_POLLING
00193 updated_cond_.wait(lock);
00194 #else
00195 unlock();
00196 usleep(500);
00197 lock();
00198 #endif
00199 }
00200 outgoing = msg_;
00201 turn_ = REALTIME;
00202
00203 unlock();
00204
00205
00206 if (keep_running_)
00207 publisher_.publish(outgoing);
00208 }
00209 is_running_ = false;
00210 }
00211
00212 std::string topic_;
00213 ros::NodeHandle node_;
00214 ros::Publisher publisher_;
00215 volatile bool is_running_;
00216 volatile bool keep_running_;
00217
00218 boost::thread thread_;
00219
00220 boost::mutex msg_mutex_;
00221
00222 #ifdef NON_POLLING
00223 boost::condition_variable updated_cond_;
00224 #endif
00225
00226 enum {REALTIME, NON_REALTIME};
00227 int turn_;
00228 };
00229
00230 }
00231
00232 #endif