ROSaic
Public Types | Public Member Functions | Data Fields | Static Private Attributes
io_comm_mosaic::CallbackHandlers Class Reference

Represents ensemble of (to be constructed) ROS messages, to be handled at once by this class. More...

#include <callbackhandlers.hpp>

Collaboration diagram for io_comm_mosaic::CallbackHandlers:
Collaboration graph
[legend]

Public Types

typedef 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> More...
 

Public Member Functions

 CallbackHandlers ()=default
 
template<typename T >
CallbackMap Insert (std::string message_key, typename CallbackHandler< T >::Callback callback)
 
void Handle (mosaicMessage &mMessage)
 
void ReadCallback (const uint8_t *data, std::size_t &size)
 Searches for mosaic messages that could potentially be decoded/parsed/published. More...
 

Data Fields

CallbackMap callbackmap_
 

Static Private Attributes

static boost::mutex callback_mutex_
 

Detailed Description

Represents ensemble of (to be constructed) ROS messages, to be handled at once by this class.

Definition at line 178 of file callbackhandlers.hpp.

Member Typedef Documentation

◆ CallbackMap

typedef std::multimap<std::string, boost::shared_ptr<AbstractCallbackHandler> > io_comm_mosaic::CallbackHandlers::CallbackMap

Key is std::string and represents the ROS message key, value is boost::shared_ptr<CallbackHandler>

Definition at line 184 of file callbackhandlers.hpp.

Constructor & Destructor Documentation

◆ CallbackHandlers()

io_comm_mosaic::CallbackHandlers::CallbackHandlers ( )
default

Member Function Documentation

◆ Handle()

void io_comm_mosaic::CallbackHandlers::Handle ( mosaicMessage mMessage)
inline

This method is called every time mMessage is found to contain some potentially useful message. The for loop forwards to a ROS message specific handle if the latter was added via callbackmap_.insert at some earlier point.

Definition at line 203 of file callbackhandlers.hpp.

References io_comm_mosaic::mosaicMessage::MessageID(), publish_gpsfix, publish_gpst, publish_navsatfix, and publish_posewithcovariancestamped.

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  }
bool publish_gpst
Whether or not to publish the sensor_msgs::TimeReference message with GPST.
bool publish_navsatfix
Whether or not to publish the sensor_msgs::NavSatFix message.
bool publish_gpsfix
Whether or not to publish the gps_common::GPSFix message.
bool publish_posewithcovariancestamped
Whether or not to publish the geometry_msgs::PoseWithCovarianceStamped message.
Here is the call graph for this function:

◆ Insert()

template<typename T >
CallbackMap io_comm_mosaic::CallbackHandlers::Insert ( std::string  message_key,
typename CallbackHandler< T >::Callback  callback 
)
inline

Insert adds a pair to our multimap, with the message_ID being the key; this method is called by handlers_ in rosaic_node.cpp T would be a (custom or not) ROS message, e.g. rosaic::PVTGeodetic, or nmea_msgs::GPGGA

Definition at line 191 of file callbackhandlers.hpp.

Referenced by rosaic_node::ROSaicNode::DefineMessages().

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  }
Here is the caller graph for this function:

◆ ReadCallback()

void io_comm_mosaic::CallbackHandlers::ReadCallback ( const uint8_t *  data,
std::size_t &  size 
)
inline

Searches for mosaic messages that could potentially be decoded/parsed/published.

Parameters
[in]dataBuffer passed on from AsyncManager class
[in]sizeSize of the buffer

Definition at line 332 of file callbackhandlers.hpp.

References io_comm_mosaic::mosaicMessage::BlockLength(), cd_condition, cd_count, cd_mutex, cd_received, io_comm_mosaic::mosaicMessage::End(), io_comm_mosaic::mosaicMessage::Found(), io_comm_mosaic::AbstractCallbackHandler::Handle(), io_comm_mosaic::mosaicMessage::IsConnectionDescriptor(), io_comm_mosaic::mosaicMessage::IsErrorMessage(), io_comm_mosaic::mosaicMessage::IsNMEA(), io_comm_mosaic::mosaicMessage::IsResponse(), io_comm_mosaic::mosaicMessage::IsSBF(), io_comm_mosaic::mosaicMessage::MessageID(), mosaic_tcp_port, io_comm_mosaic::mosaicMessage::Pos(), response_condition, response_mutex, response_received, io_comm_mosaic::mosaicMessage::Search(), and io_comm_mosaic::mosaicMessage::SegmentEnd().

Referenced by io_comm_mosaic::Comm_IO::SetManager().

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  }
boost::condition_variable response_condition
Condition variable complementing "response_mutex".
uint32_t cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
void Handle(mosaicMessage &mMessage)
bool cd_received
Determines whether the connection descriptor was received from mosaic.
bool response_received
Determines whether a command reply was received from mosaic.
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".
boost::mutex cd_mutex
Mutex to control changes of global variable "cd_received".
boost::condition_variable cd_condition
Condition variable complementing "cd_mutex".
Here is the call graph for this function:
Here is the caller graph for this function:

Field Documentation

◆ callback_mutex_

boost::mutex io_comm_mosaic::CallbackHandlers::callback_mutex_
staticprivate

The "static" keyword resolves construct-by-copying issues related to this mutex by making it available throughout the code. The mutex constructor list contains "mutex (const mutex&) = delete", hence construct-by-copying a mutex is explicitly prohibited.

The get_handlers() method of the Comm_IO class forces us to make this mutex static, since otherwise, the method would need to construct-by-copy the mutex, which is strictly prohibited.

Definition at line 409 of file callbackhandlers.hpp.

◆ callbackmap_

CallbackMap io_comm_mosaic::CallbackHandlers::callbackmap_

Callback handlers multimap for mosaic messages; it needs to be public since we copy-assign (did not work otherwise) new callbackmap_, after inserting a pair to the multimap within the DefineMessages() method of the ROSaicNode class, onto its old version.

Definition at line 403 of file callbackhandlers.hpp.

Referenced by rosaic_node::ROSaicNode::DefineMessages().


The documentation for this class was generated from the following files: