queue.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Johannes Meyer, TU Darmstadt
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 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef HECTOR_POSE_ESTIMATION_QUEUE_H
00030 #define HECTOR_POSE_ESTIMATION_QUEUE_H
00031 
00032 #include <boost/array.hpp>
00033 #include <hector_pose_estimation/measurement_update.h>
00034 
00035 namespace hector_pose_estimation {
00036 
00037 class Queue {
00038 public:
00039   static const size_t capacity_ = 10;
00040 
00041   virtual ~Queue() {}
00042   virtual bool empty() const = 0;
00043   virtual bool full() const = 0;
00044   virtual size_t size() const = 0;
00045   virtual size_t capacity() const = 0;
00046 
00047   virtual void push(const MeasurementUpdate& update) = 0;
00048   virtual const MeasurementUpdate& pop() = 0;
00049   virtual void clear() = 0;
00050 };
00051 
00052 template <class Update>
00053 class Queue_ : public Queue
00054 {
00055 public:
00056   Queue_() : in_(0), out_(0), size_(0) {}
00057   virtual ~Queue_() {}
00058 
00059   virtual bool empty() const { return size_ == 0; }
00060   virtual bool full() const { return size_ == capacity_; }
00061   virtual size_t size() const { return size_; }
00062   virtual size_t capacity() const { return capacity_; }
00063 
00064   virtual void push(const MeasurementUpdate& update) {
00065     if (full()) return;
00066     data_[inc(in_)] = static_cast<Update const &>(update);
00067     size_++;
00068   }
00069 
00070   virtual const Update& pop() {
00071     if (empty()) throw std::runtime_error("queue is empty");
00072     size_--;
00073     return data_[inc(out_)];
00074   }
00075 
00076   virtual void clear() { out_ = in_ = size_ = 0; }
00077 
00078 private:
00079   static size_t inc(size_t& index) { size_t temp = index++; index %= Queue::capacity_; return temp; }
00080 
00081   boost::array<Update, Queue::capacity_> data_;
00082   size_t in_, out_, size_;
00083 };
00084 
00085 } // namespace hector_pose_estimation
00086 
00087 #endif // HECTOR_POSE_ESTIMATION_QUEUE_H


hector_pose_estimation_core
Author(s): Johannes Meyer
autogenerated on Sat Jun 8 2019 20:51:04