async_manager.hpp

Go to the documentation of this file.

// *****************************************************************************
//
// © Copyright 2020, Septentrio NV/SA.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//    1. Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//    2. Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//    3. Neither the name of the copyright holder nor the names of its
//       contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************
 
// *****************************************************************************
//
// Boost Software License - Version 1.0 - August 17th, 2003
//
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
 
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// *****************************************************************************
 
// Boost includes
#include <boost/algorithm/string/join.hpp>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/function.hpp>
#include <boost/system/error_code.hpp>
#include <boost/thread.hpp>
#include <boost/thread/condition.hpp>
 
// ROSaic includes
#include <septentrio_gnss_driver/communication/circular_buffer.hpp>
 
#ifndef ASYNC_MANAGER_HPP
#define ASYNC_MANAGER_HPP
 
namespace io_comm_rx {
 
    class Manager
    {
    public:
        typedef boost::function<void(Timestamp, const uint8_t*, std::size_t&)>
            Callback;
        virtual ~Manager() {}
        virtual void setCallback(const Callback& callback) = 0;
        virtual bool send(const std::string& cmd) = 0;
        virtual void wait(uint16_t* count) = 0;
        virtual bool isOpen() const = 0;
    };
 
    template <typename StreamT>
    class AsyncManager : public Manager
    {
    public:
        AsyncManager(ROSaicNodeBase* node, boost::shared_ptr<StreamT> stream,
                     boost::shared_ptr<boost::asio::io_service> io_service,
                     std::size_t buffer_size = 16384);
        virtual ~AsyncManager();
 
        void setCallback(const Callback& callback) { read_callback_ = callback; }
 
        void wait(uint16_t* count);
 
        bool send(const std::string& cmd);
 
        bool isOpen() const { return stream_->is_open(); }
 
    private:
        ROSaicNodeBase* node_;
 
    protected:
        void read();
 
        void asyncReadSomeHandler(const boost::system::error_code& error,
                                  std::size_t bytes_transferred);
 
        void write(const std::string& cmd);
 
        void close();
 
        void tryParsing();
 
        boost::mutex parse_mutex_;
 
        bool try_parsing_;
 
        bool allow_writing_;
 
        boost::condition_variable parsing_condition_;
 
        boost::shared_ptr<StreamT> stream_;
 
        boost::shared_ptr<boost::asio::io_service> io_service_;
 
        std::vector<uint8_t> in_;
 
        CircularBuffer circular_buffer_;
 
        boost::shared_ptr<boost::thread> async_background_thread_;
 
        boost::shared_ptr<boost::thread> parsing_thread_;
 
        boost::shared_ptr<boost::thread> waiting_thread_;
 
        Callback read_callback_;
 
        bool stopping_;
 
        const std::size_t buffer_size_;
 
        boost::asio::deadline_timer timer_;
 
        const uint16_t count_max_;
 
        void callAsyncWait(uint16_t* count);
 
        uint16_t do_read_count_;
 
        Timestamp recvTime_;
    };
 
