.. _program_listing_file__tmp_ws_src_ublox_ublox_gps_include_ublox_gps_callback.hpp:

Program Listing for File callback.hpp
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_ublox_ublox_gps_include_ublox_gps_callback.hpp>` (``/tmp/ws/src/ublox/ublox_gps/include/ublox_gps/callback.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   //==============================================================================
   // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
   // All rights reserved.
   
   // Redistribution and use in source and binary forms, with or without
   // modification, are permitted provided that the following conditions are met:
   //     * Redistributions of source code must retain the above copyright
   //       notice, this list of conditions and the following disclaimer.
   //     * 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.
   //     * Neither the name of the Flight Systems and Automatic Control group,
   //       TU Darmstadt, 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 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.
   //==============================================================================
   
   #ifndef UBLOX_GPS_CALLBACK_HPP
   #define UBLOX_GPS_CALLBACK_HPP
   
   #include <chrono>
   #include <condition_variable>
   #include <functional>
   #include <ios>
   #include <map>
   #include <memory>
   #include <mutex>
   #include <sstream>
   #include <stdexcept>
   #include <string>
   #include <utility>
   
   #include <ublox_msgs/serialization.hpp>
   
   namespace ublox_gps {
   
   class CallbackHandler {
    public:
     virtual void handle(ublox::Reader& reader) = 0;
   
     bool wait(const std::chrono::milliseconds& timeout) {
       std::unique_lock<std::mutex> lock(mutex_);
       return condition_.wait_for(lock, timeout) == std::cv_status::no_timeout;
     }
   
    protected:
     std::mutex mutex_; 
     std::condition_variable condition_; 
   };
   
   template <typename T>
   class CallbackHandler_ final : public CallbackHandler {
    public:
     using Callback = std::function<void(const T&)>;
   
     explicit CallbackHandler_(const Callback& func = Callback(), int debug = 1) : func_(func), debug_(debug) {}
   
     virtual const T& get() { return message_; }
   
     void handle(ublox::Reader& reader) override {
       std::lock_guard<std::mutex> lock(mutex_);
       try {
         if (!reader.read<T>(message_)) {
           // RCLCPP_DEBUG_COND(debug_ >= 2,
           //                "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)",
           //                static_cast<unsigned int>(reader.classId()),
           //                static_cast<unsigned int>(reader.messageId()),
           //                reader.length());
           condition_.notify_all();
           return;
         }
       } catch (const std::runtime_error& e) {
         // RCLCPP_DEBUG_COND(debug_ >= 2,
         //                "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)",
         //                static_cast<unsigned int>(reader.classId()),
         //                static_cast<unsigned int>(reader.messageId()),
         //                reader.length());
         condition_.notify_all();
         return;
       }
   
       if (func_) {
         func_(message_);
       }
       condition_.notify_all();
     }
   
    private:
     Callback func_; 
     T message_; 
     int debug_;
   };
   
   class CallbackHandlers final {
    public:
     explicit CallbackHandlers(int debug) : debug_(debug) {}
   
     template <typename T>
     void insert(typename CallbackHandler_<T>::Callback callback) {
       std::lock_guard<std::mutex> lock(callback_mutex_);
       callbacks_.insert(
         std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID),
                        std::make_shared<CallbackHandler_<T>>(callback, debug_)));
     }
   
     template <typename T>
     void insert(
         typename CallbackHandler_<T>::Callback callback,
         unsigned int message_id) {
       std::lock_guard<std::mutex> lock(callback_mutex_);
       callbacks_.insert(
         std::make_pair(std::make_pair(T::CLASS_ID, message_id),
                        std::make_shared<CallbackHandler_<T>>(callback, debug_)));
     }
   
     void set_nmea_callback(std::function<void(const std::string&)> callback) {
       std::lock_guard<std::mutex> lock(callback_mutex_);
       callback_nmea_ = callback;
     }
   
     void handle(ublox::Reader& reader) {
       // Find the callback handlers for the message & decode it
       std::lock_guard<std::mutex> lock(callback_mutex_);
       Callbacks::key_type key =
           std::make_pair(reader.classId(), reader.messageId());
       for (Callbacks::iterator callback = callbacks_.lower_bound(key);
            callback != callbacks_.upper_bound(key); ++callback) {
         callback->second->handle(reader);
       }
     }
   
     void handle_nmea(ublox::Reader& reader) {
       std::lock_guard<std::mutex> lock(callback_mutex_);
       if (callback_nmea_ == nullptr) {
         return;
       }
   
       const std::string buffer = reader.getExtraData();
       size_t nmea_start = buffer.find('$', 0);
       size_t nmea_end = buffer.find('\n', nmea_start);
       while(nmea_start != std::string::npos && nmea_end != std::string::npos) {
         std::string sentence = buffer.substr(nmea_start, nmea_end - nmea_start + 1);
         callback_nmea_(sentence);
   
         nmea_start = buffer.find('$', nmea_end + 1);
         nmea_end = buffer.find('\n', nmea_start);
       }
     }
   
     template <typename T>
     bool read(T& message, const std::chrono::milliseconds& timeout) {
       bool result = false;
       // Create a callback handler for this message
       callback_mutex_.lock();
       auto handler = std::make_shared<CallbackHandler_<T>>(typename CallbackHandler_<T>::Callback(), debug_);
       Callbacks::iterator callback = callbacks_.insert(
         (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID),
                         handler)));
       callback_mutex_.unlock();
   
       // Wait for the message
       if (handler->wait(timeout)) {
         message = handler->get();
         result = true;
       }
   
       // Remove the callback handler
       callback_mutex_.lock();
       callbacks_.erase(callback);
       callback_mutex_.unlock();
       return result;
     }
   
     size_t readCallback(unsigned char* data, std::size_t size) {
       ublox::Reader reader(data, size);
       // Read all U-Blox messages in buffer
       while (reader.search() != reader.end() && reader.found()) {
         if (debug_ >= 3) {
           // Print the received bytes
           std::ostringstream oss;
           for (ublox::Reader::iterator it = reader.pos();
                it != reader.pos() + reader.length() + 8; ++it) {
             oss << std::hex << static_cast<unsigned int>(*it) << " ";
           }
           // RCLCPP_DEBUG(logger_, "U-blox: reading %d bytes\n%s", reader.length() + 8,
           //           oss.str().c_str());
         }
   
         handle(reader);
       }
       handle_nmea(reader);
   
       // delete read bytes from ASIO input buffer
       std::copy(reader.pos(), reader.end(), data);
   
       return reader.pos() - data;
     }
   
    private:
     using Callbacks = std::multimap<std::pair<uint8_t, uint8_t>,
                                     std::shared_ptr<CallbackHandler>>;
   
     // Call back handlers for u-blox messages
     Callbacks callbacks_;
     std::mutex callback_mutex_;
     int debug_;
   
     std::function<void(const std::string &)> callback_nmea_{nullptr};
   };
   
   }  // namespace ublox_gps
   
   #endif  // UBLOX_GPS_CALLBACK_HPP