From f44f8486b3080f99ae235fbc926652fa9c5b3cae Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Fri, 17 Oct 2025 15:19:33 +0200 Subject: [PATCH 1/2] fix compilation errors --- .../fixposition_driver_lib/fixposition_driver.hpp | 2 +- fixposition_driver_lib/src/fixposition_driver.cpp | 9 +++++---- fixposition_driver_lib/src/helper.cpp | 2 +- fixposition_driver_ros2/src/data_to_ros2.cpp | 4 ++-- fixposition_driver_ros2/src/fixposition_driver_node.cpp | 2 +- 5 files changed, 10 insertions(+), 9 deletions(-) diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp index f3142e8e..8cef49eb 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp @@ -221,7 +221,7 @@ class FixpositionDriver : private boost::noncopyable { // Worker thread fpsdk::common::parser::Parser parser_; //!< Protocol parser for incoming messages fpsdk::common::thread::Thread worker_; //!< Worker thread handle - void Worker(void* arg); //!< Worker thread + bool Worker(fpsdk::common::thread::Thread& thread, void* arg); //!< Worker thread // Observers for received messages std::unordered_map> fpa_observers_; //!< FP_A message observers diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 08bf6ab2..83369e93 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -39,7 +39,8 @@ using namespace fpsdk::common::parser; FixpositionDriver::FixpositionDriver(const DriverParams& params) : /* clang-format off */ params_ { params }, - worker_ { "driver", std::bind(&FixpositionDriver::Worker, this, std::placeholders::_1) } // clang-format on + worker_ { "driver", std::bind(&FixpositionDriver::Worker, this, std::placeholders::_1, std::placeholders::_2) } + // clang-format on {} FixpositionDriver::~FixpositionDriver() { StopDriver(); } @@ -264,10 +265,10 @@ void FixpositionDriver::StopDriver() { Disconnect(); } -void FixpositionDriver::Worker(void* /*arg*/) { +bool FixpositionDriver::Worker(fpsdk::common::thread::Thread& thread, void* /*arg*/) { INFO("Driver running..."); - while (!worker_.ShouldAbort()) { + while (!thread.ShouldAbort()) { // While we're connected to the sensor... if (IsConnected()) { // Read more data from sensor and feed the parser @@ -311,7 +312,7 @@ void FixpositionDriver::Worker(void* /*arg*/) { // Reconnect after some time... else { INFO("Reconnecting in %.1f seconds...", params_.reconnect_delay_); - if (worker_.Sleep(params_.reconnect_delay_ * 1000)) { + if (thread.Sleep(params_.reconnect_delay_ * 1000) == fpsdk::common::thread::WaitRes::TIMEOUT) { break; } Connect(); diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index 781b7725..58237f53 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -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 diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index ce7297f8..b6faed6d 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -665,8 +665,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); msg.speed = (payload.speed.valid ? payload.speed.value : NAN); msg.course = (payload.course.valid ? payload.course.value : NAN); pub->publish(msg); diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 3e127dc7..d99b5be3 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -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()); PublishJumpWarning(jump_detector_, jump_pub_); } From 4aaba0e295c11ddce6bc73239d06e882d11a5cc8 Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Fri, 17 Oct 2025 15:30:17 +0200 Subject: [PATCH 2/2] fix name of member --- .../src/fixposition_driver.cpp | 2 +- .../fixposition_driver_msgs/data_to_ros.hpp | 1 + fixposition_driver_ros2/src/data_to_ros2.cpp | 19 +++++++++---------- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index ac38bff9..269b9fd1 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -270,7 +270,7 @@ void FixpositionDriver::StopDriver() { bool FixpositionDriver::Worker() { INFO("Driver running..."); - while (!thread.ShouldAbort()) { + while (!worker_.ShouldAbort()) { // While we're connected to the sensor... if (IsConnected()) { // Read more data from sensor and feed the parser diff --git a/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp b/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp index 324ba7dc..943c2bff 100644 --- a/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp +++ b/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp @@ -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; } // clang-format on return msg.consts.SIGNAL_ID_UNSPECIFIED; diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index ff037473..d5f3b158 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -520,7 +520,7 @@ void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::NmeaGga 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; @@ -545,7 +545,7 @@ void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, rclcpp::Publisher::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; @@ -566,7 +566,7 @@ void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload, rclcpp::Publisher::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); @@ -588,7 +588,7 @@ void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload, rclcpp::Publisher::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; @@ -611,8 +611,7 @@ void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload, rclcpp::Publisher::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; @@ -640,7 +639,7 @@ void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload, rclcpp::Publisher::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); } @@ -652,7 +651,7 @@ void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload, rclcpp::Publisher::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; @@ -682,7 +681,7 @@ void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload, rclcpp::Publisher::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); @@ -698,7 +697,7 @@ void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload, rclcpp::Publisher::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;