From 6f19c6fceee0cf1e1614975b3d3f88a82f09be88 Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 21 Oct 2025 16:18:17 +0200 Subject: [PATCH 1/6] implementation of novb_heading2 --- fixposition_driver_msgs/msg/NovbHeading2.msg | 24 +++++++++++ .../fixposition_driver_ros1/data_to_ros1.hpp | 2 + .../fixposition_driver_node.hpp | 1 + .../fixposition_driver_ros1/ros1_msgs.hpp | 1 + fixposition_driver_ros1/src/data_to_ros1.cpp | 41 ++++++++++++++++++ .../src/fixposition_driver_node.cpp | 12 ++++++ .../fixposition_driver_ros2/data_to_ros2.hpp | 3 ++ .../fixposition_driver_node.hpp | 1 + .../fixposition_driver_ros2/ros2_msgs.hpp | 1 + fixposition_driver_ros2/src/data_to_ros2.cpp | 42 +++++++++++++++++++ .../src/fixposition_driver_node.cpp | 12 ++++++ 11 files changed, 140 insertions(+) create mode 100644 fixposition_driver_msgs/msg/NovbHeading2.msg diff --git a/fixposition_driver_msgs/msg/NovbHeading2.msg b/fixposition_driver_msgs/msg/NovbHeading2.msg new file mode 100644 index 0000000..e5708b5 --- /dev/null +++ b/fixposition_driver_msgs/msg/NovbHeading2.msg @@ -0,0 +1,24 @@ +# Copyright (c) Fixposition AG (www.fixposition.com) and contributors +# License: see the LICENSE file +# +# NOV_B-HEADING2 data + +std_msgs/Header header +uint32 sol_status # See NovbGnssSolStat +uint32 pos_type # See NovbPosOrVelType +float32 length # Baseline length (m) +float32 heading # Heading (degrees) +float32 pitch # Pitch (degrees) +float32 reserved # Reserved +float32 heading_stdev # Heading standard deviation (degrees) +float32 pitch_stdev # Pitch standard deviation (degrees) +uint8[4] rover_stn_id # Rover station ID +uint8[4] master_stn_id # Master station ID +uint8 num_sv_tracked # Number of satellites tracked +uint8 num_sv_in_sol # Number of satellites in solution +uint8 num_sv_obs # Number of satellites with observations +uint8 num_sv_multi # Number of satellites with multi-frequency observations +uint8 sol_source # Solution source (see NovbSolSource) +uint8 ext_sol_status # Extended solution status (see NovbGnssSolExtStat) +uint8 galileo_beidou_sig_mask # Galileo and BeiDou signal mask (see NovbSigUsedGalBds) +uint8 gps_glonass_sig_mask # GPS and GLONASS signal mask (see NovbSigUsedGpsGlo) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 5d40ae7..4b8ae8b 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -58,6 +58,8 @@ bool PublishNovbBestgnsspos(const fpsdk::common::parser::novb::NovbHeader* heade ros::Publisher& pub2); bool PublishNovbInspvax(const fpsdk::common::parser::novb::NovbHeader* header, const fpsdk::common::parser::novb::NovbInspvax* payload, ros::Publisher& pub); +bool PublishNovbHeading2(const fpsdk::common::parser::novb::NovbHeader* header, + const fpsdk::common::parser::novb::NovbHeading2* payload, ros::Publisher& pub); void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, ros::Publisher& pub); void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, ros::Publisher& pub); diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index d3c5c4b..11781a8 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -87,6 +87,7 @@ class FixpositionDriverNode { ros::Publisher nmea_zda_pub_; //!< NMEA-GP-ZDA message // - NOV_B messages ros::Publisher novb_inspvax_pub_; //!< NOV_B-INSPVAX message + ros::Publisher novb_heading2_pub_; //!< NOV_B-HEADING2 message // - Odometry ros::Publisher odometry_ecef_pub_; //!< ECEF odometry ros::Publisher odometry_enu_pub_; //!< ENU odometry diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp index 4e8286d..f25dab2 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp @@ -56,6 +56,7 @@ #include // - NOV-B #include +#include #pragma GCC diagnostic pop #endif // __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__ diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 44e5222..cce0dba 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -506,6 +506,47 @@ bool PublishNovbInspvax(const novb::NovbHeader* header, const novb::NovbInspvax* // --------------------------------------------------------------------------------------------------------------------- +static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHeading2* payload, + fixposition_driver_msgs::NovbHeading2& msg) { + if ((header != NULL) && (payload != NULL) && header->IsLongHeader()) { + time::Time stamp; + if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3, + time::WnoTow::Sys::GPS})) { + msg.header.stamp = ros1::utils::ConvTime(stamp); + } + + msg.sol_status = payload->sol_status; + msg.pos_type = payload->pos_type; + msg.length = payload->length; + msg.heading = payload->heading; + msg.pitch = payload->pitch; + msg.reserved = payload->reserved; + msg.heading_stdev = payload->heading_stdev; + msg.pitch_stdev = payload->pitch_stdev; + std::copy(std::begin(payload->rover_stn_id), std::end(payload->rover_stn_id), std::begin(msg.rover_stn_id)); + std::copy(std::begin(payload->master_stn_id), std::end(payload->master_stn_id), std::begin(msg.master_stn_id)); + msg.num_sv_tracked = payload->num_sv_tracked; + msg.num_sv_in_sol = payload->num_sv_in_sol; + msg.num_sv_obs = payload->num_sv_obs; + msg.num_sv_multi = payload->num_sv_multi; + msg.sol_source = payload->sol_source; + msg.ext_sol_status = payload->ext_sol_status; + msg.galileo_beidou_sig_mask = payload->galileo_beidou_sig_mask; + msg.gps_glonass_sig_mask = payload->gps_glonass_sig_mask; + } +} + +bool PublishNovbHeading2(const novb::NovbHeader* header, const novb::NovbHeading2* payload, ros::Publisher& pub) { + if (pub.getNumSubscribers() > 0) { + fixposition_driver_msgs::NovbHeading2 msg; + NovbHeading2ToMsg(header, payload, msg); + pub.publish(msg); + } + return true; +} + +// --------------------------------------------------------------------------------------------------------------------- + void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::NmeaGga msg; diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index d0e5f22..8233852 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -277,6 +277,17 @@ bool FixpositionDriverNode::StartNode() { }); } + // NOV_B-HEADING2 + if (params_.MessageEnabled(novb::NOV_B_HEADING2_STRID)) { + _PUB(novb_heading2_pub_, fixposition_driver_msgs::NovbHeading2, output_ns + "/novb/heading2", 5); + driver_.AddNovbObserver( // + novb::NOV_B_HEADING2_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) { + if (!PublishNovbHeading2(header, (novb::NovbHeading2*)payload, novb_heading2_pub_)) { + ROS_WARN_THROTTLE(1.0, "Bad NOV_B-HEADING2"); + } + }); + } + // NMEA-GP-GGA if (params_.MessageEnabled(nmea::NmeaGgaPayload::FORMATTER)) { _PUB(nmea_gga_pub_, fixposition_driver_msgs::NmeaGga, output_ns + "/nmea/gga", 5); @@ -480,6 +491,7 @@ void FixpositionDriverNode::StopNode() { nmea_zda_pub_.shutdown(); // - NOV_B messages novb_inspvax_pub_.shutdown(); + novb_heading2_pub_.shutdown(); // - Odometry odometry_ecef_pub_.shutdown(); odometry_enu_pub_.shutdown(); diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index f3ac657..7730559 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -74,6 +74,9 @@ bool PublishNovbBestgnsspos(const fpsdk::common::parser::novb::NovbHeader* heade bool PublishNovbInspvax(const fpsdk::common::parser::novb::NovbHeader* header, const fpsdk::common::parser::novb::NovbInspvax* payload, rclcpp::Publisher::SharedPtr& pub); +bool PublishNovbHeading2(const fpsdk::common::parser::novb::NovbHeader* header, + const fpsdk::common::parser::novb::NovbHeading2* payload, + rclcpp::Publisher::SharedPtr& pub); void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, rclcpp::Publisher::SharedPtr& pub); diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 45f1457..27ba8aa 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -91,6 +91,7 @@ class FixpositionDriverNode { rclcpp::Publisher::SharedPtr nmea_zda_pub_; //!< NMEA-GP-ZDA message // - NOV_B messages rclcpp::Publisher::SharedPtr novb_inspvax_pub_; //!< NOV_B-INSPVAX message + rclcpp::Publisher::SharedPtr novb_heading2_pub_; //!< NOV_B-HEADING2 message // - Odometry rclcpp::Publisher::SharedPtr odometry_ecef_pub_; //!< ECEF odometry rclcpp::Publisher::SharedPtr odometry_enu_pub_; //!< ENU odometry diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp index 656617b..239698f 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp @@ -58,6 +58,7 @@ #include // - NOV-B #include +#include // Shortcut namespace fpmsgs = fixposition_driver_msgs::msg; diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 13f590c..6ea854c 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -516,6 +516,48 @@ bool PublishNovbInspvax(const novb::NovbHeader* header, const novb::NovbInspvax* // --------------------------------------------------------------------------------------------------------------------- +static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHeading2* payload, + fpmsgs::NovbHeading2& msg) { + if ((header != NULL) && (payload != NULL) && header->IsLongHeader()) { + time::Time stamp; + if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3, + time::WnoTow::Sys::GPS})) { + msg.header.stamp = ros2::utils::ConvTime(stamp); + } + + msg.sol_status = payload->sol_status; + msg.pos_type = payload->pos_type; + msg.length = payload->length; + msg.heading = payload->heading; + msg.pitch = payload->pitch; + msg.reserved = payload->reserved; + msg.heading_stdev = payload->heading_stdev; + msg.pitch_stdev = payload->pitch_stdev; + std::copy(std::begin(payload->rover_stn_id), std::end(payload->rover_stn_id), std::begin(msg.rover_stn_id)); + std::copy(std::begin(payload->master_stn_id), std::end(payload->master_stn_id), std::begin(msg.master_stn_id)); + msg.num_sv_tracked = payload->num_sv_tracked; + msg.num_sv_in_sol = payload->num_sv_in_sol; + msg.num_sv_obs = payload->num_sv_obs; + msg.num_sv_multi = payload->num_sv_multi; + msg.sol_source = payload->sol_source; + msg.ext_sol_status = payload->ext_sol_status; + msg.galileo_beidou_sig_mask = payload->galileo_beidou_sig_mask; + msg.gps_glonass_sig_mask = payload->gps_glonass_sig_mask; + } +} + +bool PublishNovbHeading2(const novb::NovbHeader* header, const novb::NovbHeading2* payload, + rclcpp::Publisher::SharedPtr& pub) { + if (pub->get_subscription_count() > 0) { + fpmsgs::NovbHeading2 msg; + NovbHeading2ToMsg(header, payload, msg); + pub->publish(msg); + } + return true; +} + +// --------------------------------------------------------------------------------------------------------------------- + void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 4cc4346..89f15e9 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -305,6 +305,17 @@ bool FixpositionDriverNode::StartNode() { }); } + // NOV_B-HEADING2 + if (params_.MessageEnabled(novb::NOV_B_HEADING2_STRID)) { + _PUB(novb_heading2_pub_, fpmsgs::NovbHeading2, output_ns + "/novb/heading2", qos_settings_); + driver_.AddNovbObserver( // + novb::NOV_B_HEADING2_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) { + if (!PublishNovbHeading2(header, (novb::NovbHeading2*)payload, novb_heading2_pub_)) { + RCLCPP_WARN_THROTTLE(logger_, *nh_->get_clock(), 1e3, "Bad NOV_B-HEADING2"); + } + }); + } + // NMEA-GP-GGA if (params_.MessageEnabled(nmea::NmeaGgaPayload::FORMATTER)) { _PUB(nmea_gga_pub_, fpmsgs::NmeaGga, output_ns + "/nmea/gga", qos_settings_); @@ -507,6 +518,7 @@ void FixpositionDriverNode::StopNode() { nmea_zda_pub_.reset(); // - NOV_B messages novb_inspvax_pub_.reset(); + novb_heading2_pub_.reset(); // - Odometry odometry_ecef_pub_.reset(); odometry_enu_pub_.reset(); From 51bb923b0820f94478ac44887e5d56ce7ef8cee0 Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Wed, 22 Oct 2025 10:03:05 +0200 Subject: [PATCH 2/6] apply path from @flipflip --- .../include/fixposition_driver_ros1/fixposition_driver_node.hpp | 2 +- .../include/fixposition_driver_ros1/ros1_msgs.hpp | 2 +- .../include/fixposition_driver_ros2/fixposition_driver_node.hpp | 2 +- .../include/fixposition_driver_ros2/ros2_msgs.hpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 11781a8..d9ec591 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -86,7 +86,7 @@ class FixpositionDriverNode { ros::Publisher nmea_vtg_pub_; //!< NMEA-GP-VTG message ros::Publisher nmea_zda_pub_; //!< NMEA-GP-ZDA message // - NOV_B messages - ros::Publisher novb_inspvax_pub_; //!< NOV_B-INSPVAX message + ros::Publisher novb_inspvax_pub_; //!< NOV_B-INSPVAX message ros::Publisher novb_heading2_pub_; //!< NOV_B-HEADING2 message // - Odometry ros::Publisher odometry_ecef_pub_; //!< ECEF odometry diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp index f25dab2..1e71f8e 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp @@ -55,8 +55,8 @@ #include #include // - NOV-B -#include #include +#include #pragma GCC diagnostic pop #endif // __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__ diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 27ba8aa..be87495 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -90,7 +90,7 @@ class FixpositionDriverNode { rclcpp::Publisher::SharedPtr nmea_vtg_pub_; //!< NMEA-GP-VTG message rclcpp::Publisher::SharedPtr nmea_zda_pub_; //!< NMEA-GP-ZDA message // - NOV_B messages - rclcpp::Publisher::SharedPtr novb_inspvax_pub_; //!< NOV_B-INSPVAX message + rclcpp::Publisher::SharedPtr novb_inspvax_pub_; //!< NOV_B-INSPVAX message rclcpp::Publisher::SharedPtr novb_heading2_pub_; //!< NOV_B-HEADING2 message // - Odometry rclcpp::Publisher::SharedPtr odometry_ecef_pub_; //!< ECEF odometry diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp index 239698f..3d265dc 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp @@ -57,8 +57,8 @@ #include #include // - NOV-B -#include #include +#include // Shortcut namespace fpmsgs = fixposition_driver_msgs::msg; From 56492c3e879fab7d511b19b8012cab3eb953a0fe Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Wed, 22 Oct 2025 10:03:05 +0200 Subject: [PATCH 3/6] apply path from @flipflip8952 --- .../include/fixposition_driver_ros1/fixposition_driver_node.hpp | 2 +- .../include/fixposition_driver_ros1/ros1_msgs.hpp | 2 +- .../include/fixposition_driver_ros2/fixposition_driver_node.hpp | 2 +- .../include/fixposition_driver_ros2/ros2_msgs.hpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 11781a8..d9ec591 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -86,7 +86,7 @@ class FixpositionDriverNode { ros::Publisher nmea_vtg_pub_; //!< NMEA-GP-VTG message ros::Publisher nmea_zda_pub_; //!< NMEA-GP-ZDA message // - NOV_B messages - ros::Publisher novb_inspvax_pub_; //!< NOV_B-INSPVAX message + ros::Publisher novb_inspvax_pub_; //!< NOV_B-INSPVAX message ros::Publisher novb_heading2_pub_; //!< NOV_B-HEADING2 message // - Odometry ros::Publisher odometry_ecef_pub_; //!< ECEF odometry diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp index f25dab2..1e71f8e 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp @@ -55,8 +55,8 @@ #include #include // - NOV-B -#include #include +#include #pragma GCC diagnostic pop #endif // __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__ diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 27ba8aa..be87495 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -90,7 +90,7 @@ class FixpositionDriverNode { rclcpp::Publisher::SharedPtr nmea_vtg_pub_; //!< NMEA-GP-VTG message rclcpp::Publisher::SharedPtr nmea_zda_pub_; //!< NMEA-GP-ZDA message // - NOV_B messages - rclcpp::Publisher::SharedPtr novb_inspvax_pub_; //!< NOV_B-INSPVAX message + rclcpp::Publisher::SharedPtr novb_inspvax_pub_; //!< NOV_B-INSPVAX message rclcpp::Publisher::SharedPtr novb_heading2_pub_; //!< NOV_B-HEADING2 message // - Odometry rclcpp::Publisher::SharedPtr odometry_ecef_pub_; //!< ECEF odometry diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp index 239698f..3d265dc 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp @@ -57,8 +57,8 @@ #include #include // - NOV-B -#include #include +#include // Shortcut namespace fpmsgs = fixposition_driver_msgs::msg; From 5a3bf1a56ba4965b4a8f4cf126e449ff9a2820f7 Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Wed, 22 Oct 2025 11:21:25 +0200 Subject: [PATCH 4/6] edits from suggestions (with pre-commit now) --- fixposition_driver_msgs/msg/NovbHeading2.msg | 4 ++-- fixposition_driver_ros1/src/data_to_ros1.cpp | 9 ++++++--- fixposition_driver_ros2/src/data_to_ros2.cpp | 6 ++++-- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/fixposition_driver_msgs/msg/NovbHeading2.msg b/fixposition_driver_msgs/msg/NovbHeading2.msg index e5708b5..7c9c79b 100644 --- a/fixposition_driver_msgs/msg/NovbHeading2.msg +++ b/fixposition_driver_msgs/msg/NovbHeading2.msg @@ -12,8 +12,8 @@ float32 pitch # Pitch (degrees) float32 reserved # Reserved float32 heading_stdev # Heading standard deviation (degrees) float32 pitch_stdev # Pitch standard deviation (degrees) -uint8[4] rover_stn_id # Rover station ID -uint8[4] master_stn_id # Master station ID +string rover_stn_id # Rover station ID +string master_stn_id # Master station ID uint8 num_sv_tracked # Number of satellites tracked uint8 num_sv_in_sol # Number of satellites in solution uint8 num_sv_obs # Number of satellites with observations diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index cce0dba..3a9d308 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -523,9 +523,12 @@ static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHe msg.reserved = payload->reserved; msg.heading_stdev = payload->heading_stdev; msg.pitch_stdev = payload->pitch_stdev; - std::copy(std::begin(payload->rover_stn_id), std::end(payload->rover_stn_id), std::begin(msg.rover_stn_id)); - std::copy(std::begin(payload->master_stn_id), std::end(payload->master_stn_id), std::begin(msg.master_stn_id)); - msg.num_sv_tracked = payload->num_sv_tracked; + msg.rover_stn_id = fpsdk::common::string::BufToStr( + {payload->rover_stn_id, payload->rover_stn_id + sizeof(payload->rover_stn_id)}) + msg.master_stn_id = + fpsdk::common::string::BufToStr( + {payload->master_stn_id, payload->master_stn_id + sizeof(payload->master_stn_id)}) msg.num_sv_tracked = + payload->num_sv_tracked; msg.num_sv_in_sol = payload->num_sv_in_sol; msg.num_sv_obs = payload->num_sv_obs; msg.num_sv_multi = payload->num_sv_multi; diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 6ea854c..94c2b69 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -533,8 +533,10 @@ static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHe msg.reserved = payload->reserved; msg.heading_stdev = payload->heading_stdev; msg.pitch_stdev = payload->pitch_stdev; - std::copy(std::begin(payload->rover_stn_id), std::end(payload->rover_stn_id), std::begin(msg.rover_stn_id)); - std::copy(std::begin(payload->master_stn_id), std::end(payload->master_stn_id), std::begin(msg.master_stn_id)); + msg.rover_stn_id = fpsdk::common::string::BufToStr( + {payload->rover_stn_id, payload->rover_stn_id + sizeof(payload->rover_stn_id)}); + msg.master_stn_id = fpsdk::common::string::BufToStr( + {payload->master_stn_id, payload->master_stn_id + sizeof(payload->master_stn_id)}); msg.num_sv_tracked = payload->num_sv_tracked; msg.num_sv_in_sol = payload->num_sv_in_sol; msg.num_sv_obs = payload->num_sv_obs; From cf32cc9a293f408e8a1aed6e392a0eb04327ceee Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Wed, 22 Oct 2025 11:25:04 +0200 Subject: [PATCH 5/6] ops --- fixposition_driver_ros1/src/data_to_ros1.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 3a9d308..43ee70d 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -524,11 +524,10 @@ static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHe msg.heading_stdev = payload->heading_stdev; msg.pitch_stdev = payload->pitch_stdev; msg.rover_stn_id = fpsdk::common::string::BufToStr( - {payload->rover_stn_id, payload->rover_stn_id + sizeof(payload->rover_stn_id)}) - msg.master_stn_id = - fpsdk::common::string::BufToStr( - {payload->master_stn_id, payload->master_stn_id + sizeof(payload->master_stn_id)}) msg.num_sv_tracked = - payload->num_sv_tracked; + {payload->rover_stn_id, payload->rover_stn_id + sizeof(payload->rover_stn_id)}); + msg.master_stn_id = fpsdk::common::string::BufToStr( + {payload->master_stn_id, payload->master_stn_id + sizeof(payload->master_stn_id)}) + msg.num_sv_tracked = payload->num_sv_tracked; msg.num_sv_in_sol = payload->num_sv_in_sol; msg.num_sv_obs = payload->num_sv_obs; msg.num_sv_multi = payload->num_sv_multi; From ce6de3bb8ceb2b2925c9f0c793606fd1d98de147 Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Wed, 22 Oct 2025 15:02:57 +0200 Subject: [PATCH 6/6] uff --- fixposition_driver_ros1/src/data_to_ros1.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 43ee70d..84bbdfe 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -526,8 +526,8 @@ static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHe msg.rover_stn_id = fpsdk::common::string::BufToStr( {payload->rover_stn_id, payload->rover_stn_id + sizeof(payload->rover_stn_id)}); msg.master_stn_id = fpsdk::common::string::BufToStr( - {payload->master_stn_id, payload->master_stn_id + sizeof(payload->master_stn_id)}) - msg.num_sv_tracked = payload->num_sv_tracked; + {payload->master_stn_id, payload->master_stn_id + sizeof(payload->master_stn_id)}); + msg.num_sv_tracked = payload->num_sv_tracked; msg.num_sv_in_sol = payload->num_sv_in_sol; msg.num_sv_obs = payload->num_sv_obs; msg.num_sv_multi = payload->num_sv_multi;