00001
00020 #ifndef RingBuffer_h
00021 #define RingBuffer_h
00022
00023 #include <vector>
00024 #include <algorithm>
00025 #include <iostream>
00026
00027 #include <coil/TimeValue.h>
00028 #include <coil/Mutex.h>
00029 #include <coil/Guard.h>
00030 #include <coil/Condition.h>
00031 #include <coil/stringutil.h>
00032
00033 #include <rtm/BufferBase.h>
00034 #include <rtm/BufferStatus.h>
00035
00036 #define RINGBUFFER_DEFAULT_LENGTH 8
00037
00051 namespace RTC
00052 {
00088 template <class DataType>
00089 class RingBuffer
00090 : public BufferBase<DataType>
00091 {
00092 public:
00093 BUFFERSTATUS_ENUM
00094 typedef coil::Guard<coil::Mutex> Guard;
00118 RingBuffer(long int length = RINGBUFFER_DEFAULT_LENGTH)
00119 : m_overwrite(true), m_readback(true),
00120 m_timedwrite(false), m_timedread(false),
00121 m_wtimeout(1, 0), m_rtimeout(1, 0),
00122 m_length(length),
00123 m_wpos(0), m_rpos(0), m_fillcount(0),
00124 m_buffer(m_length)
00125 {
00126 this->reset();
00127 }
00128
00144 virtual ~RingBuffer(void)
00145 {
00146 }
00147
00187 virtual void init(const coil::Properties& prop)
00188 {
00189 initLength(prop);
00190 initWritePolicy(prop);
00191 initReadPolicy(prop);
00192 }
00193
00214 virtual size_t length(void) const
00215 {
00216 Guard guard(m_posmutex);
00217 return m_length;
00218 }
00219
00242 virtual ReturnCode length(size_t n)
00243 {
00244 m_buffer.resize(n);
00245 m_length = n;
00246 this->reset();
00247 return ::RTC::BufferStatus::BUFFER_OK;
00248 }
00249
00272 virtual ReturnCode reset()
00273 {
00274 Guard guard(m_posmutex);
00275 m_fillcount = 0;
00276 m_wpos = 0;
00277 m_rpos = 0;
00278 return ::RTC::BufferStatus::BUFFER_OK;
00279 }
00280
00281
00282
00283
00304 virtual DataType* wptr(long int n = 0)
00305 {
00306 Guard guard(m_posmutex);
00307 return &m_buffer[(m_wpos + n) % m_length];
00308 }
00309
00333 virtual ReturnCode advanceWptr(long int n = 1)
00334 {
00335
00336
00337
00338
00339
00340
00341
00342 if (n > 0 && n > static_cast<long int>(m_length - m_fillcount) ||
00343 n < 0 && n < static_cast<long int>(-m_fillcount))
00344 {
00345 return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
00346 }
00347
00348 Guard guard(m_posmutex);
00349 m_wpos = (m_wpos + n) % m_length;
00350 m_fillcount += n;
00351 return ::RTC::BufferStatus::BUFFER_OK;
00352 }
00380 virtual ReturnCode put(const DataType& value)
00381 {
00382 Guard guard(m_posmutex);
00383 m_buffer[m_wpos] = value;
00384 return ::RTC::BufferStatus::BUFFER_OK;
00385 }
00386
00428 virtual ReturnCode write(const DataType& value,
00429 long int sec = -1, long int nsec = 0)
00430 {
00431 Guard guard(m_full.mutex);
00432
00433 if (full())
00434 {
00435
00436 bool timedwrite(m_timedwrite);
00437 bool overwrite(m_overwrite);
00438
00439 if (!(sec < 0))
00440 {
00441 timedwrite = true;
00442 overwrite = false;
00443 }
00444
00445 if (overwrite && !timedwrite)
00446 {
00447 advanceRptr();
00448 }
00449 else if (!overwrite && !timedwrite)
00450 {
00451 return ::RTC::BufferStatus::BUFFER_FULL;
00452 }
00453 else if (!overwrite && timedwrite)
00454 {
00455 if (sec < 0)
00456 {
00457 sec = m_wtimeout.sec();
00458 nsec = m_wtimeout.usec() * 1000;
00459 }
00460
00461 if (!m_full.cond.wait(sec, nsec))
00462 {
00463 return ::RTC::BufferStatus::TIMEOUT;
00464 }
00465 }
00466 else
00467 {
00468 return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
00469 }
00470 }
00471
00472 bool empty_(empty());
00473
00474 put(value);
00475
00476 if (empty_)
00477 {
00478 Guard eguard(m_empty.mutex);
00479 m_empty.cond.signal();
00480 }
00481 advanceWptr(1);
00482 return ::RTC::BufferStatus::BUFFER_OK;
00483 }
00484
00506 virtual size_t writable() const
00507 {
00508 Guard guard(m_posmutex);
00509 return m_length - m_fillcount;
00510 }
00511
00531 virtual bool full(void) const
00532 {
00533 Guard guard(m_posmutex);
00534 return m_length == m_fillcount;
00535 }
00536
00537
00558 virtual DataType* rptr(long int n = 0)
00559 {
00560 Guard guard(m_posmutex);
00561 return &(m_buffer[(m_rpos + n) % m_length]);
00562 }
00563
00585 virtual ReturnCode advanceRptr(long int n = 1)
00586 {
00587
00588
00589
00590
00591
00592
00593 if ((n > 0 && n > static_cast<long int>(m_fillcount)) ||
00594 (n < 0 && n < static_cast<long int>(m_fillcount - m_length)))
00595 {
00596 return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
00597 }
00598
00599 Guard guard(m_posmutex);
00600 m_rpos = (m_rpos + n) % m_length;
00601 m_fillcount -= n;
00602 return ::RTC::BufferStatus::BUFFER_OK;
00603 }
00604
00629 virtual ReturnCode get(DataType& value)
00630 {
00631 Guard gaurd(m_posmutex);
00632 value = m_buffer[m_rpos];
00633 return ::RTC::BufferStatus::BUFFER_OK;
00634 }
00635
00636
00637 virtual DataType& get()
00638 {
00639 Guard gaurd(m_posmutex);
00640 return m_buffer[m_rpos];
00641 }
00642
00643
00685 virtual ReturnCode read(DataType& value,
00686 long int sec = -1, long int nsec = 0)
00687 {
00688 Guard gaurd(m_empty.mutex);
00689
00690 if (empty())
00691 {
00692 bool timedread(m_timedread);
00693 bool readback(m_readback);
00694
00695 if (!(sec < 0))
00696 {
00697 timedread = true;
00698 readback = false;
00699 sec = m_rtimeout.sec();
00700 nsec = m_rtimeout.usec() * 1000;
00701 }
00702
00703 if (readback && !timedread)
00704 {
00705 advanceRptr(-1);
00706 }
00707 else if (!readback && !timedread)
00708 {
00709 return ::RTC::BufferStatus::BUFFER_EMPTY;
00710 }
00711 else if (!readback && timedread)
00712 {
00713
00714 if (!m_empty.cond.wait(sec, nsec))
00715 {
00716 return ::RTC::BufferStatus::TIMEOUT;
00717 }
00718 }
00719 else
00720 {
00721 return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
00722 }
00723 }
00724
00725 bool full_(full());
00726
00727 get(value);
00728 advanceRptr();
00729
00730 if (full_)
00731 {
00732 Guard fguard(m_full.mutex);
00733 m_full.cond.signal();
00734 }
00735
00736 return ::RTC::BufferStatus::BUFFER_OK;
00737 }
00738
00763 virtual size_t readable() const
00764 {
00765 Guard guard(m_posmutex);
00766 return m_fillcount;
00767 }
00768
00788 virtual bool empty(void) const
00789 {
00790 Guard guard(m_posmutex);
00791 return m_fillcount == 0;
00792 }
00793
00794 private:
00795 inline void initLength(const coil::Properties& prop)
00796 {
00797 if (!prop["length"].empty())
00798 {
00799 size_t n;
00800 if (coil::stringTo(n, prop["length"].c_str()))
00801 {
00802 if (n > 0)
00803 {
00804 this->length(n);
00805 }
00806 }
00807 }
00808 }
00809
00810 inline void initWritePolicy(const coil::Properties& prop)
00811 {
00812 std::string policy(prop["write.full_policy"]);
00813 coil::normalize(policy);
00814 if (policy == "overwrite")
00815 {
00816 m_overwrite = true;
00817 m_timedwrite = false;
00818 }
00819 else if (policy == "do_nothing")
00820 {
00821 m_overwrite = false;
00822 m_timedwrite = false;
00823 }
00824 else if (policy == "block")
00825 {
00826 m_overwrite = false;
00827 m_timedwrite = true;
00828
00829 double tm;
00830 if (coil::stringTo(tm, prop["write.timeout"].c_str()))
00831 {
00832 if (!(tm < 0))
00833 {
00834 m_wtimeout = tm;
00835 }
00836 }
00837 }
00838 }
00839
00840 inline void initReadPolicy(const coil::Properties& prop)
00841 {
00842 std::string policy(prop["read.empty_policy"]);
00843 if (policy == "readback")
00844 {
00845 m_readback = true;
00846 m_timedread = false;
00847 }
00848 else if (policy == "do_nothing")
00849 {
00850 m_readback = false;
00851 m_timedread = false;
00852 }
00853 else if (policy == "block")
00854 {
00855 m_readback = false;
00856 m_timedread = true;
00857 double tm;
00858 if (coil::stringTo(tm, prop["read.timeout"].c_str()))
00859 {
00860 m_rtimeout = tm;
00861 }
00862 }
00863 }
00864
00865 private:
00866 bool m_overwrite;
00867 bool m_readback;
00868 bool m_timedwrite;
00869 bool m_timedread;
00870 coil::TimeValue m_wtimeout;
00871 coil::TimeValue m_rtimeout;
00872
00873 size_t m_length;
00874 size_t m_wpos;
00875 size_t m_rpos;
00876 size_t m_fillcount;
00877 std::vector<DataType> m_buffer;
00878
00879 struct condition
00880 {
00881 condition() : cond(mutex) {}
00882 coil::Condition<coil::Mutex> cond;
00883 coil::Mutex mutex;
00884 };
00885
00886 mutable coil::Mutex m_posmutex;
00887 condition m_empty;
00888 condition m_full;
00889 };
00890 };
00891
00892 #endif // RingBuffer_h