34 #include <boost/make_shared.hpp>
52 if (bin_msg.data_.size() != BINARY_LENGTH)
54 std::stringstream error;
55 error <<
"Unexpected DUALANTENNAHEADING message length: " << bin_msg.data_.size();
58 novatel_gps_msgs::NovatelDualAntennaHeadingPtr ros_msg =
59 boost::make_shared<novatel_gps_msgs::NovatelDualAntennaHeading>();
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->station_id.resize(4);
93 std::copy(&bin_msg.data_[32], &bin_msg.data_[36], &ros_msg->station_id[0]);
95 ros_msg->num_satellites_tracked = bin_msg.data_[36];
96 ros_msg->num_satellites_used_in_solution = bin_msg.data_[37];
97 ros_msg->num_satellites_above_elevation_mask_angle = bin_msg.data_[38];
98 ros_msg->num_satellites_above_elevation_mask_angle_l2 = bin_msg.data_[39];
100 ros_msg->solution_source = SolutionSourceToMsgEnum(bin_msg.data_[40]);
103 ros_msg->extended_solution_status);
114 novatel_gps_msgs::NovatelDualAntennaHeadingPtr ros_msg =
115 boost::make_shared<novatel_gps_msgs::NovatelDualAntennaHeading>();
117 ros_msg->novatel_msg_header = h_parser.
ParseAscii(sentence);
119 if (sentence.body.size() != ASCII_LENGTH)
121 std::stringstream error;
122 error <<
"Unexpected number of DUALANTENNAHEADING message fields: " << sentence.body.size();
128 ros_msg->solution_status = sentence.body[0];
129 ros_msg->position_type = sentence.body[1];
131 valid = valid &&
ParseFloat(sentence.body[2], ros_msg->baseline_length);
133 valid = valid &&
ParseFloat(sentence.body[3], ros_msg->heading);
134 valid = valid &&
ParseFloat(sentence.body[4], ros_msg->pitch);
138 valid = valid &&
ParseFloat(sentence.body[6], ros_msg->heading_sigma);
139 valid = valid &&
ParseFloat(sentence.body[7], ros_msg->pitch_sigma);
141 ros_msg->station_id = sentence.body[8];
143 valid = valid &&
ParseUInt8(sentence.body[9], ros_msg->num_satellites_tracked);
144 valid = valid &&
ParseUInt8(sentence.body[10], ros_msg->num_satellites_used_in_solution);
145 valid = valid &&
ParseUInt8(sentence.body[11], ros_msg->num_satellites_above_elevation_mask_angle);
146 valid = valid &&
ParseUInt8(sentence.body[12], ros_msg->num_satellites_above_elevation_mask_angle_l2);
148 uint32_t solution_source = 0;
149 valid = valid &&
ParseUInt32(sentence.body[13], solution_source, 16);
150 ros_msg->solution_source = SolutionSourceToMsgEnum((uint8_t)solution_source);
152 uint32_t extended_solution_status = 0;
153 valid = valid &&
ParseUInt32(sentence.body[14], extended_solution_status, 16);
155 extended_solution_status, ros_msg->extended_solution_status);
159 uint32_t signal_mask = 0;
160 valid = valid &&
ParseUInt32(sentence.body[16], signal_mask, 16);
165 throw ParseException(
"Invalid field in DUALANTENNAHEADING message");
173 uint8_t source_bits = (source_mask & 0x0Cu) >> 2u;
177 return novatel_gps_msgs::NovatelDualAntennaHeading::SOURCE_PRIMARY_ANTENNA;
179 return novatel_gps_msgs::NovatelDualAntennaHeading::SOURCE_SECONDARY_ANTENNA;
181 throw ParseException(
"DUALANTENNAHEADING Solution Source could not be parsed due to unknown source");