Implementation of novb_heading2

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)
+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
+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)

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);

fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp

@@ -86,7 +86,8 @@ 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
     ros::Publisher odometry_enu_pub_;         //!< ENU odometry

fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp

@@ -55,6 +55,7 @@
 #include <fixposition_driver_msgs/NmeaVtg.h>
 #include <fixposition_driver_msgs/NmeaZda.h>
 // - NOV-B
+#include <fixposition_driver_msgs/NovbHeading2.h>
 #include <fixposition_driver_msgs/NovbInspvax.h>
 
 #pragma GCC diagnostic pop

fixposition_driver_ros1/src/data_to_ros1.cpp

@@ -506,6 +506,49 @@ 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;
+        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;
+        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;

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();

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<fpmsgs::NovbInspvax>::SharedPtr& pub);
+bool PublishNovbHeading2(const fpsdk::common::parser::novb::NovbHeader* header,
+                         const fpsdk::common::parser::novb::NovbHeading2* payload,
+                         rclcpp::Publisher<fpmsgs::NovbHeading2>::SharedPtr& pub);
 
 void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload,
                     rclcpp::Publisher<fpmsgs::NmeaGga>::SharedPtr& pub);

fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp

@@ -90,7 +90,8 @@ class FixpositionDriverNode {
     rclcpp::Publisher<fpmsgs::NmeaVtg>::SharedPtr nmea_vtg_pub_;  //!< NMEA-GP-VTG message
     rclcpp::Publisher<fpmsgs::NmeaZda>::SharedPtr nmea_zda_pub_;  //!< NMEA-GP-ZDA message
     // - NOV_B messages
-    rclcpp::Publisher<fpmsgs::NovbInspvax>::SharedPtr novb_inspvax_pub_;  //!< NOV_B-INSPVAX message
+    rclcpp::Publisher<fpmsgs::NovbInspvax>::SharedPtr novb_inspvax_pub_;    //!< NOV_B-INSPVAX message
+    rclcpp::Publisher<fpmsgs::NovbHeading2>::SharedPtr novb_heading2_pub_;  //!< NOV_B-HEADING2 message
     // - Odometry
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_ecef_pub_;        //!< ECEF odometry
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu_pub_;         //!< ENU odometry

fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp

@@ -57,6 +57,7 @@
 #include <fixposition_driver_msgs/msg/nmea_vtg.hpp>
 #include <fixposition_driver_msgs/msg/nmea_zda.hpp>
 // - NOV-B
+#include <fixposition_driver_msgs/msg/novb_heading2.hpp>
 #include <fixposition_driver_msgs/msg/novb_inspvax.hpp>
 
 // Shortcut

fixposition_driver_ros2/src/data_to_ros2.cpp

@@ -516,6 +516,50 @@ 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;
+        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;
+        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<fpmsgs::NovbHeading2>::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<fpmsgs::NmeaGga>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {

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();