00001
00020 #ifndef RTC_RINGBUFFER_H
00021 #define RTC_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), m_wcount(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_wcount = 0;
00277 m_wpos = 0;
00278 m_rpos = 0;
00279 return ::RTC::BufferStatus::BUFFER_OK;
00280 }
00281
00282
00283
00284
00305 virtual DataType* wptr(long int n = 0)
00306 {
00307 Guard guard(m_posmutex);
00308 return &m_buffer[(m_wpos + n + m_length) % m_length];
00309 }
00310
00334 virtual ReturnCode advanceWptr(long int n = 1)
00335 {
00336
00337
00338
00339
00340
00341
00342
00343 if (n > 0 && n > static_cast<long int>(m_length - m_fillcount) ||
00344 n < 0 && n < static_cast<long int>(-m_fillcount))
00345 {
00346 return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
00347 }
00348
00349 Guard guard(m_posmutex);
00350 m_wpos = (m_wpos + n + m_length) % m_length;
00351 m_fillcount += n;
00352 m_wcount += n;
00353 return ::RTC::BufferStatus::BUFFER_OK;
00354 }
00382 virtual ReturnCode put(const DataType& value)
00383 {
00384 Guard guard(m_posmutex);
00385 m_buffer[m_wpos] = value;
00386 return ::RTC::BufferStatus::BUFFER_OK;
00387 }
00388
00430 virtual ReturnCode write(const DataType& value,
00431 long int sec = -1, long int nsec = 0)
00432 {
00433 Guard guard(m_full.mutex);
00434
00435 if (full())
00436 {
00437
00438 bool timedwrite(m_timedwrite);
00439 bool overwrite(m_overwrite);
00440
00441 if (!(sec < 0))
00442 {
00443 timedwrite = true;
00444 overwrite = false;
00445 }
00446
00447 if (overwrite && !timedwrite)
00448 {
00449 advanceRptr();
00450 }
00451 else if (!overwrite && !timedwrite)
00452 {
00453 return ::RTC::BufferStatus::BUFFER_FULL;
00454 }
00455 else if (!overwrite && timedwrite)
00456 {
00457 if (sec < 0)
00458 {
00459 sec = m_wtimeout.sec();
00460 nsec = m_wtimeout.usec() * 1000;
00461 }
00462
00463 if (!m_full.cond.wait(sec, nsec))
00464 {
00465 return ::RTC::BufferStatus::TIMEOUT;
00466 }
00467 }
00468 else
00469 {
00470 return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
00471 }
00472 }
00473
00474 bool empty_(empty());
00475
00476 put(value);
00477
00478 if (empty_)
00479 {
00480 Guard eguard(m_empty.mutex);
00481 m_empty.cond.signal();
00482 }
00483 advanceWptr(1);
00484 return ::RTC::BufferStatus::BUFFER_OK;
00485 }
00486
00508 virtual size_t writable() const
00509 {
00510 Guard guard(m_posmutex);
00511 return m_length - m_fillcount;
00512 }
00513
00533 virtual bool full(void) const
00534 {
00535 Guard guard(m_posmutex);
00536 return m_length == m_fillcount;
00537 }
00538
00539
00560 virtual DataType* rptr(long int n = 0)
00561 {
00562 Guard guard(m_posmutex);
00563 return &(m_buffer[(m_rpos + n + m_length) % m_length]);
00564 }
00565
00587 virtual ReturnCode advanceRptr(long int n = 1)
00588 {
00589
00590
00591
00592
00593
00594
00595 if ((n > 0 && n > static_cast<long int>(m_fillcount)) ||
00596 (n < 0 && n < static_cast<long int>(m_fillcount - m_length)))
00597 {
00598 return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
00599 }
00600
00601 Guard guard(m_posmutex);
00602 m_rpos = (m_rpos + n + m_length) % m_length;
00603 m_fillcount -= n;
00604 return ::RTC::BufferStatus::BUFFER_OK;
00605 }
00606
00631 virtual ReturnCode get(DataType& value)
00632 {
00633 Guard gaurd(m_posmutex);
00634 value = m_buffer[m_rpos];
00635 return ::RTC::BufferStatus::BUFFER_OK;
00636 }
00637
00638
00656 virtual DataType& get()
00657 {
00658 Guard gaurd(m_posmutex);
00659 return m_buffer[m_rpos];
00660 }
00661
00662
00704 virtual ReturnCode read(DataType& value,
00705 long int sec = -1, long int nsec = 0)
00706 {
00707 Guard gaurd(m_empty.mutex);
00708
00709 if (empty())
00710 {
00711 bool timedread(m_timedread);
00712 bool readback(m_readback);
00713
00714 if (!(sec < 0))
00715 {
00716 timedread = true;
00717 readback = false;
00718 sec = m_rtimeout.sec();
00719 nsec = m_rtimeout.usec() * 1000;
00720 }
00721
00722 if (readback && !timedread)
00723 {
00724 if (!(m_wcount > 0))
00725 {
00726 return ::RTC::BufferStatus::BUFFER_EMPTY;
00727 }
00728 advanceRptr(-1);
00729 }
00730 else if (!readback && !timedread)
00731 {
00732 return ::RTC::BufferStatus::BUFFER_EMPTY;
00733 }
00734 else if (!readback && timedread)
00735 {
00736 if (sec < 0)
00737 {
00738 sec = m_rtimeout.sec();
00739 nsec = m_rtimeout.usec() * 1000;
00740 }
00741
00742 if (!m_empty.cond.wait(sec, nsec))
00743 {
00744 return ::RTC::BufferStatus::TIMEOUT;
00745 }
00746 }
00747 else
00748 {
00749 return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
00750 }
00751 }
00752
00753 bool full_(full());
00754
00755 get(value);
00756 advanceRptr();
00757
00758 if (full_)
00759 {
00760 Guard fguard(m_full.mutex);
00761 m_full.cond.signal();
00762 }
00763
00764 return ::RTC::BufferStatus::BUFFER_OK;
00765 }
00766
00791 virtual size_t readable() const
00792 {
00793 Guard guard(m_posmutex);
00794 return m_fillcount;
00795 }
00796
00816 virtual bool empty(void) const
00817 {
00818 Guard guard(m_posmutex);
00819 return m_fillcount == 0;
00820 }
00821
00822 private:
00823 inline void initLength(const coil::Properties& prop)
00824 {
00825 if (!prop["length"].empty())
00826 {
00827 size_t n;
00828 if (coil::stringTo(n, prop["length"].c_str()))
00829 {
00830 if (n > 0)
00831 {
00832 this->length(n);
00833 }
00834 }
00835 }
00836 }
00837
00838 inline void initWritePolicy(const coil::Properties& prop)
00839 {
00840 std::string policy(prop["write.full_policy"]);
00841 coil::normalize(policy);
00842 if (policy == "overwrite")
00843 {
00844 m_overwrite = true;
00845 m_timedwrite = false;
00846 }
00847 else if (policy == "do_nothing")
00848 {
00849 m_overwrite = false;
00850 m_timedwrite = false;
00851 }
00852 else if (policy == "block")
00853 {
00854 m_overwrite = false;
00855 m_timedwrite = true;
00856
00857 double tm;
00858 if (coil::stringTo(tm, prop["write.timeout"].c_str()))
00859 {
00860 if (!(tm < 0))
00861 {
00862 m_wtimeout = tm;
00863 }
00864 }
00865 }
00866 }
00867
00868 inline void initReadPolicy(const coil::Properties& prop)
00869 {
00870 std::string policy(prop["read.empty_policy"]);
00871 if (policy == "readback")
00872 {
00873 m_readback = true;
00874 m_timedread = false;
00875 }
00876 else if (policy == "do_nothing")
00877 {
00878 m_readback = false;
00879 m_timedread = false;
00880 }
00881 else if (policy == "block")
00882 {
00883 m_readback = false;
00884 m_timedread = true;
00885 double tm;
00886 if (coil::stringTo(tm, prop["read.timeout"].c_str()))
00887 {
00888 m_rtimeout = tm;
00889 }
00890 }
00891 }
00892
00893 private:
00894 bool m_overwrite;
00895 bool m_readback;
00896 bool m_timedwrite;
00897 bool m_timedread;
00898 coil::TimeValue m_wtimeout;
00899 coil::TimeValue m_rtimeout;
00900
00901 size_t m_length;
00902 size_t m_wpos;
00903 size_t m_rpos;
00904 size_t m_fillcount;
00905 size_t m_wcount;
00906 std::vector<DataType> m_buffer;
00907
00908 struct condition
00909 {
00910 condition() : cond(mutex) {}
00911 coil::Condition<coil::Mutex> cond;
00912 coil::Mutex mutex;
00913 };
00914
00915 mutable coil::Mutex m_posmutex;
00916 condition m_empty;
00917 condition m_full;
00918 };
00919 };
00920
00921 #endif // RTC_RINGBUFFER_H