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)
53 std::make_pair(
"4007", 0),
54 std::make_pair(
"5906", 1)
60 std::make_pair(
"4007", 0),
61 std::make_pair(
"5906", 1),
62 std::make_pair(
"5938", 2),
63 std::make_pair(
"5939", 3)
68 std::make_pair(
"4014", 0),
69 std::make_pair(
"4082", 1)
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"))
102 for (CallbackMap::iterator callback =
callbackmap_.lower_bound(key); callback !=
107 callback->second->handle(rx_message, callback->first);
109 catch (std::runtime_error& e)
111 throw std::runtime_error(e.what());
118 CallbackMap::key_type key =
"NavSatFix";
119 std::string ID_temp = rx_message.
messageID();
123 for (CallbackMap::iterator callback =
callbackmap_.lower_bound(key); callback !=
128 callback->second->handle(rx_message, callback->first);
130 catch (std::runtime_error& e)
132 throw std::runtime_error(e.what());
141 CallbackMap::key_type key =
"PoseWithCovarianceStamped";
142 std::string ID_temp = rx_message.
messageID();
147 for (CallbackMap::iterator callback =
callbackmap_.lower_bound(key); callback !=
152 callback->second->handle(rx_message, callback->first);
154 catch (std::runtime_error& e)
156 throw std::runtime_error(e.what());
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")
169 for (CallbackMap::iterator callback =
callbackmap_.lower_bound(key1); callback !=
174 callback->second->handle(rx_message, callback->first);
176 catch (std::runtime_error& e)
178 throw std::runtime_error(e.what());
182 CallbackMap::key_type key2 =
"DiagnosticArray";
187 for (CallbackMap::iterator callback =
callbackmap_.lower_bound(key2); callback !=
192 callback->second->handle(rx_message, callback->first);
194 catch (std::runtime_error& e)
196 throw std::runtime_error(e.what());
205 CallbackMap::key_type key1 =
"GPST";
206 std::string ID_temp = rx_message.
messageID();
208 if (ID_temp ==
"4007")
210 for (CallbackMap::iterator callback =
callbackmap_.lower_bound(key1); callback !=
215 callback->second->handle(rx_message, callback->first);
217 catch (std::runtime_error& e)
219 throw std::runtime_error(e.what());
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")
234 for (CallbackMap::iterator callback =
callbackmap_.lower_bound(key1); callback !=
239 callback->second->handle(rx_message, callback->first);
241 catch (std::runtime_error& e)
243 throw std::runtime_error(e.what());
247 CallbackMap::key_type key2 =
"GPSFix";
252 for (CallbackMap::iterator callback =
callbackmap_.lower_bound(key2); callback !=
257 callback->second->handle(rx_message, callback->first);
259 catch (std::runtime_error& e)
261 throw std::runtime_error(e.what());
276 if (rx_message.
isSBF())
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);
283 if (sbf_block_length > rx_message.
getCount())
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));
289 ID_temp ==
"4001" || ID_temp ==
"4007" || ID_temp ==
"5906" || ID_temp ==
"5908" ||
290 ID_temp ==
"5938" || ID_temp ==
"5939"))
296 gpsfix_vec.erase(gpsfix_vec.begin() +
gpsfix_map[ID_temp]);
298 if (std::all_of(gpsfix_vec.begin(), gpsfix_vec.end(), [](
bool v) {
return v; }) ==
true)
307 navsatfix_vec.erase(navsatfix_vec.begin() +
navsatfix_map[ID_temp]);
309 if (std::all_of(navsatfix_vec.begin(), navsatfix_vec.end(), [](
bool v) {
return v; }) ==
true)
314 if (
g_publish_pose ==
true && (ID_temp ==
"4007" || ID_temp ==
"5906" ||
315 ID_temp ==
"5938" || ID_temp ==
"5939"))
319 pose_vec.erase(pose_vec.begin() +
pose_map[ID_temp]);
321 if (std::all_of(pose_vec.begin(), pose_vec.end(), [](
bool v) {
return v; }) ==
true)
332 if (std::all_of(diagnostics_vec.begin(), diagnostics_vec.end(), [](
bool v) {
return v; }) ==
true)
340 boost::char_separator<char> sep(
"\r");
341 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
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));
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());
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());
366 ROS_ERROR(
"Invalid command just sent to the Rx!");
372 std::string cd(reinterpret_cast<const char*>(rx_message.
getPosBuffer()), 4);
374 ROS_INFO_COND(
g_cd_count == 0,
"The connection descriptor for the TCP connection is %s", cd.c_str());
390 catch (std::runtime_error& e)
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));
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?
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.
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.
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[]
static GPSFixMap gpsfix_map
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.
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.
static std::string do_pose_
static std::string do_navsatfix_
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...
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...
static NavSatFixMap navsatfix_map
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.
bool g_publish_gpsfix
Whether or not to publish the gps_common::GPSFix message.
std::pair< std::string, uint32_t > pose_pairs[]
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)
bool isConnectionDescriptor()
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.
static std::string do_gpsfix_
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
static std::string do_diagnostics_
bool g_pvtgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PVTGeodetic block of the current epoch has arrived or not.