ROSaic
async_manager.hpp
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 
31 // *****************************************************************************
32 //
33 // Boost Software License - Version 1.0 - August 17th, 2003
34 //
35 // Permission is hereby granted, free of charge, to any person or organization
36 // obtaining a copy of the software and accompanying documentation covered by
37 // this license (the "Software") to use, reproduce, display, distribute,
38 // execute, and transmit the Software, and to prepare derivative works of the
39 // Software, and to permit third-parties to whom the Software is furnished to
40 // do so, all subject to the following:
41 
42 // The copyright notices in the Software and this entire statement, including
43 // the above license grant, this restriction and the following disclaimer,
44 // must be included in all copies of the Software, in whole or in part, and
45 // all derivative works of the Software, unless such copies or derivative
46 // works are solely in the form of machine-executable object code generated by
47 // a source language processor.
48 //
49 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
50 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
51 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
52 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
53 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
54 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
55 // DEALINGS IN THE SOFTWARE.
56 //
57 // *****************************************************************************
58 
59 // Boost includes
60 #include <boost/thread.hpp>
61 #include <boost/thread/condition.hpp>
62 #include <boost/algorithm/string/join.hpp>
63 #include <boost/asio.hpp>
64 #include <boost/bind.hpp>
65 #include <boost/function.hpp>
66 #include <boost/system/error_code.hpp>
67 #include <boost/date_time/posix_time/posix_time.hpp>
68 
69 // ROSaic includes
71 
72 #ifndef ASYNC_MANAGER_HPP
73 #define ASYNC_MANAGER_HPP
74 
83 namespace io_comm_rx
84 {
85 
90  class Manager {
91  public:
92  typedef boost::function<void(const uint8_t*, std::size_t&)> Callback;
93  virtual ~Manager() {}
95  virtual void setCallback(const Callback& callback) = 0;
97  virtual bool send(std::string cmd, std::size_t size) = 0;
99  virtual void wait(uint16_t* count) = 0;
101  virtual bool isOpen() const = 0;
102  };
103 
104 
111  template <typename StreamT>
112  class AsyncManager : public Manager
113  {
114  public:
120  AsyncManager(boost::shared_ptr<StreamT> stream, boost::shared_ptr<boost::asio::io_service> io_service, std::size_t buffer_size = 8192);
121  virtual ~AsyncManager();
122 
123  void setCallback(const Callback& callback) { read_callback_ = callback; }
124 
125  void wait(uint16_t* count);
126 
132  bool send(std::string cmd, std::size_t size);
133 
134  bool isOpen() const { return stream_->is_open(); }
135 
136  protected:
137 
139  void read();
140 
142  void asyncReadSomeHandler(const boost::system::error_code& error, std::size_t bytes_transferred);
143 
145  void write(std::string cmd, std::size_t size);
146 
148  void close();
149 
151  void tryParsing();
152 
154  boost::mutex parse_mutex_;
155 
158 
161 
163  boost::condition_variable parsing_condition_;
164 
166  boost::shared_ptr<StreamT> stream_;
167 
169  boost::shared_ptr<boost::asio::io_service> io_service_;
170 
172  std::vector<uint8_t> in_;
173 
176 
178  uint8_t * to_be_parsed_;
179 
181  boost::shared_ptr<boost::thread> async_background_thread_;
182 
185 
187  bool stopping_;
188 
190  const std::size_t buffer_size_;
191 
193  boost::asio::deadline_timer timer_;
194 
196  const uint16_t count_max_;
197 
199  void callAsyncWait(uint16_t* count);
200 
202  uint16_t do_read_count_;
203  };
204 
205 
206  template <typename StreamT>
208  {
209  uint8_t * to_be_parsed;
210  to_be_parsed = new uint8_t[buffer_size_];
211  to_be_parsed_ = to_be_parsed;
212  bool timed_out = false;
213  std::size_t shift_bytes = 0;
214  std::size_t arg_for_read_callback = 0;
215 
216  while(!timed_out) // Loop will stop if condition variable timed out
217  {
218  boost::mutex::scoped_lock lock(parse_mutex_);
219  parsing_condition_.wait_for(lock, boost::chrono::seconds(10), [this](){return try_parsing_;});
220  bool timed_out = !try_parsing_;
221  if (timed_out) break;
222  try_parsing_ = false;
223  allow_writing_ = true;
224  std::size_t current_buffer_size = circular_buffer_.size();
225  arg_for_read_callback += current_buffer_size;
226  circular_buffer_.read(to_be_parsed + shift_bytes, current_buffer_size);
227 
228  lock.unlock();
229  parsing_condition_.notify_one();
230 
231  try
232  {
233  ROS_DEBUG("Calling read_callback_() method, with number of bytes to be parsed being %li",
234  arg_for_read_callback);
235  read_callback_(to_be_parsed_, arg_for_read_callback);
236  }
237  catch (std::size_t& parsing_failed_here)
238  {
239  to_be_parsed_ = to_be_parsed + parsing_failed_here;
240  ROS_DEBUG("Current buffer size is %li and parsing_failed_here is %li", current_buffer_size,
241  parsing_failed_here);
242  arg_for_read_callback = arg_for_read_callback - parsing_failed_here;
243  if (arg_for_read_callback < 0) // In case some parsing error was not caught, which should never happen..
244  {
245  delete [] to_be_parsed; // Freeing memory
246  to_be_parsed = new uint8_t[buffer_size_];
247  to_be_parsed_ = to_be_parsed;
248  shift_bytes = 0;
249  arg_for_read_callback = 0;
250  continue;
251  }
252  shift_bytes += current_buffer_size;
253  continue;
254  }
255  delete [] to_be_parsed; // Freeing memory
256  to_be_parsed = new uint8_t[buffer_size_];
257  to_be_parsed_ = to_be_parsed;
258  shift_bytes = 0;
259  arg_for_read_callback = 0;
260  }
261  ROS_INFO("TryParsing() method finished since it did not receive anything to parse for 10 seconds..");
262  }
263 
264 
265  template <typename StreamT>
266  bool AsyncManager<StreamT>::send(std::string cmd, std::size_t size)
267  {
268  if(size == 0)
269  {
270  ROS_ERROR("Message size to be sent to the Rx would be 0");
271  return true;
272  }
273 
274  std::vector<uint8_t> vector_temp(cmd.begin(), cmd.end());
275  uint8_t *p = &vector_temp[0];
276 
277  io_service_->post(boost::bind(&AsyncManager<StreamT>::write, this, cmd, size));
278  return true;
279  }
280 
281  template <typename StreamT>
282  void AsyncManager<StreamT>::write(std::string cmd, std::size_t size)
283  {
284  boost::asio::write(*stream_, boost::asio::buffer(cmd.data(), size));
285  // Prints the data that was sent
286  ROS_DEBUG("Sent the following %li bytes to the Rx: \n%s", size, cmd.c_str());
287  }
288 
289  template <typename StreamT>
291  {
292  timer_.async_wait(boost::bind(&AsyncManager::wait, this, count));
293  }
294 
295  template <typename StreamT>
296  AsyncManager<StreamT>::AsyncManager(boost::shared_ptr<StreamT> stream,
297  boost::shared_ptr<boost::asio::io_service> io_service,
298  std::size_t buffer_size) : timer_(*(io_service.get()), boost::posix_time::seconds(1)),
299  stopping_(false), try_parsing_(false), allow_writing_(true), do_read_count_(0), buffer_size_(buffer_size),
300  count_max_(6), circular_buffer_(buffer_size)
301  // Since buffer_size = 8912 in declaration, no need in definition any more (even yields error message,
302  // since "overwrite").
303  {
304  ROS_DEBUG("Setting the private stream variable of the AsyncManager instance.");
305  stream_ = stream;
306  io_service_ = io_service;
307  in_.resize(buffer_size_);
308 
309  io_service_->post(boost::bind(&AsyncManager<StreamT>::read, this));
310  // This function is used to ask the io_service to execute the given handler, but without allowing the io_service
311  // to call the handler from inside this function. The function signature of the handler must be: void handler();
312  // The io_service guarantees that the handler (given as parameter) will only be called in a thread in which the
313  // run(), run_one(), poll() or poll_one() member functions is currently being invoked. So the fundamental
314  // difference is that dispatch will execute the work right away if it can and queue it otherwise while post queues the work no matter what.
315  async_background_thread_.reset(new boost::thread(boost::bind(&boost::asio::io_service::run, io_service_)));
316  // Note that io_service_ is already pointer, hence need dereferencing operator & (ampersand). If the value of the
317  // pointer for the current thread is changed using reset(), then the previous value is destroyed by calling the
318  // cleanup routine. Alternatively, the stored value can be reset to NULL and the prior value returned by calling
319  // the release() member function, allowing the application to take back responsibility for destroying the object.
320  uint16_t count = 0;
321  boost::thread(boost::bind(&AsyncManager::callAsyncWait, this, &count));
322 
323  ROS_DEBUG("Launching tryParsing() thread..");
324  boost::thread(boost::bind(&AsyncManager::tryParsing, this));
325  } // Calls std::terminate() on thread just created
326 
327  template <typename StreamT>
329  {
330  async_background_thread_->join();
331  }
332 
333  template <typename StreamT>
335  {
336  stream_->async_read_some(
337  boost::asio::buffer(in_.data(),
338  in_.size()),
340  boost::asio::placeholders::error,
341  boost::asio::placeholders::bytes_transferred));
342  // The handler is async_read_some_handler, whose call is postponed to
343  // when async_read_some completes.
344  if (do_read_count_ < 5) ++do_read_count_;
345  }
346 
347  template <typename StreamT>
348  void AsyncManager<StreamT>::asyncReadSomeHandler(const boost::system::error_code& error,
349  std::size_t bytes_transferred)
350  {
351  if (error)
352  {
353  ROS_ERROR("Rx ASIO input buffer read error: %s, %li", error.message().c_str(), bytes_transferred);
354  }
355  else if (bytes_transferred > 0)
356  {
357  if (read_callback_) //Will be false in InitializeSerial (first call) since read_callback_ not added yet..
358  {
359  boost::mutex::scoped_lock lock(parse_mutex_);
360  parsing_condition_.wait(lock, [this](){return allow_writing_;});
361  circular_buffer_.write(in_.data(), bytes_transferred);
362  allow_writing_ = false;
363  try_parsing_ = true;
364  lock.unlock();
365  parsing_condition_.notify_one();
366  std::vector<uint8_t> empty;
367  in_ = empty;
368  in_.resize(buffer_size_);
369  }
370  }
371 
372  if (!stopping_)
373  io_service_->post(boost::bind(&AsyncManager<StreamT>::read, this));
374  }
375 
376  template <typename StreamT>
378  {
379  stopping_ = true;
380  boost::system::error_code error;
381  stream_->close(error);
382  if(error)
383  {
384  ROS_ERROR_STREAM("Error while closing the AsyncManager: " << error.message().c_str());
385  }
386  }
387 
388  template <typename StreamT>
389  void AsyncManager<StreamT>::wait(uint16_t* count)
390  {
391  if (*count < count_max_)
392  {
393  ++(*count);
394  timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
395  if (!(*count == count_max_))
396  {
397  timer_.async_wait(boost::bind(&AsyncManager::wait, this, count));
398  }
399  }
400  if ((*count == count_max_) && (do_read_count_ < 3))
401  // Why 3? Even if there are no incoming messages, read() is called once.
402  // It will be called a second time in TCP/IP mode since (just example) "IP10<" is transmitted.
403  {
404  ROS_INFO("No incoming messages, driver stopped, ros::spin() will spin forever unless you hit Ctrl+C.");
405  async_background_thread_->interrupt();
406  }
407  }
408 }
409 
410 #endif // for ASYNC_MANAGER_HPP
411 
412 
413 
bool isOpen() const
Determines whether or not the connection is open.
This is the central interface between ROSaic and the Rx(s), managing I/O operations such as reading m...
void read()
Reads in via async_read_some and hands certain number of bytes (bytes_transferred) over to async_read...
uint16_t do_read_count_
Number of times the DoRead() method has been called (only counts initially)
boost::asio::deadline_timer timer_
Boost timer for throwing ROS_INFO message once timed out due to lack of incoming messages.
const uint16_t count_max_
Number of seconds before ROS_INFO message is thrown (if no incoming message)
virtual void wait(uint16_t *count)=0
Waits count seconds before throwing ROS_INFO message in case no message from the receiver arrived...
void asyncReadSomeHandler(const boost::system::error_code &error, std::size_t bytes_transferred)
Handler for async_read_some (Boost library)..
const std::size_t buffer_size_
Size of in_ buffers.
virtual bool send(std::string cmd, std::size_t size)=0
Sends commands to the receiver.
std::size_t write(const uint8_t *data, std::size_t bytes)
Returns number of bytes written.
CircularBuffer circular_buffer_
Circular buffer to avoid unsuccessful SBF/NMEA parsing due to incomplete messages.
uint8_t * to_be_parsed_
Memory location where read_callback_ will start reading unless part of SBF/NMEA had to be appended be...
void close()
Closes stream "stream_".
boost::shared_ptr< boost::asio::io_service > io_service_
io_context object
std::vector< uint8_t > in_
Buffer for async_read_some() to read continuous SBF/NMEA stream.
bool send(std::string cmd, std::size_t size)
Sends commands via the I/O stream.
Class for creating, writing to and reading from a circular buffer.
void write(std::string cmd, std::size_t size)
Sends command "cmd" to the Rx.
boost::shared_ptr< boost::thread > async_background_thread_
New thread for receiving incoming messages.
Interface (in C++ terms), that could be used for any I/O manager, synchronous and asynchronous alike...
boost::shared_ptr< StreamT > stream_
Stream, represents either serial or TCP/IP connection.
boost::condition_variable parsing_condition_
Condition variable complementing "parse_mutex".
void callAsyncWait(uint16_t *count)
Handles the ROS_INFO throwing (if no incoming message)
void wait(uint16_t *count)
Waits count seconds before throwing ROS_INFO message in case no message from the receiver arrived...
Callback read_callback_
Callback to be called once message arrives.
boost::function< void(const uint8_t *, std::size_t &)> Callback
bool allow_writing_
Determines when the asyncReadSomeHandler() method should write SBF/NMEA into the circular buffer...
boost::mutex parse_mutex_
Mutex to control changes of class variable "try_parsing".
bool try_parsing_
Determines when the tryParsing() method will attempt parsing SBF/NMEA.
Declares a class for creating, writing to and reading from a circular bufffer.
AsyncManager(boost::shared_ptr< StreamT > stream, boost::shared_ptr< boost::asio::io_service > io_service, std::size_t buffer_size=8192)
Class constructor.
void tryParsing()
Tries parsing SBF/NMEA whenever the boolean class variable "try_parsing" is true. ...
virtual void setCallback(const Callback &callback)=0
Sets the callback function.
virtual bool isOpen() const =0
Determines whether or not the connection is open.
bool stopping_
Whether or not we want to sever the connection to the Rx.
void setCallback(const Callback &callback)
Sets the callback function.