Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef UBLOX_GPS_CALLBACK_H
00030 #define UBLOX_GPS_CALLBACK_H
00031
00032 #include <ros/console.h>
00033 #include <ublox/serialization/ublox_msgs.h>
00034 #include <boost/format.hpp>
00035 #include <boost/function.hpp>
00036 #include <boost/thread.hpp>
00037
00038 namespace ublox_gps {
00039
00043 class CallbackHandler {
00044 public:
00048 virtual void handle(ublox::Reader& reader) = 0;
00049
00053 bool wait(const boost::posix_time::time_duration& timeout) {
00054 boost::mutex::scoped_lock lock(mutex_);
00055 return condition_.timed_wait(lock, timeout);
00056 }
00057
00058 protected:
00059 boost::mutex mutex_;
00060 boost::condition_variable condition_;
00061 };
00062
00067 template <typename T>
00068 class CallbackHandler_ : public CallbackHandler {
00069 public:
00070 typedef boost::function<void(const T&)> Callback;
00071
00076 CallbackHandler_(const Callback& func = Callback()) : func_(func) {}
00077
00081 virtual const T& get() { return message_; }
00082
00087 void handle(ublox::Reader& reader) {
00088 boost::mutex::scoped_lock lock(mutex_);
00089 try {
00090 if (!reader.read<T>(message_)) {
00091 ROS_DEBUG_COND(debug >= 2,
00092 "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)",
00093 static_cast<unsigned int>(reader.classId()),
00094 static_cast<unsigned int>(reader.messageId()),
00095 reader.length());
00096 condition_.notify_all();
00097 return;
00098 }
00099 } catch (std::runtime_error& e) {
00100 ROS_DEBUG_COND(debug >= 2,
00101 "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)",
00102 static_cast<unsigned int>(reader.classId()),
00103 static_cast<unsigned int>(reader.messageId()),
00104 reader.length());
00105 condition_.notify_all();
00106 return;
00107 }
00108
00109 if (func_) func_(message_);
00110 condition_.notify_all();
00111 }
00112
00113 private:
00114 Callback func_;
00115 T message_;
00116 };
00117
00121 class CallbackHandlers {
00122 public:
00128 template <typename T>
00129 void insert(typename CallbackHandler_<T>::Callback callback) {
00130 boost::mutex::scoped_lock lock(callback_mutex_);
00131 CallbackHandler_<T>* handler = new CallbackHandler_<T>(callback);
00132 callbacks_.insert(
00133 std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID),
00134 boost::shared_ptr<CallbackHandler>(handler)));
00135 }
00136
00145 template <typename T>
00146 void insert(
00147 typename CallbackHandler_<T>::Callback callback,
00148 unsigned int message_id) {
00149 boost::mutex::scoped_lock lock(callback_mutex_);
00150 CallbackHandler_<T>* handler = new CallbackHandler_<T>(callback);
00151 callbacks_.insert(
00152 std::make_pair(std::make_pair(T::CLASS_ID, message_id),
00153 boost::shared_ptr<CallbackHandler>(handler)));
00154 }
00155
00160 void handle(ublox::Reader& reader) {
00161
00162 boost::mutex::scoped_lock lock(callback_mutex_);
00163 Callbacks::key_type key =
00164 std::make_pair(reader.classId(), reader.messageId());
00165 for (Callbacks::iterator callback = callbacks_.lower_bound(key);
00166 callback != callbacks_.upper_bound(key); ++callback)
00167 callback->second->handle(reader);
00168 }
00169
00175 template <typename T>
00176 bool read(T& message, const boost::posix_time::time_duration& timeout) {
00177 bool result = false;
00178
00179 callback_mutex_.lock();
00180 CallbackHandler_<T>* handler = new CallbackHandler_<T>();
00181 Callbacks::iterator callback = callbacks_.insert(
00182 (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID),
00183 boost::shared_ptr<CallbackHandler>(handler))));
00184 callback_mutex_.unlock();
00185
00186
00187 if (handler->wait(timeout)) {
00188 message = handler->get();
00189 result = true;
00190 }
00191
00192
00193 callback_mutex_.lock();
00194 callbacks_.erase(callback);
00195 callback_mutex_.unlock();
00196 return result;
00197 }
00198
00205 void readCallback(unsigned char* data, std::size_t& size) {
00206 ublox::Reader reader(data, size);
00207
00208 while (reader.search() != reader.end() && reader.found()) {
00209 if (debug >= 3) {
00210
00211 std::ostringstream oss;
00212 for (ublox::Reader::iterator it = reader.pos();
00213 it != reader.pos() + reader.length() + 8; ++it)
00214 oss << boost::format("%02x") % static_cast<unsigned int>(*it) << " ";
00215 ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8,
00216 oss.str().c_str());
00217 }
00218
00219 handle(reader);
00220 }
00221
00222
00223 std::copy(reader.pos(), reader.end(), data);
00224 size -= reader.pos() - data;
00225 }
00226
00227 private:
00228 typedef std::multimap<std::pair<uint8_t, uint8_t>,
00229 boost::shared_ptr<CallbackHandler> > Callbacks;
00230
00231
00232 Callbacks callbacks_;
00233 boost::mutex callback_mutex_;
00234 };
00235
00236 }
00237
00238 #endif // UBLOX_GPS_CALLBACK_H