ROSaic
mosaicMessage.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 
49 
51 {
52  rosaic::PVTGeodeticPtr msg = boost::make_shared<rosaic::PVTGeodetic>();
53  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
54  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
55  msg->Block_Header.CRC = data.Block_Header.CRC;
56  msg->Block_Header.ID = data.Block_Header.ID;
57  msg->Block_Header.Length = data.Block_Header.Length;
58  msg->Block_Header.TOW = data.TOW;
59  msg->Block_Header.WNc = data.WNc;
60  msg->Mode = data.Mode;
61  msg->Error = data.Error;
62  msg->Latitude = data.Latitude;
63  msg->Longitude = data.Longitude;
64  msg->Height = data.Height;
65  msg->Undulation = data.Undulation;
66  msg->Vn = data.Vn;
67  msg->Ve = data.Ve;
68  msg->Vu = data.Vu;
69  msg->COG = data.COG;
70  msg->RxClkBias = data.RxClkBias;
71  msg->RxClkDrift = data.RxClkDrift;
72  msg->TimeSystem = data.TimeSystem;
73  msg->Datum = data.Datum;
74  msg->NrSV = data.NrSV;
75  msg->WACorrInfo = data.WACorrInfo;
76  msg->ReferenceID = data.ReferenceID;
77  msg->MeanCorrAge = data.MeanCorrAge;
78  msg->SignalInfo = data.SignalInfo;
79  msg->AlertFlag = data.AlertFlag;
80  msg->NrBases = data.NrBases;
81  msg->PPPInfo = data.PPPInfo;
82  msg->Latency = data.Latency;
83  msg->HAccuracy = data.HAccuracy;
84  msg->VAccuracy = data.VAccuracy;
85  msg->Misc = data.Misc;
86  return msg;
87 }
88 
89 
91 {
92  rosaic::PVTCartesianPtr msg = boost::make_shared<rosaic::PVTCartesian>();
93  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
94  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
95  msg->Block_Header.CRC = data.Block_Header.CRC;
96  msg->Block_Header.ID = data.Block_Header.ID;
97  msg->Block_Header.Length = data.Block_Header.Length;
98  msg->Block_Header.TOW = data.TOW;
99  msg->Block_Header.WNc = data.WNc;
100  msg->Mode = data.Mode;
101  msg->Error = data.Error;
102  msg->X = data.X;
103  msg->Y = data.Y;
104  msg->Z = data.Z;
105  msg->Undulation = data.Undulation;
106  msg->Vx = data.Vx;
107  msg->Vy = data.Vy;
108  msg->Vz = data.Vz;
109  msg->COG = data.COG;
110  msg->RxClkBias = data.RxClkBias;
111  msg->RxClkDrift = data.RxClkDrift;
112  msg->TimeSystem = data.TimeSystem;
113  msg->Datum = data.Datum;
114  msg->NrSV = data.NrSV;
115  msg->WACorrInfo = data.WACorrInfo;
116  msg->ReferenceID = data.ReferenceID;
117  msg->MeanCorrAge = data.MeanCorrAge;
118  msg->SignalInfo = data.SignalInfo;
119  msg->AlertFlag = data.AlertFlag;
120  msg->NrBases = data.NrBases;
121  msg->PPPInfo = data.PPPInfo;
122  msg->Latency = data.Latency;
123  msg->HAccuracy = data.HAccuracy;
124  msg->VAccuracy = data.VAccuracy;
125  msg->Misc = data.Misc;
126  return msg;
127 }
128 
130 {
131  rosaic::PosCovCartesianPtr msg = boost::make_shared<rosaic::PosCovCartesian>();
132  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
133  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
134  msg->Block_Header.CRC = data.Block_Header.CRC;
135  msg->Block_Header.ID = data.Block_Header.ID;
136  msg->Block_Header.Length = data.Block_Header.Length;
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;
151  return msg;
152 }
153 
154 
156 {
157  rosaic::PosCovGeodeticPtr msg = boost::make_shared<rosaic::PosCovGeodetic>();
158  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
159  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
160  msg->Block_Header.CRC = data.Block_Header.CRC;
161  msg->Block_Header.ID = data.Block_Header.ID;
162  msg->Block_Header.Length = data.Block_Header.Length;
163  msg->Block_Header.TOW = data.TOW;
164  msg->Block_Header.WNc = data.WNc;
165  msg->Mode = data.Mode;
166  msg->Error = data.Error;
167  msg->Cov_latlat = data.Cov_latlat;
168  msg->Cov_lonlon = data.Cov_lonlon;
169  msg->Cov_hgthgt = data.Cov_hgthgt;
170  msg->Cov_bb = data.Cov_bb;
171  msg->Cov_latlon = data.Cov_latlon;
172  msg->Cov_lathgt = data.Cov_lathgt;
173  msg->Cov_latb = data.Cov_latb;
174  msg->Cov_lonhgt = data.Cov_lonhgt;
175  msg->Cov_lonb = data.Cov_lonb;
176  msg->Cov_hb = data.Cov_hb;
177  return msg;
178 }
179 
181 {
182  rosaic::AttEulerPtr msg = boost::make_shared<rosaic::AttEuler>();
183  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
184  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
185  msg->Block_Header.CRC = data.Block_Header.CRC;
186  msg->Block_Header.ID = data.Block_Header.ID;
187  msg->Block_Header.Length = data.Block_Header.Length;
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;
193  msg->Heading = data.Heading;
194  msg->Pitch = data.Pitch;
195  msg->Roll = data.Roll;
196  msg->PitchDot = data.PitchDot;
197  msg->RollDot = data.RollDot;
198  msg->HeadingDot = data.HeadingDot;
199  return msg;
200 };
201 
203 {
204  rosaic::AttCovEulerPtr msg = boost::make_shared<rosaic::AttCovEuler>();
205  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
206  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
207  msg->Block_Header.CRC = data.Block_Header.CRC;
208  msg->Block_Header.ID = data.Block_Header.ID;
209  msg->Block_Header.Length = data.Block_Header.Length;
210  msg->Block_Header.TOW = data.TOW;
211  msg->Block_Header.WNc = data.WNc;
212  msg->Error = data.Error;
213  msg->Cov_HeadHead = data.Cov_HeadHead;
214  msg->Cov_PitchPitch = data.Cov_PitchPitch;
215  msg->Cov_RollRoll = data.Cov_RollRoll;
216  msg->Cov_HeadPitch = data.Cov_HeadPitch;
217  msg->Cov_HeadRoll = data.Cov_HeadRoll;
218  msg->Cov_PitchRoll = data.Cov_PitchRoll;
219  return msg;
220 };
221 
229 geometry_msgs::PoseWithCovarianceStampedPtr io_comm_mosaic::mosaicMessage::PoseWithCovarianceStampedCallback()
230 {
231  geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
232  // Filling in the pose data
233  msg->pose.pose.orientation = parsing_utilities::ToQuaternion(static_cast<double>(last_atteuler_.Heading), static_cast<double>(last_atteuler_.Pitch), static_cast<double>(last_atteuler_.Roll));
234  msg->pose.pose.position.x = static_cast<double>(last_pvtgeodetic_.Longitude)*360/(2*boost::math::constants::pi<double>());
235  msg->pose.pose.position.y = static_cast<double>(last_pvtgeodetic_.Latitude)*360/(2*boost::math::constants::pi<double>());
236  msg->pose.pose.position.z = static_cast<double>(last_pvtgeodetic_.Height);
237  // Filling in the covariance data in row-major order
238  msg->pose.covariance[0] = static_cast<double>(last_poscovgeodetic_.Cov_lonlon);
239  msg->pose.covariance[1] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
240  msg->pose.covariance[2] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
241  msg->pose.covariance[3] = 0;
242  msg->pose.covariance[4] = 0;
243  msg->pose.covariance[5] = 0;
244  msg->pose.covariance[6] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
245  msg->pose.covariance[7] = static_cast<double>(last_poscovgeodetic_.Cov_latlat);
246  msg->pose.covariance[8] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
247  msg->pose.covariance[9] = 0;
248  msg->pose.covariance[10] = 0;
249  msg->pose.covariance[11] = 0;
250  msg->pose.covariance[12] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
251  msg->pose.covariance[13] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
252  msg->pose.covariance[14] = static_cast<double>(last_poscovgeodetic_.Cov_hgthgt);
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;
259  msg->pose.covariance[21] = static_cast<double>(last_attcoveuler_.Cov_RollRoll);
260  msg->pose.covariance[22] = static_cast<double>(last_attcoveuler_.Cov_PitchRoll);
261  msg->pose.covariance[23] = static_cast<double>(last_attcoveuler_.Cov_HeadRoll);
262  msg->pose.covariance[24] = 0;
263  msg->pose.covariance[25] = 0;
264  msg->pose.covariance[26] = 0;
265  msg->pose.covariance[27] = static_cast<double>(last_attcoveuler_.Cov_PitchRoll);
266  msg->pose.covariance[28] = static_cast<double>(last_attcoveuler_.Cov_PitchPitch);
267  msg->pose.covariance[29] = static_cast<double>(last_attcoveuler_.Cov_HeadPitch);
268  msg->pose.covariance[30] = 0;
269  msg->pose.covariance[31] = 0;
270  msg->pose.covariance[32] = 0;
271  msg->pose.covariance[33] = static_cast<double>(last_attcoveuler_.Cov_HeadRoll);
272  msg->pose.covariance[34] = static_cast<double>(last_attcoveuler_.Cov_PitchRoll);
273  msg->pose.covariance[35] = static_cast<double>(last_attcoveuler_.Cov_HeadHead);
274 
275  return msg;
276 }
277 
284 {
285  sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
286  uint16_t mask = 15; // We extract the first four bits using this mask.
287  uint16_t type_of_pvt = ((uint16_t) (last_pvtgeodetic_.Mode)) & mask;
288  switch(type_of_pvt)
289  {
290  case 0:
291  {
292  msg->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
293  break;
294  }
295  case 1: case 3:
296  {
297  msg->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
298  break;
299  }
300  case 2: case 4: case 5: case 7: case 8: case 10:
301  {
302  msg->status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
303  break;
304  }
305  case 6:
306  {
307  msg->status.status = sensor_msgs::NavSatStatus::STATUS_SBAS_FIX;
308  break;
309  }
310  default:
311  {
312  throw std::runtime_error("PVTGeodetic's Mode field contains an invalid type of PVT solution.");
313  }
314  }
315  bool gps_in_pvt = false;
316  bool glo_in_pvt = false;
317  bool com_in_pvt = false;
318  bool gal_in_pvt = false;
319  uint32_t mask_2 = 1;
320  for(int bit = 0; bit != 31; ++bit)
321  {
322  bool in_use = last_pvtgeodetic_.SignalInfo & mask_2;
323  if (bit <= 5 && in_use)
324  {
325  gps_in_pvt = true;
326  }
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;
330  mask_2 *= 2;
331  }
332  //ROS_DEBUG("GPS is in use: %s, GLO is in use: %s, COM is in use: %s, GAL is in use: %s", gps_in_pvt ? "true" : "false", glo_in_pvt ? "true" : "false", com_in_pvt ? "true" : "false", gal_in_pvt ? "true" : "false");
333  uint16_t service = gps_in_pvt*1+glo_in_pvt*2+com_in_pvt*4+gal_in_pvt*8; // booleans will be promoted to integers automatically
334  msg->status.service = service;
335  msg->latitude = last_pvtgeodetic_.Latitude*360/(2*boost::math::constants::pi<double>());
336  msg->longitude = last_pvtgeodetic_.Longitude*360/(2*boost::math::constants::pi<double>());
337  msg->altitude = last_pvtgeodetic_.Height;
338  msg->position_covariance[0] = static_cast<double>(last_poscovgeodetic_.Cov_lonlon);
339  msg->position_covariance[1] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
340  msg->position_covariance[2] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
341  msg->position_covariance[3] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
342  msg->position_covariance[4] = static_cast<double>(last_poscovgeodetic_.Cov_latlat);
343  msg->position_covariance[5] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
344  msg->position_covariance[6] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
345  msg->position_covariance[7] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
346  msg->position_covariance[8] = static_cast<double>(last_poscovgeodetic_.Cov_hgthgt);
347  msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
348  return msg;
349 }
350 
366 {
367  gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
368 
369  msg->status.satellites_used = static_cast<uint16_t>(last_pvtgeodetic_.NrSV);
370 
371  // MeasEpoch Processing
372  std::vector<int32_t> cno_tracked;
373  std::vector<int32_t> svid_in_sync;
374  {
375  uint8_t sb1_size = last_measepoch_.SB1Size;
376  uint8_t sb2_size = last_measepoch_.SB2Size;
377  uint8_t *sb_start = &last_measepoch_.Data[0];
378  int32_t index = sb_start - &last_measepoch_.Block_Header.SYNC1;
379  for (int32_t i = 0; i < static_cast<int32_t>(last_measepoch_.N); ++i)
380  {
381  // Define MeasEpochChannelType1 struct for the corresponding sub-block
382  MeasEpochChannelType1 *measepoch_channel_type1 = reinterpret_cast<MeasEpochChannelType1*>(&last_measepoch_.Block_Header.SYNC1 + index);
383  svid_in_sync.push_back(static_cast<int32_t>(measepoch_channel_type1->SVID));
384  uint8_t type_mask = 15; // We extract the first four bits using this mask.
385  if (measepoch_channel_type1->Type & type_mask == static_cast<uint8_t>(1) || measepoch_channel_type1->Type & type_mask == static_cast<uint8_t>(2))
386  {
387  cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->CN0)/4);
388  }
389  else
390  {
391  cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->CN0)/4+static_cast<int32_t>(10));
392  }
393  //ROS_DEBUG("static_cast<int32_t>(measepoch_channel_type1->SVID)) is %i", static_cast<int32_t>(measepoch_channel_type1->SVID));
394  index += sb1_size;
395  for (int32_t j = 0; j < static_cast<int32_t>(measepoch_channel_type1->N_Type2); j++)
396  {
397  index += sb2_size;
398  }
399  }
400  }
401 
402  // ChannelStatus Processing
403  std::vector<int32_t> svid_in_sync_2;
404  std::vector<int32_t> elevation_tracked;
405  std::vector<int32_t> azimuth_tracked;
406  std::vector<int32_t> svid_pvt;
407  svid_pvt.clear();
408  std::vector<int32_t> ordering;
409  {
410  uint8_t sb1_size = last_channelstatus_.SB1Size;
411  uint8_t sb2_size = last_channelstatus_.SB2Size;
412  uint8_t *sb_start = &last_channelstatus_.Data[0];
413  int32_t index = sb_start - &last_channelstatus_.Block_Header.SYNC1;
414  //ROS_DEBUG("index is %i", index); // yields 20, as expected
415 
416  uint16_t azimuth_mask = 511;
417  for (int32_t i = 0; i < static_cast<int32_t>(last_channelstatus_.N); i++)
418  {
419  // Define ChannelSatInfo struct for the corresponding sub-block
420  ChannelSatInfo *channel_sat_info = reinterpret_cast<ChannelSatInfo*>(&last_channelstatus_.Block_Header.SYNC1 + index);
421  bool to_be_added = false;
422  for (int32_t j = 0; j < static_cast<int32_t>(svid_in_sync.size()); ++j)
423  {
424  if (svid_in_sync[j] == static_cast<int32_t>(channel_sat_info->SVID))
425  {
426  ordering.push_back(j);
427  to_be_added = true;
428  break;
429  }
430  }
431  if (to_be_added)
432  {
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)));
436  }
437  index += sb1_size;
438  for (int32_t j = 0; j < static_cast<int32_t>(channel_sat_info->N2); j++)
439  {
440  // Define ChannelStateInfo struct for the corresponding sub-block
441  ChannelStateInfo *channel_state_info = reinterpret_cast<ChannelStateInfo*>(&last_channelstatus_.Block_Header.SYNC1 + index);
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)
445  {
446  uint16_t pvt_status_value = (channel_state_info->PVTStatus & pvt_status_mask) >> k-1;
447  //ROS_DEBUG("value is %u and channel_state_info->PVTStatus is %u", pvt_status_value, channel_state_info->PVTStatus);
448  if (pvt_status_value == 2)
449  {
450  pvt_status = true;
451  }
452  if (k > 1)
453  {
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);
455  }
456  }
457  if (pvt_status)
458  {
459  svid_pvt.push_back(static_cast<int32_t>(channel_sat_info->SVID));
460  }
461  index += sb2_size;
462  }
463  }
464  }
465  msg->status.satellite_used_prn = svid_pvt; // Entries such as int32[] in ROS messages are to be treated as std::vectors.
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;
470 
471  // Reordering CNO vector to that of all previous arrays
472  std::vector<int32_t> cno_tracked_reordered;
473  //ROS_DEBUG("size svid_in_sync is %i, size of cno_tracked is %i, size of svid_in_sync_2 is %i", static_cast<int32_t>(svid_in_sync.size()), static_cast<int32_t>(cno_tracked.size()), static_cast<int32_t>(svid_in_sync_2.size()));
474  if (static_cast<int32_t>(last_channelstatus_.N) != 0)
475  {
476  for (int32_t k = 0; k < static_cast<int32_t>(ordering.size()); ++k)
477  {
478  cno_tracked_reordered.push_back(cno_tracked[ordering[k]]);
479  }
480  }
481  msg->status.satellite_visible_snr = cno_tracked_reordered;
482 
483  // PVT Status Analysis
484  uint16_t status_mask = 15; // We extract the first four bits using this mask.
485  uint16_t type_of_pvt = ((uint16_t) (last_pvtgeodetic_.Mode)) & status_mask;
486  switch(type_of_pvt)
487  {
488  case 0:
489  {
490  msg->status.status = gps_common::GPSStatus::STATUS_NO_FIX;
491  break;
492  }
493  case 1: case 3:
494  {
495  msg->status.status = gps_common::GPSStatus::STATUS_FIX;
496  break;
497  }
498  case 2: case 4: case 5: case 7: case 8: case 10:
499  {
500  msg->status.status = gps_common::GPSStatus::STATUS_GBAS_FIX;
501  break;
502  }
503  case 6:
504  {
505  uint16_t reference_id = last_pvtgeodetic_.ReferenceID;
506  if (reference_id == 131 || reference_id == 133 || reference_id == 135 || reference_id == 135) // PRNs of the 4 WAAS satellites
507  {
508  msg->status.status = gps_common::GPSStatus::STATUS_WAAS_FIX;
509  }
510  else
511  {
512  msg->status.status = gps_common::GPSStatus::STATUS_SBAS_FIX;
513  }
514  break;
515  }
516  default:
517  {
518  throw std::runtime_error("PVTGeodetic's Mode field contains an invalid type of PVT solution.");
519  }
520  }
521  msg->status.motion_source = gps_common::GPSStatus::SOURCE_POINTS; // Doppler is not used when calculating the velocities of mosaic.
522  msg->status.orientation_source = gps_common::GPSStatus::SOURCE_POINTS; // Doppler is not used when calculating the orientation of mosaic.
523  msg->status.position_source = gps_common::GPSStatus::SOURCE_GPS;
524  msg->latitude = static_cast<double>(last_pvtgeodetic_.Latitude)*360/(2*boost::math::constants::pi<double>());
525  msg->longitude = static_cast<double>(last_pvtgeodetic_.Longitude)*360/(2*boost::math::constants::pi<double>());
526  msg->altitude = static_cast<double>(last_pvtgeodetic_.Height);
527  msg->track = static_cast<double>(last_pvtgeodetic_.COG); // Note that COG is of type float32 while track is of type float64.
528  msg->speed = std::sqrt(std::pow(static_cast<double>(last_pvtgeodetic_.Vn), 2) + std::pow(static_cast<double>(last_pvtgeodetic_.Ve), 2));
529  msg->climb = static_cast<double>(last_pvtgeodetic_.Vu);
530  msg->pitch = static_cast<double>(last_atteuler_.Pitch);
531  msg->roll = static_cast<double>(last_atteuler_.Roll);
532  if (last_dop_.PDOP == static_cast<uint16_t>(0) || last_dop_.TDOP == static_cast<uint16_t>(0))
533  {
534  msg->gdop = static_cast<double>(-1);
535  }
536  else
537  {
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));
539  }
540  if (last_dop_.PDOP == static_cast<uint16_t>(0))
541  {
542  msg->pdop = static_cast<double>(-1);
543  }
544  else
545  {
546  msg->pdop = static_cast<double>(last_dop_.PDOP)/100;
547  }
548  if (last_dop_.HDOP == static_cast<uint16_t>(0))
549  {
550  msg->hdop = static_cast<double>(-1);
551  }
552  else
553  {
554  msg->hdop = static_cast<double>(last_dop_.HDOP)/100;
555  }
556  if (last_dop_.VDOP == static_cast<uint16_t>(0))
557  {
558  msg->vdop = static_cast<double>(-1);
559  }
560  else
561  {
562  msg->vdop = static_cast<double>(last_dop_.VDOP)/100;
563  }
564  if (last_dop_.TDOP == static_cast<uint16_t>(0))
565  {
566  msg->tdop = static_cast<double>(-1);
567  }
568  else
569  {
570  msg->tdop = static_cast<double>(last_dop_.TDOP)/100;
571  }
572  msg->time = static_cast<double>(last_pvtgeodetic_.TOW)/1000 + static_cast<double>(last_pvtgeodetic_.WNc*7*24*60*60);
573  msg->err = 2*(std::sqrt(static_cast<double>(last_poscovgeodetic_.Cov_latlat) + static_cast<double>(last_poscovgeodetic_.Cov_lonlon) + static_cast<double>(last_poscovgeodetic_.Cov_hgthgt)));
574  msg->err_horz = 2*(std::sqrt(static_cast<double>(last_poscovgeodetic_.Cov_latlat) + static_cast<double>(last_poscovgeodetic_.Cov_lonlon)));
575  msg->err_vert = 2*std::sqrt(static_cast<double>(last_poscovgeodetic_.Cov_hgthgt));
576  msg->err_track = 2*(std::sqrt(std::pow(static_cast<double>(1)/(static_cast<double>(last_pvtgeodetic_.Vn)+std::pow(static_cast<double>(last_pvtgeodetic_.Ve),2)/static_cast<double>(last_pvtgeodetic_.Vn)),2)*static_cast<double>(last_poscovgeodetic_.Cov_lonlon)+std::pow((static_cast<double>(last_pvtgeodetic_.Ve))/(std::pow(static_cast<double>(last_pvtgeodetic_.Vn),2)+std::pow(static_cast<double>(last_pvtgeodetic_.Ve),2)),2)*static_cast<double>(last_poscovgeodetic_.Cov_latlat)));
577  msg->err_speed = 2*(std::sqrt(static_cast<double>(last_velcovgeodetic_.Cov_VnVn) + static_cast<double>(last_velcovgeodetic_.Cov_VeVe)));
578  msg->err_climb = 2*std::sqrt(static_cast<double>(last_velcovgeodetic_.Cov_VuVu));
579  msg->err_time = 2*std::sqrt(static_cast<double>(last_poscovgeodetic_.Cov_bb));
580  msg->err_pitch = 2*std::sqrt(static_cast<double>(last_attcoveuler_.Cov_PitchPitch));
581  msg->err_roll = 2*std::sqrt(static_cast<double>(last_attcoveuler_.Cov_RollRoll));
582  msg->position_covariance[0] = static_cast<double>(last_poscovgeodetic_.Cov_lonlon);
583  msg->position_covariance[1] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
584  msg->position_covariance[2] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
585  msg->position_covariance[3] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
586  msg->position_covariance[4] = static_cast<double>(last_poscovgeodetic_.Cov_latlat);
587  msg->position_covariance[5] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
588  msg->position_covariance[6] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
589  msg->position_covariance[7] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
590  msg->position_covariance[8] = static_cast<double>(last_poscovgeodetic_.Cov_hgthgt);
591  msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
592 
593  return msg;
594 }
595 
599 ros::Time io_comm_mosaic::TimestampSBF(uint32_t TOW, bool use_GNSS)
600 {
601  if (use_GNSS)
602  {
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; // hundredths of a second
607  if (ss >= leap_seconds)
608  {
609  ss = ss - leap_seconds;
610  }
611  else
612  {
613  if (mm >= 1)
614  {
615  --mm;
616  ss = 60 - (leap_seconds - ss);
617  }
618  else
619  {
620  if (hh >= 1)
621  {
622  --hh;
623  mm = 59;
624  ss = 60 - (leap_seconds - ss);
625  }
626  else
627  {
628  hh = 23;
629  mm = 59;
630  ss = 60 - (leap_seconds - ss);
631  }
632  }
633  }
634  boost::format fmt = boost::format("%02u%02u%02u.%02u") % hh % mm % ss % hs;
635  std::string utc_string = fmt.str();
636  //ROS_DEBUG("UTC string is %s", utc_string.c_str());
637  double utc_double;
638  string_utilities::ToDouble(utc_string, utc_double);
639  time_t unix_time_seconds = parsing_utilities::UTCtoUnix(utc_double); // This only deals with full seconds.
640  uint32_t unix_time_nanoseconds = (static_cast<uint32_t>(utc_double*100)%100)*10000000; // This works since there are two digits after the decimal point in the utc_double.
641  ros::Time time_obj(unix_time_seconds, unix_time_nanoseconds);
642  return time_obj;
643  }
644  else
645  {
646  time_t time_now = time(NULL);
647  ros::Time time_obj((uint32_t) time_now, 0);
648  return time_obj;
649  }
650 }
651 
653 {
654  if (found_) return true;
655 
656  // Verify header bytes
657  if (!this->IsSBF() && !this->IsNMEA() && !this->IsResponse() && !(read_cd && this->IsConnectionDescriptor()))
658  {
659  return false;
660  }
661 
662  found_ = true;
663  return true;
664 }
665 
667 {
668  if (found_)
669  {
670  Next();
671  }
672  // Search for message or a response header
673  for( ; count_ > 0; --count_, ++data_)
674  {
675  if (this->IsSBF() || this->IsNMEA() || this->IsResponse() || (read_cd && this->IsConnectionDescriptor()))
676  {
677  break;
678  }
679  }
680  found_ = true;
681  return data_;
682 }
683 
685 {
686  uint16_t pos = 0;
687  segment_size_ = 0;
688  uint32_t count_copy = count_;
689  if (this->IsResponse())
690  {
691  do
692  {
693  ++segment_size_;
694  ++pos;
695  --count_copy;
696  if (count_copy == 0) break;
697  } while(!((data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED)) || (data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20 && data_[pos+3] == 0x20 && data_[pos+4] == 0x4E) || (data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20 && data_[pos+3] == 0x20 && data_[pos+4] == 0x53) || (data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20 && data_[pos+3] == 0x20 && data_[pos+4] == 0x52));
698  }
699  else
700  {
701  do
702  {
703  ++segment_size_;
704  ++pos;
705  --count_copy;
706  if (count_copy == 0) break;
707  } while(!((data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED) || data_[pos] == CARRIAGE_RETURN || data_[pos] == LINE_FEED));
708  }
709  return segment_size_;
710 }
711 
713 {
714  if (this->IsSBF())
715  {
716  uint16_t mask = 8191;
717  if (*(reinterpret_cast<const uint16_t *>(data_ + 4)) & mask == static_cast<const uint16_t>(ID))
718  // Caution: reinterpret_cast is the most dangerous cast, It's used primarily for particularly weird conversions and bit manipulations, like turning a raw data stream into actual data
719  {
720  return true;
721  }
722  else
723  {
724  return false;
725  }
726  }
727  else
728  {
729  return false;
730  }
731 }
732 
733 
735 {
736  if (this->IsNMEA())
737  {
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)
744  {
745  return true;
746  }
747  else
748  {
749  return false;
750  }
751  }
752  else
753  {
754  return false;
755  }
756 }
757 
759 {
760  if (data_[0] == SBF_SYNC_BYTE_1 && data_[1] == SBF_SYNC_BYTE_2)
761  {
762  return true;
763  }
764  else
765  {
766  return false;
767  }
768 }
769 
771 {
773  {
774  return true;
775  }
776  else
777  {
778  return false;
779  }
780 }
781 
783 {
785  {
786  return true;
787  }
788  else
789  {
790  return false;
791  }
792 }
793 
795 {
797  {
798  return true;
799  }
800  else
801  {
802  return false;
803  }
804 }
805 
807 {
809  {
810  return true;
811  }
812  else
813  {
814  return false;
815  }
816 }
817 
819 {
820  if (this->IsSBF())
821  {
822  // Define bit mask:
823  uint16_t mask = 8191; // It is not as stated in the firmware: !first! three bits are for revision (not last 3), and rest for block number
824  uint16_t value = (*(reinterpret_cast<const uint16_t*>(data_+4))) & mask; // Bitwise AND gives us all but first 3 bits set to zero, rest unchanged
825  std::stringstream ss;
826  ss << value;
827  return ss.str();
828  }
829  if (this->IsNMEA())
830  {
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();
837  }
838  return std::string(); // less CPU work than return "";
839 }
840 
841 
842 
844 {
845  return data_;
846 }
847 
849 {
850  return data_ + count_;
851 }
852 
854 {
855  if (this->IsSBF())
856  {
857  uint16_t block_length;
858  // Note that static_cast<uint16_t>(data_[6]) would just take the one byte "data_[6]" and cast it as requested, !neglecting! the byte "data_[7]".
859  block_length = *(reinterpret_cast<const uint16_t*>(data_ + 6));
860  return block_length;
861  }
862  else
863  {
864  return 0;
865  }
866 }
867 
873 {
874  if (Found())
875  {
876  if (this->IsNMEA() || this->IsResponse() || (read_cd && this->IsConnectionDescriptor()))
877  {
878  if (read_cd && this->IsConnectionDescriptor() && cd_count == 2)
879  {
880  read_cd = false;
881  }
882  --count_;
883  ++data_;
884  found_ = false;
885  return data_;
886  }
887  if (this->IsSBF())
888  {
889  uint32_t jump_size;
890  if (CRCcheck_)
891  {
892  jump_size = this->BlockLength();
893  if (jump_size == 0) jump_size = 1; // Some corrupted messages that survive the CRC check (this happened) could tell ROSaic their size is 0,
894  // which would lead to an endless while loop in the ReadCallback() method of the CallbackHandlers class.
895  }
896  else
897  {
898  jump_size = 1;
899  }
900  //ROS_DEBUG("Jump about to happen with jump size %u", jump_size);
901  data_ += jump_size; count_ -= jump_size;
902  found_ = false;
903  return data_;
904  }
905  }
906  found_ = false;
907  --count_;
908  ++data_;
909  return data_;
910 }
float Cov_PitchRoll
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
float Undulation
Struct for the SBF block "AttCovEuler".
uint32_t TOW
uint8_t WACorrInfo
uint16_t TDOP
uint8_t WACorrInfo
const uint8_t * Next()
Goes to the start of the next message based on the calculated length of current message.
uint16_t WNc
#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.
uint16_t MeanCorrAge
bool found_
Whether or not a message has been found.
uint8_t AlertFlag
BlockHeader_t Block_Header
float Cov_HeadPitch
BlockHeader_t Block_Header
std::string MessageID()
Returns the MessageID of the message where data_ is pointing at at the moment, SBF identifiers embell...
double RxClkBias
uint8_t N
const uint8_t * Search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
uint16_t Az_RiseSet
uint8_t AlertFlag
Defines a class that can deal with a buffer of size bytes_transferred that is handed over from async_...
uint8_t TimeSystem
rosaic::AttCovEulerPtr AttCovEulerCallback(AttCovEuler &data)
Callback function when reading AttCovEuler blocks.
uint8_t Error
static DOP last_dop_
Since GPSFix etc. need DOP, incoming DOP blocks need to be stored.
uint32_t TOW
uint8_t Error
float RollDot
#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.
uint16_t CRC
the Check Sum !
uint16_t WNc
#define CONNECTION_DESCRIPTOR_BYTE_1
0x49 is ASCII for I - 1st character of connection descriptor sent by mosaic after initiating TCP conn...
float Cov_HeadRoll
uint16_t BlockLength()
Gets the length of the SBF block.
uint16_t HAccuracy
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&#39;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.
uint16_t Length
length of the entire message including the header. A multiple of 4 between 8 and 4096 ...
std::vector< int32_t > svid_pvt
float Roll
uint16_t VAccuracy
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.
uint16_t PDOP
float Cov_HeadHead
float Heading
uint16_t PPPInfo
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".
uint32_t TOW
uint8_t NrBases
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
uint16_t VDOP
Struct for the SBF block "AttEuler".
uint8_t NrBases
uint8_t NrSV
uint8_t Data[(80+1) *sizeof(ChannelSatInfo)+((80+1) *4) *sizeof(ChannelStateInfo)]
const uint8_t * Pos()
Gets the current position in the read buffer.
double Longitude
uint16_t ReferenceID
#define NMEA_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
uint32_t SignalInfo
float Pitch
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...
uint16_t ID
The ID is the "Block ID".
double Latitude
float RxClkDrift
float HeadingDot
Struct for the SBF sub-block "ChannelStateInfo".
BlockHeader_t Block_Header
uint16_t Latency
#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.
uint8_t SYNC1
first sync byte is $ or 0x24
uint16_t WNc
#define SBF_SYNC_BYTE_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
Struct for the SBF block "PosCovCartesian".
uint8_t TimeSystem
sensor_msgs::NavSatFixPtr NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
rosaic::PVTCartesianPtr PVTCartesianCallback(PVTCartesian &data)
Callback function when reading PVTCartesian blocks.
uint16_t MeanCorrAge
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
float PitchDot
uint8_t NrSV
#define CARRIAGE_RETURN
0x0D is ASCII for "Carriage Return", i.e. "Enter"
uint16_t HAccuracy
uint8_t SB1Size
uint16_t ReferenceID
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".
uint16_t Mode
uint8_t Misc
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-...
uint32_t TOW
float Cov_RollRoll
uint8_t Datum
BlockHeader_t Block_Header
uint16_t HDOP
uint32_t SignalInfo
uint16_t WNc
uint16_t PPPInfo
uint8_t Error
uint8_t SB2Size
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.
float Cov_PitchPitch
uint8_t SYNC2
2nd sync byte is @ or 0x40
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
uint16_t Latency
double RxClkBias
BlockHeader_t Block_Header
#define NMEA_SYNC_BYTE_2_2
0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message
uint8_t Mode
bool IsMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.
uint16_t VAccuracy