Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion fixposition_driver_lib/src/helper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -304,7 +304,7 @@ NmeaEpochData NmeaEpochData::CompleteAndReset() {
else if (rmc_.time.valid) { time_ = rmc_.time; }
else if (zda_.time.valid) { time_ = zda_.time; }
if (gga_.llh.latlon_valid) { llh_ = gga_.llh; }
else if (rmc_.llh.latlon_valid) { llh_ = rmc_.llh; }
else if (rmc_.ll.latlon_valid) { llh_ = rmc_.ll; }
else if (gll_.ll.latlon_valid) { llh_ = gll_.ll; } // last as it does not have height
// clang-format on

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -630,6 +630,7 @@ inline int NmeaSignalIdToMsg(const RosMsgT& msg, const fpsdk::common::parser::nm
case fpsdk::common::parser::nmea::NmeaSignalId::GLO_L1OF: return msg.consts.SIGNAL_ID_GLO_L1OF;
case fpsdk::common::parser::nmea::NmeaSignalId::GLO_L2OF: return msg.consts.SIGNAL_ID_GLO_L2OF;
case fpsdk::common::parser::nmea::NmeaSignalId::NAVIC_L5A: return msg.consts.SIGNAL_ID_NAVIC_L5A;
default: return msg.consts.SIGNAL_ID_UNSPECIFIED;
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

clang complains about unhandled cases otherwise

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should add and handle all enum values here and not use a default case. I'll fix that.

}
// clang-format on
return msg.consts.SIGNAL_ID_UNSPECIFIED;
Expand Down
23 changes: 11 additions & 12 deletions fixposition_driver_ros2/src/data_to_ros2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -520,7 +520,7 @@ void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGga>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGga msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe you want to align conventions and remove the underscore from talker_, being this a public member

if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -545,7 +545,7 @@ void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGll>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGll msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -566,7 +566,7 @@ void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGsa>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGsa msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.system = NmeaSystemIdToMsg(msg, payload.system);
msg.opmode = NmeaOpModeGsaToMsg(msg, payload.opmode);
msg.navmode = NmeaNavModeGsaToMsg(msg, payload.navmode);
Expand All @@ -588,7 +588,7 @@ void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGst>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGst msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.time.valid) {
msg.time_valid = true;
msg.time_h = payload.time.hours;
Expand All @@ -611,8 +611,7 @@ void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaGsv>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaGsv msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.system = NmeaSystemIdToMsg(msg, payload.system);
msg.signal = NmeaSignalIdToMsg(msg, payload.signal);
msg.num_msgs = payload.num_msgs.value;
Expand Down Expand Up @@ -640,7 +639,7 @@ void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaHdt>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaHdt msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.heading = (payload.heading.valid ? payload.heading.value : NAN);
pub->publish(msg);
}
Expand All @@ -652,7 +651,7 @@ void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaRmc>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaRmc msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.date.valid) {
msg.date_valid = true;
msg.date_y = payload.date.years;
Expand All @@ -668,8 +667,8 @@ void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload,
msg.status = NmeaStatusGllRmcToMsg(msg, payload.status);
msg.mode = NmeaModeRmcGnsToMsg(msg, payload.mode);
msg.navstatus = NmeaNavStatusRmcToMsg(msg, payload.navstatus);
msg.latitude = (payload.llh.latlon_valid ? payload.llh.lat : NAN);
msg.longitude = (payload.llh.latlon_valid ? payload.llh.lon : NAN);
msg.latitude = (payload.ll.latlon_valid ? payload.ll.lat : NAN);
msg.longitude = (payload.ll.latlon_valid ? payload.ll.lon : NAN);
Comment on lines +670 to +671
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

llh does not exist on this struct

msg.speed = (payload.speed.valid ? payload.speed.value : NAN);
msg.course = (payload.course.valid ? payload.course.value : NAN);
pub->publish(msg);
Expand All @@ -682,7 +681,7 @@ void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaVtg>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaVtg msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
msg.cogt = (payload.cogt.valid ? payload.cogt.value : NAN);
msg.cogm = (payload.cogm.valid ? payload.cogm.value : NAN);
msg.sogn = (payload.sogn.valid ? payload.sogn.value : NAN);
Expand All @@ -698,7 +697,7 @@ void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload,
rclcpp::Publisher<fpmsgs::NmeaZda>::SharedPtr& pub) {
if (pub->get_subscription_count() > 0) {
fpmsgs::NmeaZda msg;
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
msg.talker = NmeaTalkerIdToMsg(msg, payload.talker_);
if (payload.date.valid) {
msg.date_valid = true;
msg.date_y = payload.date.years;
Expand Down
2 changes: 1 addition & 1 deletion fixposition_driver_ros2/src/fixposition_driver_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -625,7 +625,7 @@ void FixpositionDriverNode::ProcessOdometryData(const OdometryData& odometry_dat

// Output jump warning
if (params_.cov_warning_ && odometry_data.valid && jump_detector_.Check(odometry_data)) {
RCLCPP_WARN(logger_, jump_detector_.warning_.c_str());
RCLCPP_WARN(logger_, "%s", jump_detector_.warning_.c_str());
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Second argument should be a format string

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, that's better. Thanks.

PublishJumpWarning(jump_detector_, jump_pub_);
}

Expand Down