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_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     boost::unique_lock<boost::mutex> lock(msg_mutex_);
00098     updated_cond_.notify_one();  // So the publishing loop can exit
00099   }
00100 
00107   bool trylock()
00108   {
00109     if (msg_mutex_.try_lock())
00110     {
00111       if (turn_ == REALTIME)
00112       {
00113         return true;
00114       }
00115       else
00116       {
00117         msg_mutex_.unlock();
00118         return false;
00119       }
00120     }
00121     else
00122       return false;
00123   }
00124 
00131   void unlockAndPublish()
00132   {
00133     turn_ = NON_REALTIME;
00134     msg_mutex_.unlock();
00135     updated_cond_.notify_one();
00136   }
00137 
00144   void lock()
00145   {
00146     msg_mutex_.lock();
00147   }
00148 
00152   void unlock()
00153   {
00154     msg_mutex_.unlock();
00155   }
00156 
00157 private:
00158   void construct(int queue_size, bool latched=false)
00159   {
00160     publisher_ = node_.advertise<Msg>(topic_, queue_size, latched);
00161     keep_running_ = true;
00162     thread_ = boost::thread(&RealtimePublisher::publishingLoop, this);
00163   }
00164 
00165 
00166   bool is_running() const { return is_running_; }
00167 
00168   void publishingLoop()
00169   {
00170     is_running_ = true;
00171     turn_ = REALTIME;
00172     while (keep_running_)
00173     {
00174       Msg outgoing;
00175       // Locks msg_ and copies it
00176       {
00177         boost::unique_lock<boost::mutex> lock(msg_mutex_);
00178         while (turn_ != NON_REALTIME)
00179         {
00180           if (!keep_running_)
00181             break;
00182           updated_cond_.wait(lock);
00183         }
00184 
00185         outgoing = msg_;
00186         turn_ = REALTIME;
00187       }
00188 
00189       // Sends the outgoing message
00190       if (keep_running_)
00191         publisher_.publish(outgoing);
00192     }
00193     is_running_ = false;
00194   }
00195 
00196   std::string topic_;
00197   ros::NodeHandle node_;
00198   ros::Publisher publisher_;
00199   volatile bool is_running_;
00200   volatile bool keep_running_;
00201 
00202   boost::thread thread_;
00203 
00204   boost::mutex msg_mutex_;  // Protects msg_
00205   boost::condition_variable updated_cond_;
00206 
00207   enum {REALTIME, NON_REALTIME};
00208   int turn_;  // Who's turn is it to use msg_?
00209 };
00210 
00211 }
00212 
00213 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends


realtime_tools
Author(s): Stuart Glaser
autogenerated on Tue Jan 8 2013 19:29:39