bestxyz.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 
30 #include <novatel_gps_driver/parsers/bestxyz.h>
31 
32 #include <novatel_gps_driver/parsers/header.h>
33 
34 #include <boost/make_shared.hpp>
35 
36 namespace novatel_gps_driver
37 {
38  const std::string BestxyzParser::MESSAGE_NAME = "BESTXYZ";
39 
40  uint32_t BestxyzParser::GetMessageId() const
41  {
42  return MESSAGE_ID;
43  }
44 
45  const std::string BestxyzParser::GetMessageName() const
46  {
47  return MESSAGE_NAME;
48  }
49 
50  novatel_gps_msgs::NovatelXYZPtr BestxyzParser::ParseBinary(const BinaryMessage& bin_msg) noexcept(false)
51  {
52  if (bin_msg.data_.size() != BINARY_LENGTH)
53  {
54  std::stringstream error;
55  error << "Unexpected BESTXYZ message length: " << bin_msg.data_.size();
56  throw ParseException(error.str());
57  }
58  novatel_gps_msgs::NovatelXYZPtr ros_msg =
59  boost::make_shared<novatel_gps_msgs::NovatelXYZ>();
60  HeaderParser header_parser;
61  ros_msg->novatel_msg_header = header_parser.ParseBinary(bin_msg);
62  ros_msg->novatel_msg_header.message_name = MESSAGE_NAME;
63 
64  uint16_t solution_status = ParseUInt16(&bin_msg.data_[0]);
65  if (solution_status > MAX_SOLUTION_STATUS)
66  {
67  std::stringstream error;
68  error << "Unknown solution status: " << solution_status;
69  throw ParseException(error.str());
70  }
71  ros_msg->solution_status = SOLUTION_STATUSES[solution_status];
72 
73  uint16_t pos_type = ParseUInt16(&bin_msg.data_[4]);
74  if (pos_type > MAX_POSITION_TYPE)
75  {
76  std::stringstream error;
77  error << "Unknown position type: " << pos_type;
78  throw ParseException(error.str());
79  }
80  ros_msg->position_type = POSITION_TYPES[pos_type];
81 
82  ros_msg->x = ParseDouble(&bin_msg.data_[8]);
83  ros_msg->y = ParseDouble(&bin_msg.data_[16]);
84  ros_msg->z = ParseDouble(&bin_msg.data_[24]);
85 
86  ros_msg->x_sigma = ParseFloat(&bin_msg.data_[32]);
87  ros_msg->y_sigma = ParseFloat(&bin_msg.data_[36]);
88  ros_msg->z_sigma = ParseFloat(&bin_msg.data_[40]);
89 
90  uint16_t vel_solution_status = ParseUInt16(&bin_msg.data_[44]);
91  if (vel_solution_status > MAX_SOLUTION_STATUS)
92  {
93  std::stringstream error;
94  error << "Unknown solution status: " << vel_solution_status;
95  throw ParseException(error.str());
96  }
97  ros_msg->velocity_solution_status = SOLUTION_STATUSES[vel_solution_status];
98 
99  uint16_t vel_type = ParseUInt16(&bin_msg.data_[48]);
100  if (vel_type > MAX_POSITION_TYPE) // Position types array includes velocity types
101  {
102  std::stringstream error;
103  error << "Unknown position type: " << vel_type;
104  throw ParseException(error.str());
105  }
106  ros_msg->velocity_type = POSITION_TYPES[vel_type];
107 
108  ros_msg->x_vel = ParseDouble(&bin_msg.data_[52]);
109  ros_msg->y_vel = ParseDouble(&bin_msg.data_[60]);
110  ros_msg->z_vel = ParseDouble(&bin_msg.data_[68]);
111 
112  ros_msg->x_vel_sigma = ParseFloat(&bin_msg.data_[76]);
113  ros_msg->y_vel_sigma = ParseFloat(&bin_msg.data_[80]);
114  ros_msg->z_vel_sigma = ParseFloat(&bin_msg.data_[84]);
115 
116  ros_msg->base_station_id.resize(4);
117  std::copy(&bin_msg.data_[88], &bin_msg.data_[92], &ros_msg->base_station_id[0]);
118 
119  ros_msg->velocity_latency = ParseFloat(&bin_msg.data_[92]);
120 
121  ros_msg->diff_age = ParseFloat(&bin_msg.data_[96]);
122  ros_msg->solution_age = ParseFloat(&bin_msg.data_[100]);
123 
124  ros_msg->num_satellites_tracked = bin_msg.data_[104];
125  ros_msg->num_satellites_used_in_solution = bin_msg.data_[105];
126  ros_msg->num_gps_and_glonass_l1_used_in_solution = bin_msg.data_[106];
127  ros_msg->num_gps_and_glonass_l1_and_l2_used_in_solution = bin_msg.data_[107];
128  // Byte 108 is reserved
129  GetExtendedSolutionStatusMessage(bin_msg.data_[109],
130  ros_msg->extended_solution_status);
131  // Byte 110 is reserved
132  GetSignalsUsed(bin_msg.data_[111], ros_msg->signal_mask);
133 
134  return ros_msg;
135  }
136 
137  novatel_gps_msgs::NovatelXYZPtr BestxyzParser::ParseAscii(const NovatelSentence& sentence) noexcept(false)
138  {
139  novatel_gps_msgs::NovatelXYZPtr msg =
140  boost::make_shared<novatel_gps_msgs::NovatelXYZ>();
141  HeaderParser h_parser;
142  msg->novatel_msg_header = h_parser.ParseAscii(sentence);
143 
144  if (sentence.body.size() != ASCII_LENGTH)
145  {
146  std::stringstream error;
147  error << "Unexpected number of BESTXYZ message fields: " << sentence.body.size();
148  throw ParseException(error.str());
149  }
150 
151  bool valid = true;
152 
153  msg->solution_status = sentence.body[0];
154  msg->position_type = sentence.body[1];
155 
156  valid = valid && ParseDouble(sentence.body[2], msg->x);
157  valid = valid && ParseDouble(sentence.body[3], msg->y);
158  valid = valid && ParseDouble(sentence.body[4], msg->z);
159 
160  valid = valid && ParseFloat(sentence.body[5], msg->x_sigma);
161  valid = valid && ParseFloat(sentence.body[6], msg->y_sigma);
162  valid = valid && ParseFloat(sentence.body[7], msg->z_sigma);
163 
164  msg->velocity_solution_status = sentence.body[8];
165  msg->velocity_type = sentence.body[9];
166 
167  valid = valid && ParseDouble(sentence.body[10], msg->x_vel);
168  valid = valid && ParseDouble(sentence.body[11], msg->y_vel);
169  valid = valid && ParseDouble(sentence.body[12], msg->z_vel);
170 
171  valid = valid && ParseFloat(sentence.body[13], msg->x_vel_sigma);
172  valid = valid && ParseFloat(sentence.body[14], msg->y_vel_sigma);
173  valid = valid && ParseFloat(sentence.body[15], msg->z_vel_sigma);
174 
175  msg->base_station_id = sentence.body[16];
176  valid = valid && ParseFloat(sentence.body[17], msg->velocity_latency);
177 
178  valid = valid && ParseFloat(sentence.body[18], msg->diff_age);
179  valid = valid && ParseFloat(sentence.body[19], msg->solution_age);
180  valid = valid && ParseUInt8(sentence.body[20], msg->num_satellites_tracked);
181  valid = valid && ParseUInt8(sentence.body[21], msg->num_satellites_used_in_solution);
182  valid = valid && ParseUInt8(sentence.body[22], msg->num_gps_and_glonass_l1_used_in_solution);
183  valid = valid && ParseUInt8(sentence.body[23], msg->num_gps_and_glonass_l1_and_l2_used_in_solution);
184 
185  // skip reserved field
186  uint32_t extended_solution_status = 0;
187  valid = valid && ParseUInt32(sentence.body[25], extended_solution_status, 16);
188  GetExtendedSolutionStatusMessage(
189  extended_solution_status, msg->extended_solution_status);
190 
191  // skip reserved field
192  uint32_t signal_mask = 0;
193  valid = valid && ParseUInt32(sentence.body[27], signal_mask, 16);
194  GetSignalsUsed(signal_mask, msg->signal_mask);
195 
196  if (!valid)
197  {
198  throw ParseException("Invalid field in BESTXYZ message");
199  }
200 
201  return msg;
202  }
203 }
novatel_gps_driver::ParseFloat
float ParseFloat(const uint8_t *buffer)
Converts a buffer containing 4 bytes into a float.
Definition: parsing_utils.cpp:123
novatel_gps_driver::HeaderParser::ParseAscii
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
novatel_gps_driver::ParseUInt8
bool ParseUInt8(const std::string &string, uint8_t &value, int32_t base=10)
Parses a string containing an integer number into a uint16_t.
Definition: parsing_utils.cpp:186
novatel_gps_driver::BestxyzParser::ParseBinary
novatel_gps_msgs::NovatelXYZPtr ParseBinary(const BinaryMessage &bin_msg) noexcept(false) override
Converts bin_msg into a ROS message pointer and returns it.
Definition: bestxyz.cpp:50
novatel_gps_driver::BestxyzParser::MESSAGE_NAME
static const std::string MESSAGE_NAME
Definition: bestxyz.h:54
novatel_gps_driver::MAX_POSITION_TYPE
const size_t MAX_POSITION_TYPE
Definition: parsing_utils.h:56
msg
msg
novatel_gps_driver::HeaderParser::ParseBinary
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
novatel_gps_driver::ParseException
Definition: parse_exception.h:40
novatel_gps_driver::GetSignalsUsed
void GetSignalsUsed(uint32_t mask, novatel_gps_msgs::NovatelSignalMask &msg)
Definition: parsing_utils.cpp:101
novatel_gps_driver::BestxyzParser::GetMessageName
const std::string GetMessageName() const override
Definition: bestxyz.cpp:45
novatel_gps_driver::ParseUInt16
uint16_t ParseUInt16(const uint8_t *buffer)
Converts a buffer containing 2 bytes into an unsigned 16-bit int.
Definition: parsing_utils.cpp:204
novatel_gps_driver::NovatelSentence
Definition: novatel_sentence.h:42
bestxyz.h
novatel_gps_driver::BinaryMessage
Definition: binary_message.h:42
novatel_gps_driver::ParseUInt32
uint32_t ParseUInt32(const uint8_t *buffer)
Converts a buffer containing 4 bytes into an unsigned 32-bit int.
Definition: parsing_utils.cpp:174
novatel_gps_driver::GetExtendedSolutionStatusMessage
void GetExtendedSolutionStatusMessage(uint32_t status, novatel_gps_msgs::NovatelExtendedSolutionStatus &msg)
Definition: parsing_utils.cpp:68
novatel_gps_driver::POSITION_TYPES
const std::string POSITION_TYPES[]
Definition: parsing_utils.h:57
novatel_gps_driver
Definition: binary_header.h:37
novatel_gps_driver::ParseDouble
double ParseDouble(const uint8_t *buffer)
Converts a buffer containing 8 bytes into a double.
Definition: parsing_utils.cpp:111
novatel_gps_driver::MAX_SOLUTION_STATUS
const size_t MAX_SOLUTION_STATUS
Definition: parsing_utils.h:49
novatel_gps_driver::HeaderParser
Definition: header.h:40
novatel_gps_driver::SOLUTION_STATUSES
const std::string SOLUTION_STATUSES[]
Definition: parsing_utils.h:50
novatel_gps_driver::BestxyzParser::ParseAscii
novatel_gps_msgs::NovatelXYZPtr ParseAscii(const NovatelSentence &sentence) noexcept(false) override
Converts sentence into a ROS message pointer and returns it.
Definition: bestxyz.cpp:137
novatel_gps_driver::BestxyzParser::MESSAGE_ID
static constexpr uint16_t MESSAGE_ID
Definition: bestxyz.h:51
novatel_gps_driver::BestxyzParser::GetMessageId
uint32_t GetMessageId() const override
Definition: bestxyz.cpp:40
header.h

novatel_gps_driver
Author(s):
autogenerated on Sun Oct 1 2023 02:58:57