    template <typename StreamT>
    void AsyncManager<StreamT>::tryParsing()
    {
        uint8_t* to_be_parsed = new uint8_t[buffer_size_ * 16];
        uint8_t* to_be_parsed_index = to_be_parsed;
        bool timed_out = false;
        std::size_t shift_bytes = 0;
        std::size_t arg_for_read_callback = 0;
 
        while (!timed_out &&
               !stopping_) // Loop will stop if condition variable timed out
        {
            boost::mutex::scoped_lock lock(parse_mutex_);
            parsing_condition_.wait_for(lock, boost::chrono::seconds(10),
                                        [this]() { return try_parsing_; });
            bool timed_out = !try_parsing_;
            if (timed_out)
                break;
            try_parsing_ = false;
            allow_writing_ = true;
            std::size_t current_buffer_size = circular_buffer_.size();
            arg_for_read_callback += current_buffer_size;
            circular_buffer_.read(to_be_parsed + shift_bytes, current_buffer_size);
            Timestamp revcTime = recvTime_;
            lock.unlock();
            parsing_condition_.notify_one();
 
            try
            {
                node_->log(
                    LogLevel::DEBUG,
                    "Calling read_callback_() method, with number of bytes to be parsed being " +
                        std::to_string(arg_for_read_callback));
                read_callback_(revcTime, to_be_parsed_index, arg_for_read_callback);
            } catch (std::size_t& parsing_failed_here)
            {
                to_be_parsed_index += parsing_failed_here;
                arg_for_read_callback -= parsing_failed_here;
                node_->log(LogLevel::DEBUG, "Current buffer size is " +
                                                std::to_string(current_buffer_size) +
                                                " and parsing_failed_here is " +
                                                std::to_string(parsing_failed_here));
                if (arg_for_read_callback < 0) // In case some parsing error was not
                                               // caught, which should never happen..
                {
                    to_be_parsed_index = to_be_parsed;
                    shift_bytes = 0;
                    arg_for_read_callback = 0;
                    continue;
                }
                shift_bytes += current_buffer_size;
                continue;
            }
            to_be_parsed_index = to_be_parsed;
            shift_bytes = 0;
            arg_for_read_callback = 0;
        }
        node_->log(
            LogLevel::INFO,
            "TryParsing() method finished since it did not receive anything to parse for 10 seconds..");
        delete[] to_be_parsed; // Freeing memory
    }
 
    template <typename StreamT>
    bool AsyncManager<StreamT>::send(const std::string& cmd)
    {
        if (cmd.size() == 0)
        {
            node_->log(LogLevel::ERROR,
                       "Message size to be sent to the Rx would be 0");
            return true;
        }
 
        io_service_->post(boost::bind(&AsyncManager<StreamT>::write, this, cmd));
        return true;
    }
 
    template <typename StreamT>
    void AsyncManager<StreamT>::write(const std::string& cmd)
    {
        boost::asio::write(*stream_, boost::asio::buffer(cmd.data(), cmd.size()));
        // Prints the data that was sent
        node_->log(LogLevel::DEBUG, "Sent the following " +
                                        std::to_string(cmd.size()) +
                                        " bytes to the Rx: \n" + cmd);
    }
 
    template <typename StreamT>
    void AsyncManager<StreamT>::callAsyncWait(uint16_t* count)
    {
        timer_.async_wait(boost::bind(&AsyncManager::wait, this, count));
    }
 
    template <typename StreamT>
    AsyncManager<StreamT>::AsyncManager(
        ROSaicNodeBase* node, boost::shared_ptr<StreamT> stream,
        boost::shared_ptr<boost::asio::io_service> io_service,
        std::size_t buffer_size) :
        node_(node),
        timer_(*(io_service.get()), boost::posix_time::seconds(1)), stopping_(false),
        try_parsing_(false), allow_writing_(true), do_read_count_(0),
        buffer_size_(buffer_size), count_max_(6), circular_buffer_(node, buffer_size)
    // Since buffer_size = 16384 in declaration, no need in definition anymore (even
    // yields error message, due to "overwrite").
    {
        node_->log(
            LogLevel::DEBUG,
            "Setting the private stream variable of the AsyncManager instance.");
        stream_ = stream;
        io_service_ = io_service;
        in_.resize(buffer_size_);
 
        io_service_->post(boost::bind(&AsyncManager<StreamT>::read, this));
        // This function is used to ask the io_service to execute the given handler,
        // but without allowing the io_service to call the handler from inside this
        // function. The function signature of the handler must be: void handler();
        // The io_service guarantees that the handler (given as parameter) will only
        // be called in a thread in which the run(), run_one(), poll() or poll_one()
        // member functions is currently being invoked. So the fundamental 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.
        async_background_thread_.reset(new boost::thread(
            boost::bind(&boost::asio::io_service::run, io_service_)));
        // Note that io_service_ is already pointer, hence need dereferencing
        // operator & (ampersand). If the value of the pointer for the current thread
        // is changed using reset(), then the previous value is destroyed by calling
        // the cleanup routine. Alternatively, the stored value can be reset to NULL
        // and the prior value returned by calling the release() member function,
        // allowing the application to take back responsibility for destroying the
        // object.
        uint16_t count = 0;
        waiting_thread_.reset(new boost::thread(
            boost::bind(&AsyncManager::callAsyncWait, this, &count)));
 
        node_->log(LogLevel::DEBUG, "Launching tryParsing() thread..");
        parsing_thread_.reset(
            new boost::thread(boost::bind(&AsyncManager::tryParsing, this)));
    } // Calls std::terminate() on thread just created
 
