ROSaic
circular_buffer.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // © Copyright 2020, Septentrio NV/SA.
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // 2. Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // 3. Neither the name of the copyright holder nor the names of its
14 // contributors may be used to endorse or promote products derived
15 // from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // *****************************************************************************
30 
32 
39 CircularBuffer::CircularBuffer(std::size_t capacity): head_(0), tail_(0), size_(0), capacity_(capacity)
40 {
41  data_ = new uint8_t[capacity];
42 }
43 
46 {
47  delete [] data_;
48  data_ = NULL;
49 }
50 
51 std::size_t CircularBuffer::write(const uint8_t *data, std::size_t bytes)
52 {
53  //ROS_DEBUG("Called CircularBuffer::write() method..");
54  if (bytes == 0) return 0;
55 
56  std::size_t capacity = capacity_;
57  std::size_t bytes_to_write = std::min(bytes, capacity - size_);
58  //ROS_DEBUG("Number of bytes_to_write is %li.", bytes_to_write);
59  if (bytes_to_write != bytes)
60  {
61  ROS_ERROR("You are trying to overwrite parts of the circular buffer that have not yet been read!");
62  }
63 
64  // Writes in a single step
65  if (bytes_to_write <= capacity - head_)
66  {
67 
68  memcpy(data_ + head_, data, bytes_to_write);
69  head_ += bytes_to_write;
70  if (head_ == capacity) head_ = 0;
71  }
72  // Writes in two steps. Here the circular nature comes to the surface
73  else
74  {
75  std::size_t size_1 = capacity - head_;
76  memcpy(data_ + head_, data, size_1);
77  std::size_t size_2 = bytes_to_write - size_1;
78  memcpy(data_, data + size_1, size_2);
79  head_ = size_2; // Hence setting head_ = 0 three lines above was not necessary.
80  }
81  size_ += bytes_to_write;
82  //ROS_DEBUG("Leaving CircularBuffer::write() method..");
83  return bytes_to_write;
84 }
85 
86 std::size_t CircularBuffer::read(uint8_t *data, std::size_t bytes)
87 {
88  //ROS_DEBUG("Called CircularBuffer::read() method..");
89  if (bytes == 0) return 0;
90  std::size_t capacity = capacity_;
91  std::size_t bytes_to_read = std::min(bytes, size_);
92  //ROS_DEBUG("Number of bytes_to_read is %li.", bytes_to_read);
93  if (bytes_to_read != bytes)
94  {
95  ROS_ERROR("You are trying to read parts of the circular buffer that have not yet been written!");
96  }
97 
98  // Read in a single step
99  if (bytes_to_read <= capacity - tail_) // Note that it is not size_ - tail_:
100  // If write() hasn't written something into all of capacity yet (first round of writing), we would still read those unknown bytes..
101  {
102  memcpy(data, data_ + tail_, bytes_to_read);
103  tail_ += bytes_to_read;
104  if (tail_ == capacity) tail_ = 0; // Same here?
105  }
106 
107  // Read in two steps
108  else
109  {
110  std::size_t size_1 = capacity - tail_;
111  memcpy(data, data_ + tail_, size_1);
112  std::size_t size_2 = bytes_to_read - size_1;
113  memcpy(data + size_1, data_, size_2);
114  tail_ = size_2;
115  }
116 
117  size_ -= bytes_to_read;
118  //ROS_DEBUG("Leaving CircularBuffer::read() method..");
119  return bytes_to_read;
120 }
std::size_t tail_
Specifies where we start reading.
uint8_t * data_
Pointer that always points to the same memory address, hence could be const pointer.
std::size_t size_
Number of bytes that have been written but not yet read.
std::size_t write(const uint8_t *data, std::size_t bytes)
Returns number of bytes written.
CircularBuffer(std::size_t capacity)
Constructor of CircularBuffer.
std::size_t capacity_
Capacity of the circular buffer.
std::size_t head_
Specifies where we start writing.
std::size_t capacity() const
Returns capacity_.
Declares a class for creating, writing to and reading from a circular bufffer.
std::size_t read(uint8_t *data, std::size_t bytes)
Returns number of bytes read.
~CircularBuffer()
Destructor of CircularBuffer.