range.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 
32 
33 #include <boost/make_shared.hpp>
34 
35 const std::string novatel_gps_driver::RangeParser::MESSAGE_NAME = "RANGE";
36 
38 {
39  return MESSAGE_ID;
40 }
41 
43 {
44  return MESSAGE_NAME;
45 }
46 
47 novatel_gps_msgs::RangePtr
49 {
50  uint32_t num_obs = ParseUInt32(&bin_msg.data_[0]);
51  if (bin_msg.data_.size() != (BINARY_OBSERVATION_SIZE * num_obs) + 4)
52  {
53  std::stringstream error;
54  error << "Unexpected range message size: " << bin_msg.data_.size();
55  throw ParseException(error.str());
56  }
57  novatel_gps_msgs::RangePtr ros_msg = boost::make_shared<novatel_gps_msgs::Range>();
58  HeaderParser h_parser;
59  ros_msg->novatel_msg_header = h_parser.ParseBinary(bin_msg);
60  ros_msg->novatel_msg_header.message_name = "RANGE";
61 
62  ros_msg->numb_of_observ = num_obs;
63  ros_msg->info.reserve(num_obs);
64  for(int i = 0; i < num_obs; i++)
65  {
66  size_t obs_offset = 4 + i * BINARY_OBSERVATION_SIZE;
67 
68  novatel_gps_msgs::RangeInformation info;
69 
70  info.prn_number = ParseUInt16(&bin_msg.data_[obs_offset]);
71  info.glofreq = ParseUInt16(&bin_msg.data_[obs_offset+2]);
72  info.psr = ParseDouble(&bin_msg.data_[obs_offset+4]);
73  info.psr_std = ParseFloat(&bin_msg.data_[obs_offset+12]);
74  info.adr = ParseDouble(&bin_msg.data_[obs_offset+16]);
75  info.adr_std = ParseFloat(&bin_msg.data_[obs_offset+24]);
76  info.dopp = ParseFloat(&bin_msg.data_[obs_offset+28]);
77  info.noise_density_ratio = ParseFloat(&bin_msg.data_[obs_offset+32]);
78  info.locktime = ParseFloat(&bin_msg.data_[obs_offset+36]);
79  info.tracking_status = ParseUInt32(&bin_msg.data_[obs_offset+40]);
80 
81  ros_msg->info.push_back(info);
82  }
83  return ros_msg;
84 }
85 
86 novatel_gps_msgs::RangePtr
88 {
89  novatel_gps_msgs::RangePtr msg = boost::make_shared<novatel_gps_msgs::Range>();
90  HeaderParser h_parser;
91  msg->novatel_msg_header = h_parser.ParseAscii(sentence);
92  if (!ParseInt32(sentence.body[0], msg->numb_of_observ, 10))
93  {
94  std::stringstream error;
95  error << "Unable to parse number of observations in RANGE log.";
96  throw ParseException(error.str());
97  }
98  uint32_t numb_of_observ = static_cast<uint32_t>(msg->numb_of_observ);
99  if (sentence.body.size() != 1 + numb_of_observ * ASCII_FIELDS)
100  {
101  std::stringstream error;
102  error << "Did not find expected number of observations in RANGE log.";
103  throw ParseException(error.str());
104  }
105  bool valid = true;
106  valid &= ParseInt32(sentence.body[0], msg->numb_of_observ, 10);
107  msg->info.resize(numb_of_observ);
108  for (int i = 0, index = 0; index < numb_of_observ; i += 10, index++)
109  {
110  valid &= ParseUInt16(sentence.body[i + 1], msg->info[index].prn_number, 10);
111  valid &= ParseUInt16(sentence.body[i + 2], msg->info[index].glofreq, 10);
112  valid &= ParseDouble(sentence.body[i + 3], msg->info[index].psr);
113  valid &= ParseFloat(sentence.body[i + 4], msg->info[index].psr_std);
114  valid &= ParseDouble(sentence.body[i + 5], msg->info[index].adr);
115  valid &= ParseFloat(sentence.body[i + 6], msg->info[index].adr_std);
116  valid &= ParseFloat(sentence.body[i + 7], msg->info[index].dopp);
117  valid &= ParseFloat(sentence.body[i + 8], msg->info[index].noise_density_ratio);
118  valid &= ParseFloat(sentence.body[i + 9], msg->info[index].locktime);
119  std::string track = "0x" + sentence.body[i + 10]; // This number is in hex
120  valid &= ParseUInt32(track, msg->info[index].tracking_status, 16);
121  }
122  if (!valid)
123  {
124  throw ParseException("Error parsing RANGE log.");
125  }
126  return msg;
127 }
static constexpr size_t ASCII_FIELDS
Definition: range.h:51
msg
uint16_t ParseUInt16(const uint8_t *buffer)
Converts a buffer containing 2 bytes into an unsigned 16-bit int.
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
double ParseDouble(const uint8_t *buffer)
Converts a buffer containing 8 bytes into a double.
novatel_gps_msgs::RangePtr ParseAscii(const NovatelSentence &sentence) noexcept(false) override
Converts sentence into a ROS message pointer and returns it.
Definition: range.cpp:87
uint32_t GetMessageId() const override
Definition: range.cpp:37
uint32_t ParseUInt32(const uint8_t *buffer)
Converts a buffer containing 4 bytes into an unsigned 32-bit int.
float ParseFloat(const uint8_t *buffer)
Converts a buffer containing 4 bytes into a float.
int32_t ParseInt32(const uint8_t *buffer)
Converts a buffer containing 4 bytes into a signed 32-bit int.
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: range.cpp:42
static constexpr uint16_t MESSAGE_ID
Definition: range.h:50
static const std::string MESSAGE_NAME
Definition: range.h:52
novatel_gps_msgs::RangePtr ParseBinary(const BinaryMessage &bin_msg) noexcept(false) override
Converts bin_msg into a ROS message pointer and returns it.
Definition: range.cpp:48
static constexpr size_t BINARY_OBSERVATION_SIZE
Definition: range.h:49


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