ROSaic
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 #ifndef MIN_NMEA_SIZE
63 #define MIN_NMEA_SIZE 9 // Minimum size of an NMEA message in bytes (e.g. proprietary "$PSSN,HRP")
64 #endif
65 
66 #ifndef MAX_NMEA_SIZE
67 #define MAX_NMEA_SIZE 89 // Maximum size of an NMEA message in bytes
68 #endif
69 
70 // Boost includes
71 #include <boost/foreach.hpp>
72 // In C++, writing a loop that iterates over a sequence is tedious --> BOOST_FOREACH(char ch, "Hello World")
73 #include <boost/function.hpp>
74 // E.g. boost::function<int(const char*)> f = std::atoi;defines a pointer f that can point to functions that
75 // expect a parameter of type const char* and return a value of type int
76 // Generally, any place in which a function pointer would be used to defer a call or make a callback,
77 // Boost.Function can be used instead to allow the user greater flexibility in the implementation of the target.
78 #include <boost/thread.hpp>
79 // Boost's thread enables the use of multiple threads of execution with shared data in portable C++ code.
80 // It provides classes and functions for managing the threads themselves, along with others for synchronizing
81 // data between the threads or providing separate copies of data specific to individual threads.
82 #include <boost/thread/condition.hpp>
83 #include <boost/tokenizer.hpp>
84 // The tokenizer class provides a container view of a series of tokens contained in a sequence, e.g. if you
85 // are not interested in non-words...
86 #include <boost/algorithm/string/join.hpp>
87 // Join algorithm is a counterpart to split algorithms. It joins strings from a 'list' by adding user defined separator.
88 #include <boost/date_time/posix_time/posix_time.hpp>
89 // The class boost::posix_time::ptime that we will use defines a location-independent time. It uses the
90 // type boost::gregorian::date, yet also stores a time.
91 #include <boost/asio.hpp>
92 // Boost.Asio may be used to perform both synchronous and asynchronous operations on I/O objects such as sockets.
93 #include <boost/bind.hpp>
94 #include <boost/format.hpp>
95 #include <boost/asio/serial_port.hpp>
96 #include <boost/thread/mutex.hpp>
97 
98 // ROSaic and C++ includes
100 #include <algorithm>
101 
110 extern bool g_dop_has_arrived_gpsfix;
119 extern bool g_atteuler_has_arrived_pose;
124 
125 extern bool g_publish_navsatfix;
126 extern bool g_publish_gpsfix;
127 extern bool g_publish_gpst;
128 extern bool g_publish_pose;
129 extern bool g_publish_diagnostics;
130 extern bool g_response_received;
131 extern boost::mutex g_response_mutex;
132 extern boost::condition_variable g_response_condition;
133 extern bool g_cd_received;
134 extern boost::mutex g_cd_mutex;
135 extern boost::condition_variable g_cd_condition;
136 extern uint32_t g_cd_count;
137 extern std::string g_rx_tcp_port;
138 
139 namespace io_comm_rx
140 {
146  {
147  public:
148  virtual void handle(RxMessage& rx_message, std::string message_key) = 0;
149 
150  bool Wait(const boost::posix_time::time_duration& timeout)
151  {
152  boost::mutex::scoped_lock lock(mutex_);
153  return condition_.timed_wait(lock, timeout);
154  }
155 
156  protected:
157  boost::mutex mutex_;
158  boost::condition_variable condition_;
159  };
160 
165  template <typename T>
167  {
168  public:
169 
170  virtual const T& Get() { return message_; }
171 
172  void handle(RxMessage& rx_message, std::string message_key)
173  {
174  boost::mutex::scoped_lock lock(mutex_);
175  try
176  {
177  if (!rx_message.read<T>(message_, message_key))
178  {
179  std::ostringstream ss;
180  ss << "Read unsuccessful: Rx decoder error for message with ID (empty field if non-determinable)" <<
181  rx_message.messageID() << ". Reason unknown.";
182  throw std::runtime_error(ss.str());
183  ROS_INFO("%s", ss.str().c_str());
184  return;
185  }
186  } catch (std::runtime_error& e)
187  {
188  std::ostringstream ss;
189  ss << "Read unsuccessful: Rx decoder error for message with ID " <<
190  rx_message.messageID() << ".\n" << e.what();
191  throw std::runtime_error(ss.str());
192  ROS_INFO("%s", ss.str().c_str());
193  return;
194  }
195 
196  condition_.notify_all();
197  }
198 
199  private:
201  };
202 
208  {
209 
210  public:
211 
213  typedef std::multimap<std::string, boost::shared_ptr<AbstractCallbackHandler>> CallbackMap;
214 
215  CallbackHandlers() = default;
216 
227  template <typename T>
228  CallbackMap insert(std::string message_key)
229  {
230  boost::mutex::scoped_lock lock(callback_mutex_);
231  // Adding typename might be cleaner, but is optional again
232  CallbackHandler<T>* handler = new CallbackHandler<T>();
233  callbackmap_.insert(std::make_pair(message_key, boost::shared_ptr<AbstractCallbackHandler>(handler)));
234  CallbackMap::key_type key = message_key;
235  ROS_DEBUG("Key %s successfully inserted into multimap: %s", message_key.c_str(),
236  ((unsigned int) callbackmap_.count(key)) ? "true" : "false");
237  return callbackmap_;
238  }
239 
244  void handle(RxMessage& rx_message);
245 
251  void readCallback(const uint8_t* data, std::size_t& size);
252 
257  CallbackMap callbackmap_;
258 
259  private:
260 
265  static boost::mutex callback_mutex_;
266 
269  static std::string do_gpsfix_;
270 
273  static std::string do_navsatfix_;
274 
277  static std::string do_pose_;
278 
281  static std::string do_diagnostics_;
282 
284  typedef std::map<std::string, uint32_t> GPSFixMap;
285 
288  static GPSFixMap gpsfix_map;
289 
291  typedef std::map<std::string, uint32_t> NavSatFixMap;
292 
295  static NavSatFixMap navsatfix_map;
296 
299  typedef std::map<std::string, uint32_t> PoseWithCovarianceStampedMap;
300 
303  static PoseWithCovarianceStampedMap pose_map;
304 
307  typedef std::map<std::string, uint32_t> DiagnosticArrayMap;
308 
311  static DiagnosticArrayMap diagnosticarray_map;
312  };
313 
314 }
315 
316 #endif
std::map< std::string, uint32_t > GPSFixMap
Shorthand for the map responsible for matching ROS message identifiers relevant for GPSFix to a uint3...
bool g_poscovgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PosCovGeodetic block of the current epoch has arrived or not...
bool g_publish_navsatfix
Whether or not to publish the sensor_msgs::NavSatFix message.
std::map< std::string, uint32_t > PoseWithCovarianceStampedMap
bool g_publish_gpst
Whether or not to publish the sensor_msgs::TimeReference message with GPST.
bool g_poscovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PosCovGeodetic block of the current epoch has arrived or not.
bool g_poscovgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the current epoch has arrived or n...
static DiagnosticArrayMap diagnosticarray_map
std::map< std::string, uint32_t > DiagnosticArrayMap
bool g_channelstatus_has_arrived_gpsfix
For GPSFix: Whether the ChannelStatus block of the current epoch has arrived or not.
boost::condition_variable condition_
CallbackMap insert(std::string message_key)
Adds a pair to the multimap "callbackmap_", with the message_key being the key.
std::string messageID()
bool Wait(const boost::posix_time::time_duration &timeout)
bool g_dop_has_arrived_gpsfix
For GPSFix: Whether the DOP block of the current epoch has arrived or not.
std::multimap< std::string, boost::shared_ptr< AbstractCallbackHandler > > CallbackMap
Key is std::string and represents the ROS message key, value is boost::shared_ptr<CallbackHandler> ...
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...
bool g_qualityind_has_arrived_diagnostics
For DiagnosticArray: Whether the QualityInd block of the current epoch has arrived or not...
Defines a class that reads messages handed over from the circular buffer.
Can search buffer for messages, read/parse them, and so on.
Definition: rx_message.hpp:200
bool g_atteuler_has_arrived_gpsfix
For GPSFix: Whether the AttEuler block of the current epoch has arrived or not.
Represents ensemble of (to be constructed) ROS messages, to be handled at once by this class...
bool g_atteuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttEuler block of the current epoch has arrived or not...
bool g_response_received
Determines whether a command reply was received from the Rx.
uint32_t g_cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
virtual void handle(RxMessage &rx_message, std::string message_key)=0
boost::mutex g_cd_mutex
Mutex to control changes of global variable "g_cd_received".
void handle(RxMessage &rx_message, std::string message_key)
bool g_publish_pose
Whether or not to publish the geometry_msgs::PoseWithCovarianceStamped message.
std::string g_rx_tcp_port
Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
bool g_pvtgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the current epoch has arrived or not...
bool g_cd_received
Determines whether the connection descriptor was received from the Rx.
bool g_attcoveuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttCovEuler block of the current epoch has arrived or not...
std::map< std::string, uint32_t > NavSatFixMap
Shorthand for the map responsible for matching ROS message identifiers relevant for NavSatFix to a ui...
boost::condition_variable g_cd_condition
Condition variable complementing "g_cd_mutex".
bool g_receiverstatus_has_arrived_diagnostics
For DiagnosticArray: Whether the ReceiverStatus block of the current epoch has arrived or not...
boost::condition_variable g_response_condition
Condition variable complementing "g_response_mutex".
bool g_measepoch_has_arrived_gpsfix
For GPSFix: Whether the MeasEpoch block of the current epoch has arrived or not.
static boost::mutex callback_mutex_
bool g_publish_gpsfix
Whether or not to publish the gps_common::GPSFix message.
bool g_pvtgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
bool read(typename boost::call_traits< T >::reference message, std::string message_key, bool search=false)
Performs the CRC check (if SBF) and populates ROS message "message" with the necessary content...
Definition: rx_message.hpp:477
bool g_attcoveuler_has_arrived_gpsfix
For GPSFix: Whether the AttCovEuler block of the current epoch has arrived or not.
bool g_publish_diagnostics
Whether or not to publish the diagnostic_msgs::DiagnosticArray message.
bool g_pvtgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
static PoseWithCovarianceStampedMap pose_map
bool g_velcovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the VelCovGeodetic block of the current epoch has arrived or not.