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> 72 #ifndef ASYNC_MANAGER_HPP 73 #define ASYNC_MANAGER_HPP 92 typedef boost::function<void(const uint8_t*, std::size_t&)>
Callback;
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;
111 template <
typename StreamT>
120 AsyncManager(boost::shared_ptr<StreamT> stream, boost::shared_ptr<boost::asio::io_service> io_service, std::size_t buffer_size = 8192);
125 void wait(uint16_t* count);
132 bool send(std::string cmd, std::size_t size);
134 bool isOpen()
const {
return stream_->is_open(); }
142 void asyncReadSomeHandler(
const boost::system::error_code& error, std::size_t bytes_transferred);
145 void write(std::string cmd, std::size_t size);
199 void callAsyncWait(uint16_t* count);
206 template <
typename StreamT>
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;
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);
229 parsing_condition_.notify_one();
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);
237 catch (std::size_t& parsing_failed_here)
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)
245 delete [] to_be_parsed;
246 to_be_parsed =
new uint8_t[buffer_size_];
247 to_be_parsed_ = to_be_parsed;
249 arg_for_read_callback = 0;
252 shift_bytes += current_buffer_size;
255 delete [] to_be_parsed;
256 to_be_parsed =
new uint8_t[buffer_size_];
257 to_be_parsed_ = to_be_parsed;
259 arg_for_read_callback = 0;
261 ROS_INFO(
"TryParsing() method finished since it did not receive anything to parse for 10 seconds..");
265 template <
typename StreamT>
270 ROS_ERROR(
"Message size to be sent to the Rx would be 0");
274 std::vector<uint8_t> vector_temp(cmd.begin(), cmd.end());
275 uint8_t *p = &vector_temp[0];
281 template <
typename StreamT>
284 boost::asio::write(*stream_, boost::asio::buffer(cmd.data(), size));
286 ROS_DEBUG(
"Sent the following %li bytes to the Rx: \n%s", size, cmd.c_str());
289 template <
typename StreamT>
295 template <
typename StreamT>
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)
304 ROS_DEBUG(
"Setting the private stream variable of the AsyncManager instance.");
323 ROS_DEBUG(
"Launching tryParsing() thread..");
327 template <
typename StreamT>
333 template <
typename StreamT>
337 boost::asio::buffer(
in_.data(),
340 boost::asio::placeholders::error,
341 boost::asio::placeholders::bytes_transferred));
347 template <
typename StreamT>
349 std::size_t bytes_transferred)
353 ROS_ERROR(
"Rx ASIO input buffer read error: %s, %li", error.message().c_str(), bytes_transferred);
355 else if (bytes_transferred > 0)
366 std::vector<uint8_t> empty;
376 template <
typename StreamT>
380 boost::system::error_code error;
384 ROS_ERROR_STREAM(
"Error while closing the AsyncManager: " << error.message().c_str());
388 template <
typename StreamT>
394 timer_.expires_at(
timer_.expires_at() + boost::posix_time::seconds(1));
404 ROS_INFO(
"No incoming messages, driver stopped, ros::spin() will spin forever unless you hit Ctrl+C.");
410 #endif // for ASYNC_MANAGER_HPP 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.