68 #include <type_traits>
70 #include <boost/spirit/include/qi.hpp>
231 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129,
232 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252,
233 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c,
234 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
235 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672,
236 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738,
237 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861,
238 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
239 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc,
240 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5,
241 0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b,
242 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
243 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9,
244 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3,
245 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c,
246 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
247 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3,
248 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
249 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676,
250 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
251 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c,
252 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16,
253 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b,
254 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
255 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36,
256 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
258 namespace qi = boost::spirit::qi;
264 template <
typename Val>
268 std::is_same<uint16_t, Val>::value || std::is_same<uint32_t, Val>::value ||
269 std::is_same<uint64_t, Val>::value || std::is_same<float, Val>::value ||
270 std::is_same<double, Val>::value);
272 if (std::is_same<uint16_t, Val>::value)
275 }
else if (std::is_same<uint32_t, Val>::value)
278 }
else if (std::is_same<float, Val>::value)
280 s = std::numeric_limits<float>::quiet_NaN();
281 }
else if (std::is_same<double, Val>::value)
283 s = std::numeric_limits<double>::quiet_NaN();
292 template <
typename Val>
295 static_assert(std::is_same<float, Val>::value ||
296 std::is_same<double, Val>::value);
298 if constexpr (std::is_same<float, Val>::value)
301 s = std::numeric_limits<float>::quiet_NaN();
302 }
else if constexpr (std::is_same<double, Val>::value)
305 s = std::numeric_limits<double>::quiet_NaN();
313 template <
typename It,
typename Val>
317 std::is_same<int8_t, Val>::value || std::is_same<uint8_t, Val>::value ||
318 std::is_same<int16_t, Val>::value || std::is_same<uint16_t, Val>::value ||
319 std::is_same<int32_t, Val>::value || std::is_same<uint32_t, Val>::value ||
320 std::is_same<int64_t, Val>::value || std::is_same<uint64_t, Val>::value ||
321 std::is_same<float, Val>::value || std::is_same<double, Val>::value);
323 if constexpr (std::is_same<int8_t, Val>::value)
325 qi::parse(it, it + 1, qi::char_, val);
326 }
else if constexpr (std::is_same<uint8_t, Val>::value)
328 qi::parse(it, it + 1, qi::byte_, val);
329 }
else if constexpr ((std::is_same<int16_t, Val>::value) ||
330 (std::is_same<uint16_t, Val>::value))
332 qi::parse(it, it + 2, qi::little_word, val);
333 }
else if constexpr ((std::is_same<int32_t, Val>::value) ||
334 (std::is_same<uint32_t, Val>::value))
336 qi::parse(it, it + 4, qi::little_dword, val);
337 }
else if constexpr ((std::is_same<int64_t, Val>::value) ||
338 (std::is_same<uint64_t, Val>::value))
340 qi::parse(it, it + 8, qi::little_qword, val);
341 }
else if constexpr (std::is_same<float, Val>::value)
343 qi::parse(it, it + 4, qi::little_bin_float, val);
345 }
else if constexpr (std::is_same<double, Val>::value)
347 qi::parse(it, it + 8, qi::little_bin_double, val);
356 template <
typename It>
360 qi::parse(it, it + num, qi::repeat(num)[qi::char_], val);
362 val.erase(std::remove(val.begin(), val.end(),
'\0'), val.end());
369 template <
typename It,
typename Hdr>
387 block_header.id = ID & 8191;
388 block_header.revision = ID >> 13;
399 template <
typename It>
407 std::advance(it, sb2_length - 8);
414 template <
typename It>
429 std::to_string(msg.
n2));
434 std::advance(it, sb1_length - 12);
447 template <
typename It>
463 "Parse error: Too many ChannelSatInfo " + std::to_string(msg.
n));
470 for (
auto& satInfo : msg.
satInfo)
487 template <
typename It>
503 msg.
pdop = temp / 100.0;
505 msg.
tdop = temp / 100.0;
507 msg.
hdop = temp / 100.0;
509 msg.
vdop = temp / 100.0;
524 template <
typename It>
537 std::advance(it, sb2_length - 12);
544 template <
typename It>
562 std::advance(it, sb1_length - 20);
566 std::to_string(msg.n2));
569 msg.type2.resize(msg.n2);
570 for (
auto& type2 : msg.type2)
581 template <
typename It>
587 if (msg.block_header.id != 4027)
590 std::to_string(msg.block_header.id));
597 std::to_string(msg.n));
603 if (msg.block_header.revision > 0)
606 msg.type1.resize(msg.n);
607 for (
auto& type1 : msg.type1)
625 template <
typename It>
631 if (msg.block_header.id != 4245)
634 std::to_string(msg.block_header.id));
655 template <
typename It>
661 std::advance(it, sb_length - 7);
668 template <
typename It>
674 if (msg.block_header.id != 4092)
677 std::to_string(msg.block_header.id));
684 msg.rfband.resize(msg.n);
685 for (
auto& rfband : msg.rfband)
701 template <
typename It>
759 template <
typename It>
765 if (msg.block_header.id != 5914)
768 std::to_string(msg.block_header.id));
791 template <
typename It>
797 if (msg.block_header.id != 4006)
800 std::to_string(msg.block_header.id));
823 if (msg.block_header.revision > 0)
828 if (msg.block_header.revision > 1)
847 template <
typename It>
853 if (msg.block_header.id != 4007)
856 std::to_string(msg.block_header.id));
879 if (msg.block_header.revision > 0)
884 if (msg.block_header.revision > 1)
903 template <
typename It>
909 if (msg.block_header.id != 5938)
912 std::to_string(msg.block_header.id));
925 if (use_ros_axis_orientation)
927 msg.heading = -msg.heading + 90;
928 msg.pitch = -msg.pitch;
929 msg.pitch_dot = -msg.pitch_dot;
930 msg.heading_dot = -msg.heading_dot;
944 template <
typename It>
947 bool use_ros_axis_orientation)
951 if (msg.block_header.id != 5939)
954 std::to_string(msg.block_header.id));
965 if (use_ros_axis_orientation)
967 msg.cov_headroll = -msg.cov_headroll;
968 msg.cov_pitchroll = -msg.cov_pitchroll;
982 template <
typename It>
1000 std::advance(it, sb_length - 52);
1007 template <
typename It>
1013 if (msg.block_header.id != 4043)
1016 std::to_string(msg.block_header.id));
1023 "Parse error: Too many VectorInfoCart " + std::to_string(msg.n));
1027 msg.vector_info_cart.resize(msg.n);
1028 for (
auto& vector_info_cart : msg.vector_info_cart)
1044 template <
typename It>
1062 std::advance(it, sb_length - 52);
1069 template <
typename It>
1075 if (msg.block_header.id != 4028)
1078 std::to_string(msg.block_header.id));
1085 "Parse error: Too many VectorInfoGeod " + std::to_string(msg.n));
1089 msg.vector_info_geod.resize(msg.n);
1090 for (
auto& vector_info_geod : msg.vector_info_geod)
1106 template <
typename It>
1109 bool use_ros_axis_orientation)
1113 if ((msg.block_header.id != 4225) && (msg.block_header.id != 4229))
1116 std::to_string(msg.block_header.id));
1131 if ((msg.sb_list & 1) != 0)
1142 if ((msg.sb_list & 2) != 0)
1147 if (use_ros_axis_orientation)
1149 msg.heading = -msg.heading + 90;
1150 msg.pitch = -msg.pitch;
1158 if ((msg.sb_list & 4) != 0)
1169 if ((msg.sb_list & 8) != 0)
1180 if ((msg.sb_list & 16) != 0)
1191 if ((msg.sb_list & 32) != 0)
1202 if ((msg.sb_list & 64) != 0)
1207 if (use_ros_axis_orientation)
1209 msg.heading_roll_cov = -msg.heading_roll_cov;
1210 msg.pitch_roll_cov = -msg.pitch_roll_cov;
1218 if ((msg.sb_list & 128) != 0)
1241 template <
typename It>
1247 if (msg.block_header.id != 5905)
1250 std::to_string(msg.block_header.id));
1277 template <
typename It>
1283 if (msg.block_header.id != 5906)
1286 std::to_string(msg.block_header.id));
1313 template <
typename It>
1319 if (msg.block_header.id != 5907)
1322 std::to_string(msg.block_header.id));
1349 template <
typename It>
1355 if (msg.block_header.id != 5908)
1358 std::to_string(msg.block_header.id));
1385 template <
typename It>
1401 "Parse error: Too many indicators " + std::to_string(msg.
n));
1406 std::vector<uint16_t> indicators;
1423 template <
typename It>
1430 std::advance(it, sb_length - 4);
1437 template <
typename It>
1458 "Parse error: Too many AGCState " + std::to_string(msg.
n));
1481 template <
typename It>
1487 if (msg.block_header.id != 5914)
1490 std::to_string(msg.block_header.id));
1513 template <
typename It>
1516 bool use_ros_axis_orientation)
1520 if ((msg.block_header.id != 4226) && (msg.block_header.id != 4230))
1523 std::to_string(msg.block_header.id));
1539 if ((msg.sb_list & 1) != 0)
1550 if ((msg.sb_list & 2) != 0)
1555 if (use_ros_axis_orientation)
1557 msg.heading = -msg.heading + 90;
1558 msg.pitch = -msg.pitch;
1566 if ((msg.sb_list & 4) != 0)
1577 if ((msg.sb_list & 8) != 0)
1588 if ((msg.sb_list & 16) != 0)
1599 if ((msg.sb_list & 32) != 0)
1610 if ((msg.sb_list & 64) != 0)
1615 if (use_ros_axis_orientation)
1617 msg.heading_roll_cov = -msg.heading_roll_cov;
1618 msg.pitch_roll_cov = -msg.pitch_roll_cov;
1626 if ((msg.sb_list & 128) != 0)
1649 template <
typename It>
1655 if (msg.block_header.id != 4224)
1658 std::to_string(msg.block_header.id));
1669 if (use_ros_axis_orientation)
1671 msg.ant_lever_arm_y = -msg.ant_lever_arm_y;
1672 msg.ant_lever_arm_z = -msg.ant_lever_arm_z;
1687 template <
typename It>
1690 bool use_ros_axis_orientation)
1694 if (msg.block_header.id != 4244)
1697 std::to_string(msg.block_header.id));
1705 if (use_ros_axis_orientation)
1707 msg.lever_arm_y = -msg.lever_arm_y;
1708 msg.lever_arm_z = -msg.lever_arm_z;
1722 template <
typename It>
1725 bool use_ros_axis_orientation,
bool& hasImuMeas)
1729 if (msg.block_header.id != 4050)
1732 std::to_string(msg.block_header.id));
1737 if (msg.sb_length != 28)
1740 "Parse error: Wrong sb_length " + std::to_string(msg.sb_length));
1744 msg.acceleration_x = std::numeric_limits<double>::quiet_NaN();
1745 msg.acceleration_y = std::numeric_limits<double>::quiet_NaN();
1746 msg.acceleration_z = std::numeric_limits<double>::quiet_NaN();
1748 msg.angular_rate_x = std::numeric_limits<double>::quiet_NaN();
1749 msg.angular_rate_y = std::numeric_limits<double>::quiet_NaN();
1750 msg.angular_rate_z = std::numeric_limits<double>::quiet_NaN();
1752 msg.velocity_x = std::numeric_limits<double>::quiet_NaN();
1753 msg.velocity_y = std::numeric_limits<double>::quiet_NaN();
1754 msg.velocity_z = std::numeric_limits<double>::quiet_NaN();
1756 msg.std_dev_x = std::numeric_limits<double>::quiet_NaN();
1757 msg.std_dev_y = std::numeric_limits<double>::quiet_NaN();
1758 msg.std_dev_z = std::numeric_limits<double>::quiet_NaN();
1760 msg.sensor_temperature = std::numeric_limits<float>::quiet_NaN();
1761 msg.zero_velocity_flag = std::numeric_limits<double>::quiet_NaN();
1763 msg.source.resize(msg.n);
1764 msg.sensor_model.resize(msg.n);
1765 msg.type.resize(msg.n);
1766 msg.obs_info.resize(msg.n);
1767 bool hasAcc =
false;
1768 bool hasOmega =
false;
1770 for (
size_t i = 0; i < msg.n; i++)
1777 switch (msg.type[i])
1800 msg.sensor_temperature = temp / 100.0f;
1802 msg.sensor_temperature = std::numeric_limits<float>::quiet_NaN();
1803 std::advance(it, 22);
1814 if (use_ros_axis_orientation)
1816 msg.velocity_y = -msg.velocity_y;
1817 msg.velocity_z = -msg.velocity_z;
1824 std::advance(it, 16);
1831 "Unknown external sensor measurement type in SBF ExtSensorMeas: " +
1832 std::to_string(msg.type[i]));
1833 std::advance(it, 24);
1843 hasImuMeas = hasAcc && hasOmega;