34 #include <boost/make_shared.hpp>
52 if (bin_msg.data_.size() != BINARY_LENGTH)
54 std::stringstream error;
55 error <<
"Unexpected HEADING2 message length: " << bin_msg.data_.size();
58 novatel_gps_msgs::NovatelHeading2Ptr ros_msg =
59 boost::make_shared<novatel_gps_msgs::NovatelHeading2>();
61 ros_msg->novatel_msg_header = header_parser.
ParseBinary(bin_msg);
62 ros_msg->novatel_msg_header.message_name = MESSAGE_NAME;
64 uint16_t solution_status =
ParseUInt16(&bin_msg.data_[0]);
67 std::stringstream error;
68 error <<
"Unknown solution status: " << solution_status;
76 std::stringstream error;
77 error <<
"Unknown position type: " << pos_type;
82 ros_msg->baseline_length =
ParseFloat(&bin_msg.data_[8]);
84 ros_msg->heading =
ParseFloat(&bin_msg.data_[12]);
85 ros_msg->pitch =
ParseFloat(&bin_msg.data_[16]);
89 ros_msg->heading_sigma =
ParseFloat(&bin_msg.data_[24]);
90 ros_msg->pitch_sigma =
ParseFloat(&bin_msg.data_[28]);
92 ros_msg->rover_station_id.resize(4);
93 std::copy(&bin_msg.data_[32], &bin_msg.data_[36], &ros_msg->rover_station_id[0]);
95 ros_msg->master_station_id.resize(4);
96 std::copy(&bin_msg.data_[36], &bin_msg.data_[40], &ros_msg->master_station_id[0]);
98 ros_msg->num_satellites_tracked = bin_msg.data_[40];
99 ros_msg->num_satellites_used_in_solution = bin_msg.data_[41];
100 ros_msg->num_satellites_above_elevation_mask_angle = bin_msg.data_[42];
101 ros_msg->num_satellites_above_elevation_mask_angle_l2 = bin_msg.data_[43];
103 ros_msg->solution_source = SolutionSourceToMsgEnum(bin_msg.data_[44]);
106 ros_msg->extended_solution_status);
117 novatel_gps_msgs::NovatelHeading2Ptr ros_msg =
118 boost::make_shared<novatel_gps_msgs::NovatelHeading2>();
120 ros_msg->novatel_msg_header = h_parser.
ParseAscii(sentence);
122 if (sentence.body.size() != ASCII_LENGTH)
124 std::stringstream error;
125 error <<
"Unexpected number of HEADING2 message fields: " << sentence.body.size();
131 ros_msg->solution_status = sentence.body[0];
132 ros_msg->position_type = sentence.body[1];
134 valid = valid &&
ParseFloat(sentence.body[2], ros_msg->baseline_length);
136 valid = valid &&
ParseFloat(sentence.body[3], ros_msg->heading);
137 valid = valid &&
ParseFloat(sentence.body[4], ros_msg->pitch);
141 valid = valid &&
ParseFloat(sentence.body[6], ros_msg->heading_sigma);
142 valid = valid &&
ParseFloat(sentence.body[7], ros_msg->pitch_sigma);
144 ros_msg->rover_station_id = sentence.body[8];
146 ros_msg->master_station_id = sentence.body[9];
148 valid = valid &&
ParseUInt8(sentence.body[10], ros_msg->num_satellites_tracked);
149 valid = valid &&
ParseUInt8(sentence.body[11], ros_msg->num_satellites_used_in_solution);
150 valid = valid &&
ParseUInt8(sentence.body[12], ros_msg->num_satellites_above_elevation_mask_angle);
151 valid = valid &&
ParseUInt8(sentence.body[13], ros_msg->num_satellites_above_elevation_mask_angle_l2);
153 uint32_t solution_source = 0;
154 valid = valid &&
ParseUInt32(sentence.body[14], solution_source, 16);
155 ros_msg->solution_source = SolutionSourceToMsgEnum((uint8_t)solution_source);
157 uint32_t extended_solution_status = 0;
158 valid = valid &&
ParseUInt32(sentence.body[15], extended_solution_status, 16);
160 extended_solution_status, ros_msg->extended_solution_status);
164 uint32_t signal_mask = 0;
165 valid = valid &&
ParseUInt32(sentence.body[17], signal_mask, 16);
178 uint8_t source_bits = (source_mask & 0x0Cu) >> 2u;
182 return novatel_gps_msgs::NovatelHeading2::SOURCE_PRIMARY_ANTENNA;
184 return novatel_gps_msgs::NovatelHeading2::SOURCE_SECONDARY_ANTENNA;
186 throw ParseException(
"HEADING2 Solution Source could not be parsed due to unknown source");