header.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
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 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * 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 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived 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 SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
31 
32 #include <ros/ros.h>
33 
35 {
36  return 0;
37 }
38 
40 {
41  return "HEADER";
42 }
43 
44 novatel_gps_msgs::NovatelMessageHeader novatel_gps_driver::HeaderParser::ParseBinary(
45  const novatel_gps_driver::BinaryMessage& bin_msg) noexcept(false)
46 {
47  // No point in checking whether the port identifier is valid here, because
48  // the variable's range is 0-255 and this array has 256 values in it.
49  novatel_gps_msgs::NovatelMessageHeader msg;
50  msg.port = PORT_IDENTIFIERS[bin_msg.header_.port_address_];
51  msg.sequence_num = bin_msg.header_.sequence_;
52  msg.percent_idle_time = bin_msg.header_.idle_time_;
53  switch (bin_msg.header_.time_status_)
54  {
55  case 20:
56  msg.gps_time_status = "UNKNOWN";
57  break;
58  case 60:
59  msg.gps_time_status = "APPROXIMATE";
60  break;
61  case 80:
62  msg.gps_time_status = "COARSEADJUSTING";
63  break;
64  case 100:
65  msg.gps_time_status = "COARSE";
66  break;
67  case 120:
68  msg.gps_time_status = "COARSESTEERING";
69  break;
70  case 130:
71  msg.gps_time_status = "FREEWHEELING";
72  break;
73  case 140:
74  msg.gps_time_status = "FINEADJUSTING";
75  break;
76  case 160:
77  msg.gps_time_status = "FINE";
78  break;
79  case 170:
80  msg.gps_time_status = "FINEBACKUPSTEERING";
81  break;
82  case 180:
83  msg.gps_time_status = "FINESTEERING";
84  break;
85  case 200:
86  msg.gps_time_status = "SATTIME";
87  break;
88  default:
89  {
90  std::stringstream error;
91  error << "Unknown GPS time status: " << bin_msg.header_.time_status_;
92  throw ParseException(error.str());
93  }
94  }
95  msg.gps_week_num = bin_msg.header_.week_;
96  msg.gps_seconds = static_cast<double>(bin_msg.header_.gps_ms_) / 1000.0;
97  GetNovatelReceiverStatusMessage(bin_msg.header_.receiver_status_, msg.receiver_status);
98  msg.receiver_software_version = bin_msg.header_.receiver_sw_version_;
99 
100  return msg;
101 }
102 
103 novatel_gps_msgs::NovatelMessageHeader novatel_gps_driver::HeaderParser::ParseAscii(
104  const novatel_gps_driver::NovatelSentence& sentence) noexcept(false)
105 {
106  if (sentence.header.size() != NOVATEL_MESSAGE_HEADER_LENGTH)
107  {
108  std::stringstream error;
109  error <<"Novatel message header size wrong: expected "
111  << ", got %zu"<< sentence.header.size();
112  throw ParseException(error.str());
113  }
114 
115  bool valid = true;
116 
117  novatel_gps_msgs::NovatelMessageHeader msg;
118  msg.message_name = sentence.header[0];
119  msg.port = sentence.header[1];
120  valid = valid && ParseUInt32(sentence.header[2], msg.sequence_num);
121  valid = valid && ParseFloat(sentence.header[3], msg.percent_idle_time);
122  msg.gps_time_status = sentence.header[4];
123  valid = valid && ParseUInt32(sentence.header[5], msg.gps_week_num);
124  valid = valid && ParseDouble(sentence.header[6], msg.gps_seconds);
125 
126  uint32_t receiver_status_code = 0;
127  valid = valid && ParseUInt32(sentence.header[7], receiver_status_code, 16);
128  GetNovatelReceiverStatusMessage(receiver_status_code, msg.receiver_status);
129 
130  valid = valid && ParseUInt32(sentence.header[9], msg.receiver_software_version);
131 
132  if (!valid)
133  {
134  throw ParseException("Header was invalid.");
135  }
136  return msg;
137 }
void GetNovatelReceiverStatusMessage(uint32_t status, novatel_gps_msgs::NovatelReceiverStatus &receiver_status_msg)
msg
const std::string PORT_IDENTIFIERS[]
Definition: parsing_utils.h:87
novatel_gps_msgs::NovatelMessageHeader ParseAscii(const NovatelSentence &sentence) noexcept(false) override
Converts sentence into a ROS message pointer and returns it.
Definition: header.cpp:103
const size_t NOVATEL_MESSAGE_HEADER_LENGTH
Definition: parsing_utils.h:47
double ParseDouble(const uint8_t *buffer)
Converts a buffer containing 8 bytes into a double.
uint32_t ParseUInt32(const uint8_t *buffer)
Converts a buffer containing 4 bytes into an unsigned 32-bit int.
uint32_t GetMessageId() const override
Definition: header.cpp:34
float ParseFloat(const uint8_t *buffer)
Converts a buffer containing 4 bytes into a float.
novatel_gps_msgs::NovatelMessageHeader ParseBinary(const BinaryMessage &bin_msg) noexcept(false) override
Converts bin_msg into a ROS message pointer and returns it.
Definition: header.cpp:44
const std::string GetMessageName() const override
Definition: header.cpp:39


novatel_gps_driver
Author(s):
autogenerated on Thu Jul 16 2020 03:17:30