ros2_canopen  master
C++ ROS CANopen Library
exchange.hpp
Go to the documentation of this file.
1 // Copyright 2022 Harshavadan Deshpande
2 // Christoph Hellmann Santos
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 #ifndef EXCHANGE_HPP
16 #define EXCHANGE_HPP
17 
18 #include <boost/lockfree/queue.hpp>
19 #include <boost/optional.hpp>
20 #include <boost/thread.hpp>
21 
22 namespace ros2_canopen
23 {
24 
25 struct COData
26 {
27 public:
28  uint16_t index_;
29  uint8_t subindex_;
30  uint32_t data_;
31 };
32 
33 struct COEmcy
34 {
35 public:
36  uint16_t eec;
37  uint8_t er;
38  uint8_t msef[5];
39 };
40 
47 template <typename T>
48 class SafeQueue
49 {
50 private:
51  std::size_t capacity_;
52  std::unique_ptr<boost::lockfree::queue<T>> queue_;
53 
54 public:
59  explicit SafeQueue(std::size_t capacity = 10)
60  : capacity_(capacity), queue_(new boost::lockfree::queue<T>(capacity_))
61  {
62  }
63 
68  void push(T value) { queue_->push(std::move(value)); }
69 
74  std::optional<T> try_pop()
75  {
76  T value;
77  if (queue_->pop(value)) return std::optional<T>(std::move(value));
78  return std::optional<T>();
79  }
80 
85  bool try_pop(T & value)
86  {
87  if (queue_->pop(value)) return true;
88  return false;
89  }
90 
95  boost::optional<T> wait_and_pop()
96  {
97  T value;
98  while (!queue_->pop(value)) boost::this_thread::yield();
99  return value;
100  }
101 
106  void wait_and_pop(T & value)
107  {
108  while (!queue_->pop(value)) boost::this_thread::yield();
109  }
110 
116  boost::optional<T> wait_and_pop_for(const std::chrono::milliseconds & timeout)
117  {
118  T value;
119  auto start_time = std::chrono::steady_clock::now();
120  while (!queue_->pop(value))
121  {
122  if (
123  timeout != std::chrono::milliseconds::zero() &&
124  std::chrono::steady_clock::now() - start_time >= timeout)
125  return boost::none;
126  boost::this_thread::yield();
127  }
128  return value;
129  }
130 
136  bool wait_and_pop_for(const std::chrono::milliseconds & timeout, T & value)
137  {
138  auto start_time = std::chrono::steady_clock::now();
139  while (!queue_->pop(value))
140  {
141  if (
142  timeout != std::chrono::milliseconds::zero() &&
143  std::chrono::steady_clock::now() - start_time >= timeout)
144  return false;
145  boost::this_thread::yield();
146  }
147  return true;
148  }
149 
150  bool empty() const { return queue_->empty(); }
151 };
152 } // namespace ros2_canopen
153 
154 #endif // EXCHANGE_HPP
Thread Safe Queue for CANOpen Data Exchange.
Definition: exchange.hpp:49
bool try_pop(T &value)
Try to pop a value from the queue.
Definition: exchange.hpp:85
void wait_and_pop(T &value)
Wait for a value to be available in the queue for a given timeout.
Definition: exchange.hpp:106
std::optional< T > try_pop()
Try to pop a value from the queue.
Definition: exchange.hpp:74
SafeQueue(std::size_t capacity=10)
Constructor for the SafeQueue.
Definition: exchange.hpp:59
boost::optional< T > wait_and_pop()
Wait for a value to be available in the queue.
Definition: exchange.hpp:95
bool empty() const
Definition: exchange.hpp:150
boost::optional< T > wait_and_pop_for(const std::chrono::milliseconds &timeout)
Wait for a value to be available in the queue for a given timeout.
Definition: exchange.hpp:116
void push(T value)
Push a value to the queue.
Definition: exchange.hpp:68
bool wait_and_pop_for(const std::chrono::milliseconds &timeout, T &value)
Wait for a value to be available in the queue for a given timeout.
Definition: exchange.hpp:136
Definition: configuration_manager.hpp:28
Definition: exchange.hpp:26
uint32_t data_
Definition: exchange.hpp:30
uint8_t subindex_
Definition: exchange.hpp:29
uint16_t index_
Definition: exchange.hpp:28
Definition: exchange.hpp:34
uint16_t eec
Definition: exchange.hpp:36
uint8_t er
Definition: exchange.hpp:37
uint8_t msef[5]
Definition: exchange.hpp:38