diff --git a/fixposition_driver_msgs/msg/NovbHeading2.msg b/fixposition_driver_msgs/msg/NovbHeading2.msg new file mode 100644 index 0000000..7c9c79b --- /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) +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) 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..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,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 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..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,6 +55,7 @@ #include #include // - NOV-B +#include #include #pragma GCC diagnostic pop diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 44e5222..84bbdfe 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/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; 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..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,8 @@ 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 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..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,6 +57,7 @@ #include #include // - NOV-B +#include #include // Shortcut diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 13f590c..94c2b69 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/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::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();