time.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 #include <boost/make_shared.hpp>
32 
33 const std::string novatel_gps_driver::TimeParser::MESSAGE_NAME = "TIME";
34 
36 {
37  return MESSAGE_ID;
38 }
39 
41 {
42  return MESSAGE_NAME;
43 }
44 
45 novatel_gps_msgs::TimePtr novatel_gps_driver::TimeParser::ParseBinary(const novatel_gps_driver::BinaryMessage& msg) noexcept(false)
46 {
47  if (msg.data_.size() != BINARY_LENGTH)
48  {
49  std::stringstream error;
50  error << "Unexpected time message size: " << msg.data_.size();
51  throw ParseException(error.str());
52  }
53 
54  novatel_gps_msgs::TimePtr ros_msg = boost::make_shared<novatel_gps_msgs::Time>();
55 
56  uint32_t clock_status = ParseUInt32(&msg.data_[0]);
57  switch (clock_status)
58  {
59  case 0:
60  ros_msg->clock_status = "VALID";
61  break;
62  case 1:
63  ros_msg->clock_status = "CONVERGING";
64  break;
65  case 2:
66  ros_msg->clock_status = "ITERATING";
67  break;
68  case 3:
69  ros_msg->clock_status = "INVALID";
70  break;
71  default:
72  {
73  std::stringstream error;
74  error << "Unexpected clock status: " << clock_status;
75  throw ParseException(error.str());
76  }
77  }
78  ros_msg->offset = ParseDouble(&msg.data_[4]);
79  ros_msg->offset_std = ParseDouble(&msg.data_[12]);
80  ros_msg->utc_offset = ParseDouble(&msg.data_[20]);
81  ros_msg->utc_year = ParseUInt32(&msg.data_[28]);
82  ros_msg->utc_month = msg.data_[32];
83  ros_msg->utc_day = msg.data_[33];
84  ros_msg->utc_hour = msg.data_[34];
85  ros_msg->utc_minute = msg.data_[35];
86  ros_msg->utc_millisecond = ParseUInt32(&msg.data_[36]);
87  uint32_t utc_status = ParseUInt32(&msg.data_[40]);
88  switch (utc_status)
89  {
90  case 0:
91  ros_msg->utc_status = "Invalid";
92  break;
93  case 1:
94  ros_msg->utc_status = "Valid";
95  break;
96  case 2:
97  ros_msg->utc_status = "Warning";
98  break;
99  default:
100  {
101  std::stringstream error;
102  error << "Unexpected UTC status: " << utc_status;
103  throw ParseException(error.str());
104  }
105  }
106 
107  return ros_msg;
108 }
109 
110 novatel_gps_msgs::TimePtr
112 {
113  novatel_gps_msgs::TimePtr msg = boost::make_shared<novatel_gps_msgs::Time>();
114  if (sentence.body.size() != ASCII_FIELD)
115  {
116  std::stringstream error;
117  error << "Unexpected number of fields in TIME log: " << sentence.body.size();
118  throw ParseException(error.str());
119  }
120  bool valid = true;
121  msg->clock_status = sentence.body[0];
122  valid &= ParseDouble(sentence.body[1], msg->offset);
123  valid &= ParseDouble(sentence.body[2], msg->offset_std);
124  valid &= ParseDouble(sentence.body[3], msg->utc_offset);
125  valid &= ParseUInt32(sentence.body[4], msg->utc_year, 10);
126  valid &= ParseUInt8(sentence.body[5], msg->utc_month, 10);
127  valid &= ParseUInt8(sentence.body[6], msg->utc_day, 10);
128  valid &= ParseUInt8(sentence.body[7], msg->utc_hour, 10);
129  valid &= ParseUInt8(sentence.body[8], msg->utc_minute, 10);
130  valid &= ParseUInt32(sentence.body[9], msg->utc_millisecond, 10);
131  msg->utc_status = sentence.body[10];
132 
133  if (!valid)
134  {
135  throw ParseException("Error parsing TIME log.");
136  }
137 
138  return msg;
139 }
msg
bool ParseUInt8(const std::string &string, uint8_t &value, int32_t base=10)
Parses a string containing an integer number into a uint16_t.
novatel_gps_msgs::TimePtr ParseAscii(const NovatelSentence &sentence) noexcept(false) override
Converts sentence into a ROS message pointer and returns it.
Definition: time.cpp:111
double ParseDouble(const uint8_t *buffer)
Converts a buffer containing 8 bytes into a double.
static constexpr size_t ASCII_FIELD
Definition: time.h:50
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: time.cpp:35
novatel_gps_msgs::TimePtr ParseBinary(const BinaryMessage &bin_msg) noexcept(false) override
Converts bin_msg into a ROS message pointer and returns it.
Definition: time.cpp:45
static constexpr uint16_t MESSAGE_ID
Definition: time.h:49
const std::string GetMessageName() const override
Definition: time.cpp:40
static const std::string MESSAGE_NAME
Definition: time.h:51
static constexpr size_t BINARY_LENGTH
Definition: time.h:48


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