ROSaic
callbackhandlers.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 CALLBACKHANDLERS_HPP
60 #define CALLBACKHANDLERS_HPP
61 
62 // Boost includes
63 #include <boost/foreach.hpp>
64 // In C++, writing a loop that iterates over a sequence is tedious --> BOOST_FOREACH(char ch, "Hello World")
65 #include <boost/function.hpp>
66 // E.g. boost::function<int(const char*)> f = std::atoi;defines a pointer f that can point to functions that expect a parameter of type const char* and return a value of type int
67 // Generally, any place in which a function pointer would be used to defer a call or make a callback, Boost.Function can be used instead to allow the user greater flexibility in the implementation of the target.
68 #include <boost/thread.hpp>
69 // Boost's thread enables the use of multiple threads of execution with shared data in portable C++ code. It provides classes and functions for managing the threads themselves, along with others for synchronizing data between the threads or providing separate copies of data specific to individual threads.
70 #include <boost/thread/condition.hpp>
71 #include <boost/tokenizer.hpp>
72 // The tokenizer class provides a container view of a series of tokens contained in a sequence, e.g. if you are not interested in non-words...
73 #include <boost/algorithm/string/join.hpp>
74 // Join algorithm is a counterpart to split algorithms. It joins strings from a 'list' by adding user defined separator.
75 #include <boost/date_time/posix_time/posix_time.hpp>
76 // The class boost::posix_time::ptime that we will use defines a location-independent time. It uses the type boost::gregorian::date, yet also stores a time.
77 #include <boost/asio.hpp>
78 // Very important: Boost.Asio may be used to perform both synchronous and asynchronous operations on I/O objects such as sockets.
79 #include <boost/bind.hpp>
80 #include <boost/format.hpp>
81 #include <boost/asio/serial_port.hpp>
82 #include <boost/thread/mutex.hpp>
83 
84 // ROSaic includes
86 
93 extern bool publish_navsatfix;
94 extern bool publish_gpsfix;
95 extern bool publish_gpst;
97 extern bool response_received;
98 extern boost::mutex response_mutex;
99 extern boost::condition_variable response_condition;
100 extern bool cd_received;
101 extern boost::mutex cd_mutex;
102 extern boost::condition_variable cd_condition;
103 extern uint32_t cd_count;
104 extern std::string mosaic_tcp_port;
105 
106 namespace io_comm_mosaic
107 {
113  {
114  public:
115  virtual void Handle(mosaicMessage& mMessage, std::string message_key) = 0;
116 
117  bool Wait(const boost::posix_time::time_duration& timeout)
118  {
119  boost::mutex::scoped_lock lock(mutex_);
120  return condition_.timed_wait(lock, timeout);
121  }
122 
123  protected:
124  boost::mutex mutex_;
125  boost::condition_variable condition_;
126  };
127 
132  template <typename T>
134  {
135  public:
136  typedef boost::function<void(const T&)> Callback;
137 
138  CallbackHandler(const Callback& func = Callback()) : func_(func) {}
139 
140  virtual const T& Get() { return message_; }
141 
142  void Handle(mosaicMessage& mMessage, std::string message_key)
143  {
144  boost::mutex::scoped_lock lock(mutex_);
145  try
146  {
147  if (!mMessage.Read<T>(message_, message_key))
148  {
149  std::ostringstream ss;
150  ss << "Read unsuccessful: mosaic decoder error for message with ID (empty field if non-determinable)" << mMessage.MessageID() << ". Reason unknown.";
151  throw std::runtime_error(ss.str());
152  ROS_INFO("%s", ss.str().c_str());
153  return;
154  }
155  } catch (std::runtime_error& e)
156  {
157  std::ostringstream ss;
158  ss << "Read unsuccessful: mosaic decoder error for message with ID " << mMessage.MessageID() << ".\nReason: " << e.what();
159  throw std::runtime_error(ss.str());
160  ROS_INFO("%s", ss.str().c_str());
161  return;
162  }
163 
164  condition_.notify_all();
165  if (func_) func_(message_);
166  }
167 
168  private:
169  Callback func_;
171  };
172 
179  {
180 
181  public:
182 
184  typedef std::multimap<std::string, boost::shared_ptr<AbstractCallbackHandler>> CallbackMap;
185 
186  CallbackHandlers() = default;
187 
188  template <typename T>
191  CallbackMap Insert(std::string message_key, typename CallbackHandler<T>::Callback callback) // typename is not needed here
192  {
193  boost::mutex::scoped_lock lock(callback_mutex_);
194  CallbackHandler<T>* handler = new CallbackHandler<T>(callback); // Adding typename might be cleaner, but is optional again
195  callbackmap_.insert(std::make_pair(message_key, boost::shared_ptr<AbstractCallbackHandler>(handler)));
196  CallbackMap::key_type key = message_key;
197  ROS_DEBUG("Key successfully inserted to multimap: %s", ((unsigned int) callbackmap_.count(key)) ? "true" : "false");
198  return callbackmap_;
199  }
200 
203  void Handle(mosaicMessage& mMessage)
204  {
205  // Find the ROS message callback handler for equivalent mosaic message at hand & call it
206  boost::mutex::scoped_lock lock(callback_mutex_);
207  CallbackMap::key_type key = mMessage.MessageID();
208  std::string ID_temp = mMessage.MessageID();
209  if (!(ID_temp == "4013" || ID_temp == "4027" || ID_temp == "4001"|| ID_temp == "5908"))
210  // We only want to handle SatVisibility, ChannelStatus, DOP and VelCovGeodetic blocks in case GPSFix messages are to be published, see few lines below.
211  {
212  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback != callbackmap_.upper_bound(key); ++callback)
213  {
214  try
215  {
216  callback->second->Handle(mMessage, callback->first);
217  }
218  catch (std::runtime_error& e)
219  {
220  throw std::runtime_error(e.what());
221  }
222  }
223  }
224  // Call NavSatFix callback function if it was added
225  if (publish_navsatfix)
226  {
227  CallbackMap::key_type key = "NavSatFix";
228  std::string ID_temp = mMessage.MessageID();
229  if (ID_temp == "4007") // If no new PVTGeodetic block is coming in, there is no need to publish NavSatFix anew.
230  // It would be wasteful to also call the NavSatFix handle if the PosCovGeodetic block comes in.
231  {
232  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback != callbackmap_.upper_bound(key); ++callback)
233  {
234  try
235  {
236  callback->second->Handle(mMessage, callback->first);
237  }
238  catch (std::runtime_error& e)
239  {
240  throw std::runtime_error(e.what());
241  }
242  }
243  }
244  }
245  // Call geometry_msgs::PoseWithCovarianceStamped callback function if it was added
247  {
248  CallbackMap::key_type key = "PoseWithCovarianceStamped";
249  std::string ID_temp = mMessage.MessageID();
250  if (ID_temp == "4007") // If no new PVTGeodetic block is coming in, there is no need to publish PoseWithCovarianceStamped anew.
251  // It would be wasteful to also call the PoseWithCovarianceStamped handle if the PosCovGeodetic block etc. comes in.
252  {
253  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback != callbackmap_.upper_bound(key); ++callback)
254  {
255  try
256  {
257  callback->second->Handle(mMessage, callback->first);
258  }
259  catch (std::runtime_error& e)
260  {
261  throw std::runtime_error(e.what());
262  }
263  }
264  }
265  }
266  // Call sensor_msgs::TimeReference (with GPST) callback function if it was added
267  if (publish_gpst)
268  {
269  CallbackMap::key_type key = "GPST";
270  std::string ID_temp = mMessage.MessageID();
271  if (ID_temp == "4007") // If no new PVTGeodetic block is coming in, there is no need to publish PoseWithCovarianceStamped anew.
272  // It would be wasteful to also call the PoseWithCovarianceStamped handle if the PosCovGeodetic block etc. comes in.
273  {
274  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback != callbackmap_.upper_bound(key); ++callback)
275  {
276  try
277  {
278  callback->second->Handle(mMessage, callback->first);
279  }
280  catch (std::runtime_error& e)
281  {
282  throw std::runtime_error(e.what());
283  }
284  }
285  }
286  }
287  // Call GPSFix callback function if it was added
288  if (publish_gpsfix)
289  {
290  CallbackMap::key_type key1 = "GPSFix";
291  std::string ID_temp = mMessage.MessageID();
292  if (ID_temp == "4007") // If no new PVTGeodetic block is coming in, there is no need to publish GPSFix anew.
293  // It would be wasteful to also call the GPSFix handle when any of the PosCovGeodetic, SatVisibility, ChannelStatus etc. blocks comes in.
294  {
295  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1); callback != callbackmap_.upper_bound(key1); ++callback)
296  {
297  try
298  {
299  callback->second->Handle(mMessage, callback->first);
300  }
301  catch (std::runtime_error& e)
302  {
303  throw std::runtime_error(e.what());
304  }
305  }
306  }
307  CallbackMap::key_type key2 = mMessage.MessageID();
308  if (ID_temp == "4013" || ID_temp == "4027" || ID_temp == "4001" || ID_temp == "5908")
309  // Even though we are not interested in publishing SatVisibility (4012) and ChannelStatus (4013) ROS messages, we have to save
310  // some contents of these incoming blocks in order to publish the GPSFix message. When these 2 blocks come in, the unchanged GPSFix message is published anew.
311  {
312  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2); callback != callbackmap_.upper_bound(key2); ++callback)
313  {
314  try
315  {
316  callback->second->Handle(mMessage, callback->first);
317  }
318  catch (std::runtime_error& e)
319  {
320  throw std::runtime_error(e.what());
321  }
322  }
323  }
324  }
325  }
326 
332  void ReadCallback(const uint8_t* data, std::size_t& size)
333  {
334  mosaicMessage mMessage(data, size);
335  // Read !all! (there might be many) messages in the buffer
336  while (mMessage.Search() != mMessage.End() && mMessage.Found())
337  {
338  // Print the found message (if NMEA) or just show messageID (if SBF)..
339  if (mMessage.IsSBF())
340  {
341  unsigned long sbf_block_length;
342  sbf_block_length = (unsigned long) mMessage.BlockLength(); // C-like cast notation (since functional notation did not work, although https://www.cplusplus.com/doc/tutorial/typecasting/ suggests otherwise)
343  ROS_DEBUG("ROSaic reading SBF block %s with %lu bytes...", mMessage.MessageID().c_str(), sbf_block_length); // Recall: The long data type is at least 32 bits.
344  if (sbf_block_length < 15) // If the SBF block is so corrupted that not even its header's Length field could be properly read, don't bother to process the block, but technically not necessary here.
345  {
346  continue;
347  }
348  }
349  if (mMessage.IsNMEA())
350  {
351  boost::char_separator<char> sep("\r");
352  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
353  std::size_t nmea_size = std::min(mMessage.SegmentEnd(), static_cast<std::size_t>(89));
354  std::string block_in_string(reinterpret_cast<const char*>(mMessage.Pos()), nmea_size); // Syntax: new_string_name (const char* s, size_t n); size_t is either 2 or 8 bytes, depending on your system
355  tokenizer tokens(block_in_string, sep);
356  ROS_DEBUG("The NMEA message is ready to be parsed. It reads: %s", (*tokens.begin()).c_str());
357  }
358  if (mMessage.IsResponse())
359  {
360  std::size_t response_size = mMessage.SegmentEnd();
361  std::string block_in_string(reinterpret_cast<const char*>(mMessage.Pos()), response_size);
362  ROS_DEBUG("mosaic's response contains %li bytes and reads:\n %s", response_size, block_in_string.c_str());
363  {
364  boost::mutex::scoped_lock lock(response_mutex);
365  response_received = true;
366  lock.unlock();
367  response_condition.notify_one();
368  }
369  if (mMessage.IsErrorMessage())
370  {
371  ROS_ERROR("Invalid command just sent to mosaic!");
372  }
373  }
374  if (mMessage.IsConnectionDescriptor())
375  {
376  std::string cd(reinterpret_cast<const char*>(mMessage.Pos()), 4);
377  mosaic_tcp_port = cd;
378  ROS_INFO_COND(cd_count == 0, "The connection descriptor for the TCP connection is %s", cd.c_str());
379  if (cd_count < 3) ++cd_count;
380  if (cd_count == 2)
381  {
382  boost::mutex::scoped_lock lock(cd_mutex);
383  cd_received = true;
384  lock.unlock();
385  cd_condition.notify_one();
386  }
387  }
388  //ROS_DEBUG("Handing over from readcallback to Handle while count is %d", mMessage.GetCount());
389  try
390  {
391  Handle(mMessage);
392  }
393  catch (std::runtime_error& e)
394  {
395  ROS_DEBUG("Without a (circular) buffer and a new thread, we would have faced an incomplete message right now. %s", e.what());
396  throw (static_cast<std::size_t>(mMessage.Pos() - data));
397  }
398  }
399  }
400 
403  CallbackMap callbackmap_;
404 
405  private:
406 
409  static boost::mutex callback_mutex_;
410 
411  };
412 }
413 
414 #endif
bool publish_gpst
Whether or not to publish the sensor_msgs::TimeReference message with GPST.
Represents ensemble of (to be constructed) ROS messages, to be handled at once by this class...
std::string MessageID()
Returns the MessageID of the message where data_ is pointing at at the moment, SBF identifiers embell...
const uint8_t * Search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
Defines a class that can deal with a buffer of size bytes_transferred that is handed over from async_...
CallbackHandler(const Callback &func=Callback())
bool Wait(const boost::posix_time::time_duration &timeout)
CallbackMap Insert(std::string message_key, typename CallbackHandler< T >::Callback callback)
const uint8_t * End()
Gets the end position in the read buffer.
uint16_t BlockLength()
Gets the length of the SBF block.
bool IsErrorMessage()
Determines whether data_ currently points to an error message reply from mosaic.
boost::condition_variable response_condition
Condition variable complementing "response_mutex".
void Handle(mosaicMessage &mMessage, std::string message_key)
bool Found()
Has an NMEA message or SBF block been found in the buffer?
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
uint32_t cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
bool publish_navsatfix
Whether or not to publish the sensor_msgs::NavSatFix message.
void Handle(mosaicMessage &mMessage)
const uint8_t * Pos()
Gets the current position in the read buffer.
bool cd_received
Determines whether the connection descriptor was received from mosaic.
void ReadCallback(const uint8_t *data, std::size_t &size)
Searches for mosaic messages that could potentially be decoded/parsed/published.
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
bool response_received
Determines whether a command reply was received from mosaic.
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> ...
std::string mosaic_tcp_port
Mosaic TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
boost::mutex response_mutex
Mutex to control changes of global variable "response_received".
bool Read(typename boost::call_traits< T >::reference message, std::string message_key, bool search=false)
Performs the CRC check (if SBF block) and populates message with the necessary content (mapped 1-to-1...
bool IsSBF()
Determines whether data_ currently points to an SBF block.
bool IsConnectionDescriptor()
Determines whether data_ currently points to a connection descriptor (right after initiating TCP conn...
boost::mutex cd_mutex
Mutex to control changes of global variable "cd_received".
Can search buffer for messages, read/parse them, and so on.
virtual void Handle(mosaicMessage &mMessage, std::string message_key)=0
Abstract class representing a generic callback handler, includes high-level functionality such as wai...
bool publish_gpsfix
Whether or not to publish the gps_common::GPSFix message.
std::size_t SegmentEnd()
Determines size of message that data_ is currently pointing at.
boost::condition_variable cd_condition
Condition variable complementing "cd_mutex".
bool publish_posewithcovariancestamped
Whether or not to publish the geometry_msgs::PoseWithCovarianceStamped message.
boost::function< void(const T &)> Callback