52 rosaic::PVTGeodeticPtr msg = boost::make_shared<rosaic::PVTGeodetic>();
58 msg->Block_Header.TOW = data.
TOW;
59 msg->Block_Header.WNc = data.
WNc;
60 msg->Mode = data.
Mode;
61 msg->Error = data.
Error;
73 msg->Datum = data.
Datum;
74 msg->NrSV = data.
NrSV;
85 msg->Misc = data.
Misc;
92 rosaic::PVTCartesianPtr msg = boost::make_shared<rosaic::PVTCartesian>();
98 msg->Block_Header.TOW = data.
TOW;
99 msg->Block_Header.WNc = data.
WNc;
100 msg->Mode = data.
Mode;
101 msg->Error = data.
Error;
113 msg->Datum = data.
Datum;
114 msg->NrSV = data.
NrSV;
125 msg->Misc = data.
Misc;
131 rosaic::PosCovCartesianPtr msg = boost::make_shared<rosaic::PosCovCartesian>();
137 msg->Block_Header.TOW = data.
TOW;
138 msg->Block_Header.WNc = data.
WNc;
139 msg->Mode = data.
Mode;
140 msg->Error = data.
Error;
141 msg->Cov_xx = data.
Cov_xx;
142 msg->Cov_yy = data.
Cov_yy;
143 msg->Cov_zz = data.
Cov_zz;
144 msg->Cov_bb = data.
Cov_bb;
145 msg->Cov_xy = data.
Cov_xy;
146 msg->Cov_xz = data.
Cov_xz;
147 msg->Cov_xb = data.
Cov_xb;
148 msg->Cov_yz = data.
Cov_yz;
149 msg->Cov_yb = data.
Cov_yb;
150 msg->Cov_zb = data.
Cov_zb;
157 rosaic::PosCovGeodeticPtr msg = boost::make_shared<rosaic::PosCovGeodetic>();
163 msg->Block_Header.TOW = data.
TOW;
164 msg->Block_Header.WNc = data.
WNc;
165 msg->Mode = data.
Mode;
166 msg->Error = data.
Error;
170 msg->Cov_bb = data.
Cov_bb;
176 msg->Cov_hb = data.
Cov_hb;
182 rosaic::AttEulerPtr msg = boost::make_shared<rosaic::AttEuler>();
188 msg->Block_Header.TOW = data.
TOW;
189 msg->Block_Header.WNc = data.
WNc;
190 msg->NrSV = data.
NrSV;
191 msg->Error = data.
Error;
192 msg->Mode = data.
Mode;
194 msg->Pitch = data.
Pitch;
195 msg->Roll = data.
Roll;
204 rosaic::AttCovEulerPtr msg = boost::make_shared<rosaic::AttCovEuler>();
210 msg->Block_Header.TOW = data.
TOW;
211 msg->Block_Header.WNc = data.
WNc;
212 msg->Error = data.
Error;
231 geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
235 msg->pose.pose.position.y =
static_cast<double>(
last_pvtgeodetic_.
Latitude)*360/(2*boost::math::constants::pi<double>());
241 msg->pose.covariance[3] = 0;
242 msg->pose.covariance[4] = 0;
243 msg->pose.covariance[5] = 0;
247 msg->pose.covariance[9] = 0;
248 msg->pose.covariance[10] = 0;
249 msg->pose.covariance[11] = 0;
253 msg->pose.covariance[15] = 0;
254 msg->pose.covariance[16] = 0;
255 msg->pose.covariance[17] = 0;
256 msg->pose.covariance[18] = 0;
257 msg->pose.covariance[19] = 0;
258 msg->pose.covariance[20] = 0;
262 msg->pose.covariance[24] = 0;
263 msg->pose.covariance[25] = 0;
264 msg->pose.covariance[26] = 0;
268 msg->pose.covariance[30] = 0;
269 msg->pose.covariance[31] = 0;
270 msg->pose.covariance[32] = 0;
285 sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
292 msg->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
297 msg->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
300 case 2:
case 4:
case 5:
case 7:
case 8:
case 10:
302 msg->status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
307 msg->status.status = sensor_msgs::NavSatStatus::STATUS_SBAS_FIX;
312 throw std::runtime_error(
"PVTGeodetic's Mode field contains an invalid type of PVT solution.");
315 bool gps_in_pvt =
false;
316 bool glo_in_pvt =
false;
317 bool com_in_pvt =
false;
318 bool gal_in_pvt =
false;
320 for(
int bit = 0; bit != 31; ++bit)
323 if (bit <= 5 && in_use)
327 if (8 <= bit && bit <= 12 && in_use) glo_in_pvt =
true;
328 if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) com_in_pvt =
true;
329 if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) gal_in_pvt =
true;
333 uint16_t service = gps_in_pvt*1+glo_in_pvt*2+com_in_pvt*4+gal_in_pvt*8;
334 msg->status.service = service;
347 msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
367 gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
372 std::vector<int32_t> cno_tracked;
373 std::vector<int32_t> svid_in_sync;
383 svid_in_sync.push_back(static_cast<int32_t>(measepoch_channel_type1->
SVID));
384 uint8_t type_mask = 15;
385 if (measepoch_channel_type1->
Type & type_mask == static_cast<uint8_t>(1) || measepoch_channel_type1->
Type & type_mask == static_cast<uint8_t>(2))
387 cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->
CN0)/4);
391 cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->
CN0)/4+static_cast<int32_t>(10));
395 for (int32_t j = 0; j < static_cast<int32_t>(measepoch_channel_type1->
N_Type2); j++)
403 std::vector<int32_t> svid_in_sync_2;
404 std::vector<int32_t> elevation_tracked;
405 std::vector<int32_t> azimuth_tracked;
408 std::vector<int32_t> ordering;
416 uint16_t azimuth_mask = 511;
421 bool to_be_added =
false;
422 for (int32_t j = 0; j < static_cast<int32_t>(svid_in_sync.size()); ++j)
424 if (svid_in_sync[j] == static_cast<int32_t>(channel_sat_info->
SVID))
426 ordering.push_back(j);
433 svid_in_sync_2.push_back(static_cast<int32_t>(channel_sat_info->
SVID));
434 elevation_tracked.push_back(static_cast<int32_t>(channel_sat_info->
Elev));
435 azimuth_tracked.push_back(static_cast<int32_t>((channel_sat_info->
Az_RiseSet & azimuth_mask)));
438 for (int32_t j = 0; j < static_cast<int32_t>(channel_sat_info->
N2); j++)
442 bool pvt_status =
false;
443 uint16_t pvt_status_mask = std::pow(2,15)+std::pow(2,14);
444 for(
int k = 15; k != -1; k -= 2)
446 uint16_t pvt_status_value = (channel_state_info->
PVTStatus & pvt_status_mask) >> k-1;
448 if (pvt_status_value == 2)
454 pvt_status_mask = pvt_status_mask - std::pow(2,k) - std::pow(2,k-1) + std::pow(2,k-2) + std::pow(2,k-3);
459 svid_pvt.push_back(static_cast<int32_t>(channel_sat_info->
SVID));
465 msg->status.satellite_used_prn =
svid_pvt;
466 msg->status.satellites_visible =
static_cast<uint16_t
>(svid_in_sync.size());
467 msg->status.satellite_visible_prn = svid_in_sync_2;
468 msg->status.satellite_visible_z = elevation_tracked;
469 msg->status.satellite_visible_azimuth = azimuth_tracked;
472 std::vector<int32_t> cno_tracked_reordered;
476 for (int32_t k = 0; k < static_cast<int32_t>(ordering.size()); ++k)
478 cno_tracked_reordered.push_back(cno_tracked[ordering[k]]);
481 msg->status.satellite_visible_snr = cno_tracked_reordered;
484 uint16_t status_mask = 15;
490 msg->status.status = gps_common::GPSStatus::STATUS_NO_FIX;
495 msg->status.status = gps_common::GPSStatus::STATUS_FIX;
498 case 2:
case 4:
case 5:
case 7:
case 8:
case 10:
500 msg->status.status = gps_common::GPSStatus::STATUS_GBAS_FIX;
506 if (reference_id == 131 || reference_id == 133 || reference_id == 135 || reference_id == 135)
508 msg->status.status = gps_common::GPSStatus::STATUS_WAAS_FIX;
512 msg->status.status = gps_common::GPSStatus::STATUS_SBAS_FIX;
518 throw std::runtime_error(
"PVTGeodetic's Mode field contains an invalid type of PVT solution.");
521 msg->status.motion_source = gps_common::GPSStatus::SOURCE_POINTS;
522 msg->status.orientation_source = gps_common::GPSStatus::SOURCE_POINTS;
523 msg->status.position_source = gps_common::GPSStatus::SOURCE_GPS;
534 msg->gdop =
static_cast<double>(-1);
538 msg->gdop = std::sqrt(std::pow(static_cast<double>(
last_dop_.
PDOP)/100, 2) + std::pow(static_cast<double>(
last_dop_.
TDOP)/100, 2));
542 msg->pdop =
static_cast<double>(-1);
550 msg->hdop =
static_cast<double>(-1);
558 msg->vdop =
static_cast<double>(-1);
566 msg->tdop =
static_cast<double>(-1);
591 msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
603 uint16_t hh = (TOW%(1000*60*60*24))/(60*60*1000);
604 uint16_t mm = ((TOW%(1000*60*60*24))-hh*(60*60*1000))/(60*1000);
605 uint16_t ss = ((TOW%(1000*60*60*24))-hh*(60*60*1000)-mm*(60*1000))/(1000);
606 uint16_t hs = ((TOW%(1000*60*60*24))-hh*(60*60*1000)-mm*(60*1000)-ss*1000)/10;
634 boost::format fmt = boost::format(
"%02u%02u%02u.%02u") % hh % mm % ss % hs;
635 std::string utc_string = fmt.str();
640 uint32_t unix_time_nanoseconds = (
static_cast<uint32_t
>(utc_double*100)%100)*10000000;
641 ros::Time time_obj(unix_time_seconds, unix_time_nanoseconds);
646 time_t time_now = time(NULL);
647 ros::Time time_obj((uint32_t) time_now, 0);
688 uint32_t count_copy =
count_;
696 if (count_copy == 0)
break;
706 if (count_copy == 0)
break;
716 uint16_t mask = 8191;
717 if (*(reinterpret_cast<const uint16_t *>(
data_ + 4)) & mask == static_cast<const uint16_t>(ID))
738 boost::char_separator<char> sep(
",");
739 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
740 std::size_t nmea_size = std::min(this->
SegmentEnd(), static_cast<std::size_t>(89));
741 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
742 tokenizer tokens(block_in_string,sep);
743 if (*tokens.begin() == ID)
823 uint16_t mask = 8191;
824 uint16_t value = (*(
reinterpret_cast<const uint16_t*
>(
data_+4))) & mask;
825 std::stringstream ss;
831 boost::char_separator<char> sep(
",");
832 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
833 std::size_t nmea_size = std::min(this->
SegmentEnd(), static_cast<std::size_t>(89));
834 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
835 tokenizer tokens(block_in_string,sep);
836 return *tokens.begin();
838 return std::string();
857 uint16_t block_length;
859 block_length = *(
reinterpret_cast<const uint16_t*
>(
data_ + 6));
893 if (jump_size == 0) jump_size = 1;
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
Struct for the SBF block "AttCovEuler".
const uint8_t * Next()
Goes to the start of the next message based on the calculated length of current message.
#define CONNECTION_DESCRIPTOR_BYTE_2
0x50 is ASCII for P - 2nd character of connection descriptor sent by mosaic after initiating TCP conn...
std::size_t segment_size_
Helps to determine size of response message / NMEA message / SBF block.
bool found_
Whether or not a message has been found.
BlockHeader_t Block_Header
BlockHeader_t Block_Header
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_...
rosaic::AttCovEulerPtr AttCovEulerCallback(AttCovEuler &data)
Callback function when reading AttCovEuler blocks.
static DOP last_dop_
Since GPSFix etc. need DOP, incoming DOP blocks need to be stored.
#define RESPONSE_SYNC_BYTE_2
0x52 is ASCII for R (for "Response") - 2nd byte in each response from mosaic
#define SBF_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
const uint8_t * End()
Gets the end position in the read buffer.
#define CONNECTION_DESCRIPTOR_BYTE_1
0x49 is ASCII for I - 1st character of connection descriptor sent by mosaic after initiating TCP conn...
uint16_t BlockLength()
Gets the length of the SBF block.
Struct for the SBF block "VelCovGeodetic".
rosaic::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic &data)
Callback function when reading PVTGeodetic blocks.
bool IsErrorMessage()
Determines whether data_ currently points to an error message reply from mosaic.
bool ToDouble(const std::string &string, double &value)
Interprets the contents of "string" as a floating point number of type double, stores its value in "v...
Struct for the SBF sub-block "MeasEpochChannelType1".
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer's size in t...
rosaic::AttEulerPtr AttEulerCallback(AttEuler &data)
Callback function when reading AttEuler blocks.
#define RESPONSE_SYNC_BYTE_3
0x3F is ASCII for ? - 3rd byte in the response message from mosaic in case the command was invalid ...
static VelCovGeodetic last_velcovgeodetic_
Since GPSFix etc. need VelCovGeodetic (for signal-to-noise ratios), incoming VelCovGeodetic blocks ne...
Struct for the SBF block "PosCovGeodetic".
static AttCovEuler last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
std::vector< int32_t > svid_pvt
rosaic::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian &data)
Callback function when reading PosCovCartesian blocks.
gps_common::GPSFixPtr GPSFixCallback()
"Callback" function when constructing GPSFix messages
bool Found()
Has an NMEA message or SBF block been found in the buffer?
geometry_msgs::Quaternion ToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.
uint32_t cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
bool read_cd
Whether or not we still want to read the connection descriptor, which we only want in the very beginn...
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
Struct for the SBF block "PVTGeodetic".
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
Struct for the SBF block "AttEuler".
uint8_t Data[(80+1) *sizeof(ChannelSatInfo)+((80+1) *4) *sizeof(ChannelStateInfo)]
const uint8_t * Pos()
Gets the current position in the read buffer.
#define NMEA_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
static ChannelStatus last_channelstatus_
Since GPSFix etc. need ChannelStatus, incoming ChannelStatus blocks need to be stored.
bool CRCcheck_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
Struct for the SBF sub-block "ChannelStateInfo".
BlockHeader_t Block_Header
#define RESPONSE_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each response from mosaic
Struct for the SBF block "PVTCartesian".
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
#define SBF_SYNC_BYTE_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
Struct for the SBF block "PosCovCartesian".
sensor_msgs::NavSatFixPtr NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
rosaic::PVTCartesianPtr PVTCartesianCallback(PVTCartesian &data)
Callback function when reading PVTCartesian blocks.
ros::Time TimestampSBF(uint32_t TOW, bool use_GNSS)
Calculates the timestamp, in the Unix Epoch time format, either using the TOW as transmitted with the...
#define NMEA_SYNC_BYTE_2_1
0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message
#define CARRIAGE_RETURN
0x0D is ASCII for "Carriage Return", i.e. "Enter"
bool IsSBF()
Determines whether data_ currently points to an SBF block.
Struct for the SBF sub-block "ChannelSatInfo".
Struct for the SBF block "ChannelStatus".
const uint8_t * data_
The pointer to the buffer of messages.
Struct for the SBF block "MeasEpoch".
bool IsConnectionDescriptor()
Determines whether data_ currently points to a connection descriptor (right after initiating TCP conn...
Struct for the SBF block "DOP".
#define LINE_FEED
0x0A is ASCII for "Line Feed", i.e. "New Line"
time_t UTCtoUnix(double utc_double)
Converts UTC time from the without-colon-delimiter format, type double, to Unix Epoch time (a number-...
BlockHeader_t Block_Header
static MeasEpoch last_measepoch_
Since GPSFix etc. need MeasEpoch (for signal-to-noise ratios), incoming MeasEpoch blocks need to be s...
uint8_t Data[((80+1) *sizeof(MeasEpochChannelType1)+(((80+1)) *(((7) *(3)) -1)) *sizeof(MeasEpochChannelType2))]
rosaic::PosCovGeodeticPtr PosCovGeodeticCallback(PosCovGeodetic &data)
Callback function when reading PosCovGeodetic blocks.
static uint32_t count_gpsfix_
Number of times the gps_common::GPSFix message has been published.
BlockHeader_t Block_Header
BlockHeader_t Block_Header
std::size_t SegmentEnd()
Determines size of message that data_ is currently pointing at.
uint32_t leap_seconds
The number of leap seconds that have been inserted into the UTC time.
BlockHeader_t Block_Header
BlockHeader_t Block_Header
#define NMEA_SYNC_BYTE_2_2
0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message
bool IsMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.