callback_handlers.hpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // © Copyright 2020, Septentrio NV/SA.
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // 2. Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // 3. Neither the name of the copyright holder nor the names of its
14 // contributors may be used to endorse or promote products derived
15 // from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // *****************************************************************************
30 
31 // *****************************************************************************
32 //
33 // Boost Software License - Version 1.0 - August 17th, 2003
34 //
35 // Permission is hereby granted, free of charge, to any person or organization
36 // obtaining a copy of the software and accompanying documentation covered by
37 // this license (the "Software") to use, reproduce, display, distribute,
38 // execute, and transmit the Software, and to prepare derivative works of the
39 // Software, and to permit third-parties to whom the Software is furnished to
40 // do so, all subject to the following:
41 
42 // The copyright notices in the Software and this entire statement, including
43 // the above license grant, this restriction and the following disclaimer,
44 // must be included in all copies of the Software, in whole or in part, and
45 // all derivative works of the Software, unless such copies or derivative
46 // works are solely in the form of machine-executable object code generated by
47 // a source language processor.
48 //
49 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
50 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
51 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
52 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
53 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
54 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
55 // DEALINGS IN THE SOFTWARE.
56 //
57 // *****************************************************************************
58 
59 #ifndef CALLBACK_HANDLERS_HPP
60 #define CALLBACK_HANDLERS_HPP
61 
62 // Boost includes
63 #include <boost/foreach.hpp>
64 // In C++, writing a loop that iterates over a sequence is tedious -->
65 // BOOST_FOREACH(char ch, "Hello World")
66 #include <boost/function.hpp>
67 // E.g. boost::function<int(const char*)> f = std::atoi;defines a pointer f that can
68 // point to functions that expect a parameter of type const char* and return a value
69 // of type int Generally, any place in which a function pointer would be used to
70 // defer a call or make a callback, Boost.Function can be used instead to allow the
71 // user greater flexibility in the implementation of the target.
72 #include <boost/thread.hpp>
73 // Boost's thread enables the use of multiple threads of execution with shared data
74 // in portable C++ code. It provides classes and functions for managing the threads
75 // themselves, along with others for synchronizing data between the threads or
76 // providing separate copies of data specific to individual threads.
77 #include <boost/thread/condition.hpp>
78 #include <boost/tokenizer.hpp>
79 // The tokenizer class provides a container view of a series of tokens contained in a
80 // sequence, e.g. if you are not interested in non-words...
81 #include <boost/algorithm/string/join.hpp>
82 // Join algorithm is a counterpart to split algorithms. It joins strings from a
83 // 'list' by adding user defined separator.
84 #include <boost/date_time/posix_time/posix_time.hpp>
85 // The class boost::posix_time::ptime that we will use defines a location-independent
86 // time. It uses the type boost::gregorian::date, yet also stores a time.
87 #include <boost/asio.hpp>
88 // Boost.Asio may be used to perform both synchronous and asynchronous operations on
89 // I/O objects such as sockets.
90 #include <boost/asio/serial_port.hpp>
91 #include <boost/bind.hpp>
92 #include <boost/format.hpp>
93 #include <boost/thread/mutex.hpp>
94 
95 // ROSaic and C++ includes
96 #include <algorithm>
98 
105 extern bool g_response_received;
106 extern boost::mutex g_response_mutex;
107 extern boost::condition_variable g_response_condition;
108 extern bool g_cd_received;
109 extern boost::mutex g_cd_mutex;
110 extern boost::condition_variable g_cd_condition;
111 extern uint32_t g_cd_count;
112 extern std::string g_rx_tcp_port;
113 
114 namespace io_comm_rx {
121  {
122  public:
123  virtual void handle(RxMessage& rx_message, std::string message_key) = 0;
124 
125  bool Wait(const boost::posix_time::time_duration& timeout)
126  {
127  boost::mutex::scoped_lock lock(mutex_);
128  return condition_.timed_wait(lock, timeout);
129  }
130 
131  protected:
132  boost::mutex mutex_;
133  boost::condition_variable condition_;
134  };
135 
140  template <typename T>
142  {
143  public:
144  virtual const T& Get() { return message_; }
145 
146  void handle(RxMessage& rx_message, std::string message_key)
147  {
148  boost::mutex::scoped_lock lock(mutex_);
149  try
150  {
151  if (!rx_message.read(message_key))
152  {
153  std::ostringstream ss;
154  ss << "Rx decoder error for message with ID (empty field if non-determinable)"
155  << rx_message.messageID() << ". Reason unknown.";
156  throw std::runtime_error(ss.str());
157  return;
158  }
159  } catch (std::runtime_error& e)
160  {
161  std::ostringstream ss;
162  ss << "Rx decoder error for message with ID "
163  << rx_message.messageID() << ".\n"
164  << e.what();
165  throw std::runtime_error(ss.str());
166  return;
167  }
168 
169  condition_.notify_all();
170  }
171 
172  private:
174  };
175 
182  {
183 
184  public:
187  typedef std::multimap<std::string,
190 
192  node_(node),
193  rx_message_(node, settings),
194  settings_(settings)
195  {}
196 
208  template <typename T>
209  CallbackMap insert(std::string message_key)
210  {
211  boost::mutex::scoped_lock lock(callback_mutex_);
212  // Adding typename might be cleaner, but is optional again
213  CallbackHandler<T>* handler = new CallbackHandler<T>();
214  callbackmap_.insert(std::make_pair(
215  message_key, boost::shared_ptr<AbstractCallbackHandler>(handler)));
216  CallbackMap::key_type key = message_key;
217  node_->log(LogLevel::DEBUG, "Key " + message_key + " successfully inserted into multimap: " +
218  (((unsigned int)callbackmap_.count(key)) ? "true" : "false"));
219  return callbackmap_;
220  }
221 
226  void handle();
227 
235  void readCallback(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size);
236 
242 
243  private:
246 
249 
252 
258  static boost::mutex callback_mutex_;
259 
262  static std::string do_gpsfix_;
263 
266  static std::string do_insgpsfix_;
267 
271  static std::string do_navsatfix_;
272 
275  static std::string do_insnavsatfix_;
276 
280  static std::string do_pose_;
281 
285  static std::string do_inspose_;
286 
290  static std::string do_diagnostics_;
291 
295  static std::string do_imu_;
296 
300  static std::string do_inslocalization_;
301 
304  typedef std::unordered_map<std::string, uint32_t> GPSFixMap;
305 
308  static GPSFixMap gpsfix_map;
309 
312  typedef std::unordered_map<std::string, uint32_t> NavSatFixMap;
313 
316  static NavSatFixMap navsatfix_map;
317 
320  typedef std::unordered_map<std::string, uint32_t> PoseWithCovarianceStampedMap;
321 
324  static PoseWithCovarianceStampedMap pose_map;
325 
328  typedef std::unordered_map<std::string, uint32_t> DiagnosticArrayMap;
329 
332  static DiagnosticArrayMap diagnosticarray_map;
333 
336  typedef std::unordered_map<std::string, uint32_t> ImuMap;
337 
340  static ImuMap imu_map;
341 
344  typedef std::unordered_map<std::string, uint32_t> LocalizationMap;
345 
348  static LocalizationMap localization_map;
349  };
350 
351 } // namespace io_comm_rx
352 
353 #endif
bool read(std::string message_key, bool search=false)
Performs the CRC check (if SBF) and publishes ROS messages.
CallbackHandlers(ROSaicNodeBase *node, Settings *settings)
std::unordered_map< std::string, uint32_t > PoseWithCovarianceStampedMap
RxMessage rx_message_
RxMessage parser.
std::multimap< std::string, boost::shared_ptr< AbstractCallbackHandler > > CallbackMap
std::string messageID()
Represents ensemble of (to be constructed) ROS messages, to be handled at once by this class...
bool Wait(const boost::posix_time::time_duration &timeout)
static LocalizationMap localization_map
uint64_t Timestamp
Definition: typedefs.hpp:77
boost::mutex g_response_mutex
Mutex to control changes of global variable "g_response_received".
Abstract class representing a generic callback handler, includes high-level functionality such as wai...
Defines a class that reads messages handed over from the circular buffer.
static boost::mutex callback_mutex_
static std::string do_inslocalization_
bool g_response_received
Determines whether a command reply was received from the Rx.
uint32_t g_cd_count
std::unordered_map< std::string, uint32_t > LocalizationMap
boost::mutex g_cd_mutex
Mutex to control changes of global variable "g_cd_received".
CallbackMap insert(std::string message_key)
Adds a pair to the multimap "callbackmap_", with the message_key being the key.
std::string g_rx_tcp_port
Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
Settings struct.
Definition: rx_message.hpp:135
ROSaicNodeBase * node_
Pointer to Node.
virtual void handle(RxMessage &rx_message, std::string message_key)=0
bool g_cd_received
Determines whether the connection descriptor was received from the Rx.
static DiagnosticArrayMap diagnosticarray_map
boost::condition_variable g_cd_condition
Condition variable complementing "g_cd_mutex".
boost::condition_variable condition_
std::unordered_map< std::string, uint32_t > ImuMap
boost::condition_variable g_response_condition
Condition variable complementing "g_response_mutex".
std::unordered_map< std::string, uint32_t > GPSFixMap
std::unordered_map< std::string, uint32_t > NavSatFixMap
This class is the base class for abstraction.
Definition: typedefs.hpp:160
Can search buffer for messages, read/parse them, and so on.
Definition: rx_message.hpp:393
void handle(RxMessage &rx_message, std::string message_key)
std::unordered_map< std::string, uint32_t > DiagnosticArrayMap
static PoseWithCovarianceStampedMap pose_map


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Thu Jun 23 2022 02:11:38