realtime_publisher.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Publishing ROS messages is difficult, as the publish function is
00032  * not realtime safe.  This class provides the proper locking so that
00033  * you can call publish in realtime and a separate (non-realtime)
00034  * thread will ensure that the message gets published over ROS.
00035  *
00036  * Author: Stuart Glaser
00037  */
00038 #ifndef REALTIME_TOOLS__REALTIME_PUBLISHER_H_
00039 #define REALTIME_TOOLS__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();  // So the publishing loop can exit
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     // never actually block on the lock
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       // Locks msg_ and copies it
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       // Sends the outgoing message
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_;  // Protects msg_
00221 
00222 #ifdef NON_POLLING
00223   boost::condition_variable updated_cond_;
00224 #endif
00225 
00226   enum {REALTIME, NON_REALTIME};
00227   int turn_;  // Who's turn is it to use msg_?
00228 };
00229 
00230 }
00231 
00232 #endif


realtime_tools
Author(s): Stuart Glaser
autogenerated on Thu Jun 6 2019 20:05:02