ROSaic
callback_handlers.cpp
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 
32 
39 std::pair<std::string, uint32_t> gpsfix_pairs[] =
40 {
41  std::make_pair("4013", 0),
42  std::make_pair("4027", 1),
43  std::make_pair("4001", 2),
44  std::make_pair("4007", 3),
45  std::make_pair("5906", 4),
46  std::make_pair("5908", 5),
47  std::make_pair("5938", 6),
48  std::make_pair("5939", 7)
49 };
50 
51 std::pair<std::string, uint32_t> navsatfix_pairs[] =
52 {
53  std::make_pair("4007", 0),
54  std::make_pair("5906", 1)
55 };
56 
57 
58 std::pair<std::string, uint32_t> pose_pairs[] =
59 {
60  std::make_pair("4007", 0),
61  std::make_pair("5906", 1),
62  std::make_pair("5938", 2),
63  std::make_pair("5939", 3)
64 };
65 
66 std::pair<std::string, uint32_t> diagnosticarray_pairs[] =
67 {
68  std::make_pair("4014", 0),
69  std::make_pair("4082", 1)
70 };
71 
72 namespace io_comm_rx
73 {
75 
82 
83  std::string CallbackHandlers::do_gpsfix_ = "4007";
84  std::string CallbackHandlers::do_navsatfix_ = "4007";
85  std::string CallbackHandlers::do_pose_ = "4007";
86  std::string CallbackHandlers::do_diagnostics_ = "4014";
87 
91  {
92  // Find the ROS message callback handler for the equivalent Rx message (SBF/NMEA) at hand & call it
93  boost::mutex::scoped_lock lock(callback_mutex_);
94  CallbackMap::key_type key = rx_message.messageID();
95  std::string ID_temp = rx_message.messageID();
96  if (!(ID_temp == "4013" || ID_temp == "4027" || ID_temp == "4001"|| ID_temp == "5908" ||
97  ID_temp == "4014" || ID_temp == "4082" || ID_temp == "5902"))
98  // We only want to handle ChannelStatus, MeasEpoch, DOP, VelCovGeodetic, ReceiverStatus,
99  // QualityInd and ReceiverSetup blocks in case GPSFix and DiagnosticArray messages are to
100  // be published, respectively, see few lines below.
101  {
102  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback !=
103  callbackmap_.upper_bound(key); ++callback)
104  {
105  try
106  {
107  callback->second->handle(rx_message, callback->first);
108  }
109  catch (std::runtime_error& e)
110  {
111  throw std::runtime_error(e.what());
112  }
113  }
114  }
115  // Call NavSatFix callback function if it was added
117  {
118  CallbackMap::key_type key = "NavSatFix";
119  std::string ID_temp = rx_message.messageID();
120  if (ID_temp == do_navsatfix_)
121  // The last incoming block among PVTGeodetic and PosCovGeodetic triggers the publishing of NavSatFix.
122  {
123  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback !=
124  callbackmap_.upper_bound(key); ++callback)
125  {
126  try
127  {
128  callback->second->handle(rx_message, callback->first);
129  }
130  catch (std::runtime_error& e)
131  {
132  throw std::runtime_error(e.what());
133  }
134  }
135  do_navsatfix_ = std::string();
136  }
137  }
138  // Call geometry_msgs::PoseWithCovarianceStamped callback function if it was added
139  if (g_publish_pose)
140  {
141  CallbackMap::key_type key = "PoseWithCovarianceStamped";
142  std::string ID_temp = rx_message.messageID();
143  if (ID_temp == do_pose_)
144  // The last incoming block among PVTGeodetic, PosCovGeodetic, AttEuler and AttCovEuler triggers
145  // the publishing of PoseWithCovarianceStamped.
146  {
147  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback !=
148  callbackmap_.upper_bound(key); ++callback)
149  {
150  try
151  {
152  callback->second->handle(rx_message, callback->first);
153  }
154  catch (std::runtime_error& e)
155  {
156  throw std::runtime_error(e.what());
157  }
158  }
159  do_pose_ = std::string();
160  }
161  }
162  // Call diagnostic_msgs::DiagnosticArray callback function if it was added
164  {
165  CallbackMap::key_type key1 = rx_message.messageID();
166  std::string ID_temp = rx_message.messageID();
167  if (ID_temp == "4014" || ID_temp == "4082" || ID_temp == "5902")
168  {
169  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1); callback !=
170  callbackmap_.upper_bound(key1); ++callback)
171  {
172  try
173  {
174  callback->second->handle(rx_message, callback->first);
175  }
176  catch (std::runtime_error& e)
177  {
178  throw std::runtime_error(e.what());
179  }
180  }
181  }
182  CallbackMap::key_type key2 = "DiagnosticArray";
183  if (ID_temp == do_diagnostics_)
184  // The last incoming block among ReceiverStatus, QualityInd and ReceiverSetup triggers
185  // the publishing of DiagnosticArray.
186  {
187  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2); callback !=
188  callbackmap_.upper_bound(key2); ++callback)
189  {
190  try
191  {
192  callback->second->handle(rx_message, callback->first);
193  }
194  catch (std::runtime_error& e)
195  {
196  throw std::runtime_error(e.what());
197  }
198  }
199  do_diagnostics_ = std::string();
200  }
201  }
202  // Call sensor_msgs::TimeReference (with GPST) callback function if it was added
203  if (g_publish_gpst)
204  {
205  CallbackMap::key_type key1 = "GPST";
206  std::string ID_temp = rx_message.messageID();
207  // If no new PVTGeodetic block is coming in, there is no need to publish sensor_msgs::TimeReference (with GPST) anew.
208  if (ID_temp == "4007")
209  {
210  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1); callback !=
211  callbackmap_.upper_bound(key1); ++callback)
212  {
213  try
214  {
215  callback->second->handle(rx_message, callback->first);
216  }
217  catch (std::runtime_error& e)
218  {
219  throw std::runtime_error(e.what());
220  }
221  }
222  }
223  }
224  // Call GPSFix callback function if it was added
225  if (g_publish_gpsfix)
226  {
227  std::string ID_temp = rx_message.messageID();
228  CallbackMap::key_type key1 = rx_message.messageID();
229  if (ID_temp == "4013" || ID_temp == "4027" || ID_temp == "4001" || ID_temp == "5908")
230  // Even though we are not interested in publishing ChannelStatus (4013), MeasEpoch (4027), DOP (4001)
231  // and VelCovGeodetic (5908) ROS messages, we have to save some contents of these incoming blocks
232  // in order to publish the GPSFix message.
233  {
234  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1); callback !=
235  callbackmap_.upper_bound(key1); ++callback)
236  {
237  try
238  {
239  callback->second->handle(rx_message, callback->first);
240  }
241  catch (std::runtime_error& e)
242  {
243  throw std::runtime_error(e.what());
244  }
245  }
246  }
247  CallbackMap::key_type key2 = "GPSFix";
248  if (ID_temp == do_gpsfix_)
249  // The last incoming block among ChannelStatus (4013), MeasEpoch (4027), DOP (4001) and
250  // VelCovGeodetic (5908) triggers the publishing of GPSFix.
251  {
252  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2); callback !=
253  callbackmap_.upper_bound(key2); ++callback)
254  {
255  try
256  {
257  callback->second->handle(rx_message, callback->first);
258  }
259  catch (std::runtime_error& e)
260  {
261  throw std::runtime_error(e.what());
262  }
263  }
264  do_gpsfix_ = std::string();
265  }
266  }
267  }
268 
269  void CallbackHandlers::readCallback(const uint8_t* data, std::size_t& size)
270  {
271  RxMessage rx_message(data, size);
272  // Read !all! (there might be many) messages in the buffer
273  while (rx_message.search() != rx_message.getEndBuffer() && rx_message.found())
274  {
275  // Print the found message (if NMEA) or just show messageID (if SBF)..
276  if (rx_message.isSBF())
277  {
278  std::size_t sbf_block_length;
279  std::string ID_temp = rx_message.messageID();
280  sbf_block_length = static_cast<std::size_t>(rx_message.getBlockLength());
281  ROS_DEBUG("ROSaic reading SBF block %s made up of %li bytes...", ID_temp.c_str(), sbf_block_length);
282  // If full message did not yet arrive, throw an error message.
283  if (sbf_block_length > rx_message.getCount())
284  {
285  ROS_DEBUG("Not a valid SBF block, parts of the SBF block are yet to be received. Ignore..");
286  throw (static_cast<std::size_t>(rx_message.getPosBuffer() - data));
287  }
288  if (g_publish_gpsfix == true && (ID_temp == "4013" || ID_temp == "4027" ||
289  ID_temp == "4001" || ID_temp == "4007" || ID_temp == "5906" || ID_temp == "5908" ||
290  ID_temp == "5938" || ID_temp == "5939"))
291  {
292  std::vector<bool> gpsfix_vec = {g_channelstatus_has_arrived_gpsfix, g_measepoch_has_arrived_gpsfix,
296  gpsfix_vec.erase(gpsfix_vec.begin() + gpsfix_map[ID_temp]);
297  // Checks whether all entries in gpsfix_vec are true
298  if (std::all_of(gpsfix_vec.begin(), gpsfix_vec.end(), [](bool v) { return v; }) == true)
299  {
300  do_gpsfix_ = ID_temp;
301  }
302  }
303  if (g_publish_navsatfix == true && (ID_temp == "4007" || ID_temp == "5906"))
304  {
305  std::vector<bool> navsatfix_vec = {g_pvtgeodetic_has_arrived_navsatfix,
307  navsatfix_vec.erase(navsatfix_vec.begin() + navsatfix_map[ID_temp]);
308  // Checks whether all entries in navsatfix_vec are true
309  if (std::all_of(navsatfix_vec.begin(), navsatfix_vec.end(), [](bool v) { return v; }) == true)
310  {
311  do_navsatfix_ = ID_temp;
312  }
313  }
314  if (g_publish_pose == true && (ID_temp == "4007" || ID_temp == "5906" ||
315  ID_temp == "5938" || ID_temp == "5939"))
316  {
319  pose_vec.erase(pose_vec.begin() + pose_map[ID_temp]);
320  // Checks whether all entries in pose_vec are true
321  if (std::all_of(pose_vec.begin(), pose_vec.end(), [](bool v) { return v; }) == true)
322  {
323  do_pose_ = ID_temp;
324  }
325  }
326  if (g_publish_diagnostics == true && (ID_temp == "4014" || ID_temp == "4082"))
327  {
328  std::vector<bool> diagnostics_vec = {g_receiverstatus_has_arrived_diagnostics,
330  diagnostics_vec.erase(diagnostics_vec.begin() + diagnosticarray_map[ID_temp]);
331  // Checks whether all entries in diagnostics_vec are true
332  if (std::all_of(diagnostics_vec.begin(), diagnostics_vec.end(), [](bool v) { return v; }) == true)
333  {
334  do_diagnostics_ = ID_temp;
335  }
336  }
337  }
338  if (rx_message.isNMEA())
339  {
340  boost::char_separator<char> sep("\r");
341  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
342  std::size_t nmea_size = rx_message.messageSize();
343  if (nmea_size < MIN_NMEA_SIZE || nmea_size > MAX_NMEA_SIZE)
344  {
345  ROS_DEBUG("Not a valid NMEA message, parts of the message are yet to be received. Ignore..");
346  throw (static_cast<std::size_t>(rx_message.getPosBuffer() - data));
347  }
348  // Syntax: new_string_name (const char* s, size_t n); size_t is either 2 or 8 bytes, depending on your system
349  std::string block_in_string(reinterpret_cast<const char*>(rx_message.getPosBuffer()), nmea_size);
350  tokenizer tokens(block_in_string, sep);
351  ROS_DEBUG("The NMEA message is ready to be parsed. It reads: %s", (*tokens.begin()).c_str());
352  }
353  if (rx_message.isResponse()) // If the response is not sent at once, only first part is ROS_DEBUG-printed
354  {
355  std::size_t response_size = rx_message.messageSize();
356  std::string block_in_string(reinterpret_cast<const char*>(rx_message.getPosBuffer()), response_size);
357  ROS_DEBUG("The Rx's response contains %li bytes and reads:\n %s", response_size, block_in_string.c_str());
358  {
359  boost::mutex::scoped_lock lock(g_response_mutex);
360  g_response_received = true;
361  lock.unlock();
362  g_response_condition.notify_one();
363  }
364  if (rx_message.isErrorMessage())
365  {
366  ROS_ERROR("Invalid command just sent to the Rx!");
367  }
368  continue;
369  }
370  if (rx_message.isConnectionDescriptor())
371  {
372  std::string cd(reinterpret_cast<const char*>(rx_message.getPosBuffer()), 4);
373  g_rx_tcp_port = cd;
374  ROS_INFO_COND(g_cd_count == 0, "The connection descriptor for the TCP connection is %s", cd.c_str());
375  if (g_cd_count < 3) ++g_cd_count;
376  if (g_cd_count == 2)
377  {
378  boost::mutex::scoped_lock lock(g_cd_mutex);
379  g_cd_received = true;
380  lock.unlock();
381  g_cd_condition.notify_one();
382  }
383  continue;
384  }
385  //ROS_DEBUG("Handing over from readcallback to handle while count is %li", rx_message.getCount());
386  try
387  {
388  handle(rx_message);
389  }
390  catch (std::runtime_error& e)
391  {
392  ROS_DEBUG("Without a (circular) buffer and a new thread, we would have faced an incomplete message right now. %s", e.what());
393  throw (static_cast<std::size_t>(rx_message.getPosBuffer() - data));
394  }
395  }
396  }
397 }
bool g_atteuler_has_arrived_gpsfix
For GPSFix: Whether the AttEuler block of the current epoch has arrived or not.
void readCallback(const uint8_t *data, std::size_t &size)
Searches for Rx messages that could potentially be decoded/parsed/published.
bool found()
Has an NMEA message, SBF block or command reply been found in the buffer?
Definition: rx_message.cpp:820
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_channelstatus_has_arrived_gpsfix
For GPSFix: Whether the ChannelStatus block of the current epoch has arrived or not.
std::size_t getCount()
Returns the count_ variable.
Definition: rx_message.hpp:241
std::map< std::string, uint32_t > PoseWithCovarianceStampedMap
bool g_cd_received
Determines whether the connection descriptor was received from the Rx.
bool g_dop_has_arrived_gpsfix
For GPSFix: Whether the DOP block of the current epoch has arrived or not.
boost::mutex g_response_mutex
Mutex to control changes of global variable "g_response_received".
std::string g_rx_tcp_port
Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
bool isResponse()
Determines whether data_ currently points to an NMEA message.
Definition: rx_message.cpp:972
static DiagnosticArrayMap diagnosticarray_map
std::map< std::string, uint32_t > DiagnosticArrayMap
std::pair< std::string, uint32_t > diagnosticarray_pairs[]
bool g_attcoveuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttCovEuler block of the current epoch has arrived or not...
std::pair< std::string, uint32_t > navsatfix_pairs[]
std::string messageID()
bool g_attcoveuler_has_arrived_gpsfix
For GPSFix: Whether the AttCovEuler block of the current epoch has arrived or not.
bool g_qualityind_has_arrived_diagnostics
For DiagnosticArray: Whether the QualityInd 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_publish_pose
Whether or not to publish the geometry_msgs::PoseWithCovarianceStamped message.
Can search buffer for messages, read/parse them, and so on.
Definition: rx_message.hpp:200
bool isErrorMessage()
Determines whether data_ currently points to an error message reply from the Rx.
boost::mutex g_cd_mutex
Mutex to control changes of global variable "g_cd_received".
bool isNMEA()
Determines whether data_ currently points to an NMEA message.
Definition: rx_message.cpp:952
uint16_t getBlockLength()
Gets the length of the SBF block.
bool g_poscovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PosCovGeodetic block of the current epoch has arrived or not.
bool g_velcovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the VelCovGeodetic block of the current epoch has arrived or not.
boost::condition_variable g_cd_condition
Condition variable complementing "g_cd_mutex".
void handle(RxMessage &rx_message)
Ćalled every time rx_message is found to contain some potentially useful message. ...
std::size_t messageSize()
Determines size of the message (also command reply) that data_ is currently pointing at...
Definition: rx_message.cpp:852
bool g_poscovgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PosCovGeodetic block of the current epoch has arrived or not...
const uint8_t * getEndBuffer()
Gets the end position in the read buffer.
std::pair< std::string, uint32_t > gpsfix_pairs[]
std::map< std::string, uint32_t > NavSatFixMap
Shorthand for the map responsible for matching ROS message identifiers relevant for NavSatFix to a ui...
bool g_atteuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttEuler block of the current epoch has arrived or not...
bool g_measepoch_has_arrived_gpsfix
For GPSFix: Whether the MeasEpoch block of the current epoch has arrived or not.
bool g_publish_navsatfix
Whether or not to publish the sensor_msgs::NavSatFix message.
bool g_pvtgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
static boost::mutex callback_mutex_
Handles callbacks when reading NMEA/SBF messages.
bool isSBF()
Determines whether data_ currently points to an SBF block.
Definition: rx_message.cpp:933
bool g_publish_gpsfix
Whether or not to publish the gps_common::GPSFix message.
std::pair< std::string, uint32_t > pose_pairs[]
#define MAX_NMEA_SIZE
bool g_receiverstatus_has_arrived_diagnostics
For DiagnosticArray: Whether the ReceiverStatus block of the current epoch has arrived or not...
const uint8_t * getPosBuffer()
Gets the current position in the read buffer.
const uint8_t * search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
Definition: rx_message.cpp:834
bool g_poscovgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the current epoch has arrived or n...
bool g_response_received
Determines whether a command reply was received from the Rx.
bool g_pvtgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the current epoch has arrived or not...
bool g_publish_gpst
Whether or not to publish the sensor_msgs::TimeReference message with GPST.
uint32_t g_cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
boost::condition_variable g_response_condition
Condition variable complementing "g_response_mutex".
static PoseWithCovarianceStampedMap pose_map
bool g_pvtgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PVTGeodetic block of the current epoch has arrived or not.