    template <typename StreamT>
    AsyncManager<StreamT>::~AsyncManager()
    {
        close();
        io_service_->stop();
        try_parsing_ = true;
        parsing_condition_.notify_one();
        parsing_thread_->join();
        waiting_thread_->join();
        async_background_thread_->join();
    }
 
    template <typename StreamT>
    void AsyncManager<StreamT>::read()
    {
        stream_->async_read_some(
            boost::asio::buffer(in_.data(), in_.size()),
            boost::bind(&AsyncManager<StreamT>::asyncReadSomeHandler, this,
                        boost::asio::placeholders::error,
                        boost::asio::placeholders::bytes_transferred));
        // The handler is async_read_some_handler, whose call is postponed to
        // when async_read_some completes.
        if (do_read_count_ < 5)
            ++do_read_count_;
    }
 
    template <typename StreamT>
    void AsyncManager<StreamT>::asyncReadSomeHandler(
        const boost::system::error_code& error, std::size_t bytes_transferred)
    {
        if (error)
        {
            node_->log(LogLevel::ERROR,
                       "Rx ASIO input buffer read error: " + error.message() + ", " +
                           std::to_string(bytes_transferred));
        } else if (bytes_transferred > 0)
        {
            Timestamp inTime = node_->getTime();
            if (read_callback_ &&
                !stopping_) // Will be false in InitializeSerial (first call)
                            // since read_callback_ not added yet..
            {
                boost::mutex::scoped_lock lock(parse_mutex_);
                parsing_condition_.wait(lock, [this]() { return allow_writing_; });
                circular_buffer_.write(in_.data(), bytes_transferred);
                allow_writing_ = false;
                try_parsing_ = true;
                recvTime_ = inTime;
                lock.unlock();
                parsing_condition_.notify_one();
            }
        }
 
        if (!stopping_)
            io_service_->post(boost::bind(&AsyncManager<StreamT>::read, this));
    }
 
    template <typename StreamT>
    void AsyncManager<StreamT>::close()
    {
        stopping_ = true;
        boost::system::error_code error;
        stream_->close(error);
        if (error)
        {
            node_->log(LogLevel::ERROR,
                       "Error while closing the AsyncManager: " + error.message());
        }
    }
 
    template <typename StreamT>
    void AsyncManager<StreamT>::wait(uint16_t* count)
    {
        if (*count < count_max_)
        {
            ++(*count);
            timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
            if (!(*count == count_max_))
            {
                timer_.async_wait(boost::bind(&AsyncManager::wait, this, count));
            }
        }
        if ((*count == count_max_) && (do_read_count_ < 3))
        // Why 3? Even if there are no incoming messages, read() is called once.
        // It will be called a second time in TCP/IP mode since (just example)
        // "IP10<" is transmitted.
        {
            node_->log(
                LogLevel::INFO,
                "No incoming messages, driver stopped, ros::spin() will spin forever unless you hit Ctrl+C.");
            async_background_thread_->interrupt();
        }
    }
} // namespace io_comm_rx
 
#endif // for ASYNC_MANAGER_HPP