Program Listing for File callback.hpp
↰ Return to documentation for file (/tmp/ws/src/ublox/ublox_gps/include/ublox_gps/callback.hpp
)
//==============================================================================
// 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