00001 /* 00002 * Copyright (c) 2013 hiDOF, 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: Wim Meeussen 00037 */ 00038 00039 #ifndef REALTIME_TOOLS_REALTIME_BUFFER 00040 #define REALTIME_TOOLS_REALTIME_BUFFER 00041 00042 #include <boost/thread/mutex.hpp> 00043 00044 namespace realtime_tools 00045 { 00046 00047 template <class T> 00048 class RealtimeBuffer 00049 { 00050 public: 00051 RealtimeBuffer() 00052 : new_data_available_(false) 00053 { 00054 // allocate memory 00055 non_realtime_data_ = new T(); 00056 realtime_data_ = new T(); 00057 } 00058 00059 ~RealtimeBuffer() 00060 { 00061 if (non_realtime_data_) 00062 delete non_realtime_data_; 00063 if (realtime_data_) 00064 delete realtime_data_; 00065 } 00066 00067 RealtimeBuffer(RealtimeBuffer &source) 00068 { 00069 // allocate memory 00070 non_realtime_data_ = new T(); 00071 realtime_data_ = new T(); 00072 00073 // Copy the data from old RTB to new RTB 00074 writeFromNonRT(*source.readFromNonRT()); 00075 } 00076 00080 RealtimeBuffer &operator =(const RealtimeBuffer& source) 00081 { 00082 if (this == &source) 00083 return *this; 00084 00085 // Copy the data from old RTB to new RTB 00086 writeFromNonRT(*source.readFromNonRT()); 00087 00088 return *this; 00089 } 00090 00091 T* readFromRT() 00092 { 00093 // Check if the data is currently being written to (is locked) 00094 if (mutex_.try_lock()) 00095 { 00096 // swap pointers 00097 if (new_data_available_) 00098 { 00099 T* tmp = realtime_data_; 00100 realtime_data_ = non_realtime_data_; 00101 non_realtime_data_ = tmp; 00102 new_data_available_ = false; 00103 } 00104 mutex_.unlock(); 00105 } 00106 return realtime_data_; 00107 } 00108 00109 T* readFromNonRT() const 00110 { 00111 boost::mutex::scoped_lock lock(mutex_); 00112 00113 if (new_data_available_) 00114 return non_realtime_data_; 00115 else 00116 return realtime_data_; 00117 } 00118 00119 void writeFromNonRT(const T& data) 00120 { 00121 // get lock 00122 lock(); 00123 00124 // copy data into non-realtime buffer 00125 *non_realtime_data_ = data; 00126 new_data_available_ = true; 00127 00128 // release lock 00129 mutex_.unlock(); 00130 } 00131 00132 void initRT(const T& data) 00133 { 00134 *non_realtime_data_ = data; 00135 *realtime_data_ = data; 00136 } 00137 00138 private: 00139 void lock() 00140 { 00141 #ifdef NON_POLLING 00142 mutex_.lock(); 00143 #else 00144 while (!mutex_.try_lock()) 00145 usleep(500); 00146 #endif 00147 } 00148 00149 T* realtime_data_; 00150 T* non_realtime_data_; 00151 bool new_data_available_; 00152 00153 // Set as mutable so that readFromNonRT() can be performed on a const buffer 00154 mutable boost::mutex mutex_; 00155 00156 }; // class 00157 }// namespace 00158 00159 #endif