Skip to content

Commit 9f3a1d0

Browse files
grizzigrizzi
authored
Add NOV_B-HEADING2 output (#95)
1 parent 778cb51 commit 9f3a1d0

File tree

11 files changed

+146
-2
lines changed

11 files changed

+146
-2
lines changed

‎fixposition_driver_msgs/msg/NovbHeading2.msg‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
2+
# License: see the LICENSE file
3+
#
4+
# NOV_B-HEADING2 data
5+
6+
std_msgs/Header header
7+
uint32 sol_status # See NovbGnssSolStat
8+
uint32 pos_type # See NovbPosOrVelType
9+
float32 length # Baseline length (m)
10+
float32 heading # Heading (degrees)
11+
float32 pitch # Pitch (degrees)
12+
float32 reserved # Reserved
13+
float32 heading_stdev # Heading standard deviation (degrees)
14+
float32 pitch_stdev # Pitch standard deviation (degrees)
15+
string rover_stn_id # Rover station ID
16+
string master_stn_id # Master station ID
17+
uint8 num_sv_tracked # Number of satellites tracked
18+
uint8 num_sv_in_sol # Number of satellites in solution
19+
uint8 num_sv_obs # Number of satellites with observations
20+
uint8 num_sv_multi # Number of satellites with multi-frequency observations
21+
uint8 sol_source # Solution source (see NovbSolSource)
22+
uint8 ext_sol_status # Extended solution status (see NovbGnssSolExtStat)
23+
uint8 galileo_beidou_sig_mask # Galileo and BeiDou signal mask (see NovbSigUsedGalBds)
24+
uint8 gps_glonass_sig_mask # GPS and GLONASS signal mask (see NovbSigUsedGpsGlo)

‎fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,8 @@ bool PublishNovbBestgnsspos(const fpsdk::common::parser::novb::NovbHeader* heade
5858
ros::Publisher& pub2);
5959
bool PublishNovbInspvax(const fpsdk::common::parser::novb::NovbHeader* header,
6060
const fpsdk::common::parser::novb::NovbInspvax* payload, ros::Publisher& pub);
61+
bool PublishNovbHeading2(const fpsdk::common::parser::novb::NovbHeader* header,
62+
const fpsdk::common::parser::novb::NovbHeading2* payload, ros::Publisher& pub);
6163

6264
void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, ros::Publisher& pub);
6365
void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, ros::Publisher& pub);

‎fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,8 @@ class FixpositionDriverNode {
8686
ros::Publisher nmea_vtg_pub_; //!< NMEA-GP-VTG message
8787
ros::Publisher nmea_zda_pub_; //!< NMEA-GP-ZDA message
8888
// - NOV_B messages
89-
ros::Publisher novb_inspvax_pub_; //!< NOV_B-INSPVAX message
89+
ros::Publisher novb_inspvax_pub_; //!< NOV_B-INSPVAX message
90+
ros::Publisher novb_heading2_pub_; //!< NOV_B-HEADING2 message
9091
// - Odometry
9192
ros::Publisher odometry_ecef_pub_; //!< ECEF odometry
9293
ros::Publisher odometry_enu_pub_; //!< ENU odometry

‎fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@
5555
#include <fixposition_driver_msgs/NmeaVtg.h>
5656
#include <fixposition_driver_msgs/NmeaZda.h>
5757
// - NOV-B
58+
#include <fixposition_driver_msgs/NovbHeading2.h>
5859
#include <fixposition_driver_msgs/NovbInspvax.h>
5960

6061
#pragma GCC diagnostic pop

‎fixposition_driver_ros1/src/data_to_ros1.cpp‎

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -506,6 +506,49 @@ bool PublishNovbInspvax(const novb::NovbHeader* header, const novb::NovbInspvax*
506506

507507
// ---------------------------------------------------------------------------------------------------------------------
508508

509+
static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHeading2* payload,
510+
fixposition_driver_msgs::NovbHeading2& msg) {
511+
if ((header != NULL) && (payload != NULL) && header->IsLongHeader()) {
512+
time::Time stamp;
513+
if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3,
514+
time::WnoTow::Sys::GPS})) {
515+
msg.header.stamp = ros1::utils::ConvTime(stamp);
516+
}
517+
518+
msg.sol_status = payload->sol_status;
519+
msg.pos_type = payload->pos_type;
520+
msg.length = payload->length;
521+
msg.heading = payload->heading;
522+
msg.pitch = payload->pitch;
523+
msg.reserved = payload->reserved;
524+
msg.heading_stdev = payload->heading_stdev;
525+
msg.pitch_stdev = payload->pitch_stdev;
526+
msg.rover_stn_id = fpsdk::common::string::BufToStr(
527+
{payload->rover_stn_id, payload->rover_stn_id + sizeof(payload->rover_stn_id)});
528+
msg.master_stn_id = fpsdk::common::string::BufToStr(
529+
{payload->master_stn_id, payload->master_stn_id + sizeof(payload->master_stn_id)});
530+
msg.num_sv_tracked = payload->num_sv_tracked;
531+
msg.num_sv_in_sol = payload->num_sv_in_sol;
532+
msg.num_sv_obs = payload->num_sv_obs;
533+
msg.num_sv_multi = payload->num_sv_multi;
534+
msg.sol_source = payload->sol_source;
535+
msg.ext_sol_status = payload->ext_sol_status;
536+
msg.galileo_beidou_sig_mask = payload->galileo_beidou_sig_mask;
537+
msg.gps_glonass_sig_mask = payload->gps_glonass_sig_mask;
538+
}
539+
}
540+
541+
bool PublishNovbHeading2(const novb::NovbHeader* header, const novb::NovbHeading2* payload, ros::Publisher& pub) {
542+
if (pub.getNumSubscribers() > 0) {
543+
fixposition_driver_msgs::NovbHeading2 msg;
544+
NovbHeading2ToMsg(header, payload, msg);
545+
pub.publish(msg);
546+
}
547+
return true;
548+
}
549+
550+
// ---------------------------------------------------------------------------------------------------------------------
551+
509552
void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, ros::Publisher& pub) {
510553
if (pub.getNumSubscribers() > 0) {
511554
fixposition_driver_msgs::NmeaGga msg;

‎fixposition_driver_ros1/src/fixposition_driver_node.cpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -277,6 +277,17 @@ bool FixpositionDriverNode::StartNode() {
277277
});
278278
}
279279

280+
// NOV_B-HEADING2
281+
if (params_.MessageEnabled(novb::NOV_B_HEADING2_STRID)) {
282+
_PUB(novb_heading2_pub_, fixposition_driver_msgs::NovbHeading2, output_ns + "/novb/heading2", 5);
283+
driver_.AddNovbObserver( //
284+
novb::NOV_B_HEADING2_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) {
285+
if (!PublishNovbHeading2(header, (novb::NovbHeading2*)payload, novb_heading2_pub_)) {
286+
ROS_WARN_THROTTLE(1.0, "Bad NOV_B-HEADING2");
287+
}
288+
});
289+
}
290+
280291
// NMEA-GP-GGA
281292
if (params_.MessageEnabled(nmea::NmeaGgaPayload::FORMATTER)) {
282293
_PUB(nmea_gga_pub_, fixposition_driver_msgs::NmeaGga, output_ns + "/nmea/gga", 5);
@@ -480,6 +491,7 @@ void FixpositionDriverNode::StopNode() {
480491
nmea_zda_pub_.shutdown();
481492
// - NOV_B messages
482493
novb_inspvax_pub_.shutdown();
494+
novb_heading2_pub_.shutdown();
483495
// - Odometry
484496
odometry_ecef_pub_.shutdown();
485497
odometry_enu_pub_.shutdown();

‎fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,9 @@ bool PublishNovbBestgnsspos(const fpsdk::common::parser::novb::NovbHeader* heade
7474
bool PublishNovbInspvax(const fpsdk::common::parser::novb::NovbHeader* header,
7575
const fpsdk::common::parser::novb::NovbInspvax* payload,
7676
rclcpp::Publisher<fpmsgs::NovbInspvax>::SharedPtr& pub);
77+
bool PublishNovbHeading2(const fpsdk::common::parser::novb::NovbHeader* header,
78+
const fpsdk::common::parser::novb::NovbHeading2* payload,
79+
rclcpp::Publisher<fpmsgs::NovbHeading2>::SharedPtr& pub);
7780

7881
void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload,
7982
rclcpp::Publisher<fpmsgs::NmeaGga>::SharedPtr& pub);

‎fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,8 @@ class FixpositionDriverNode {
9090
rclcpp::Publisher<fpmsgs::NmeaVtg>::SharedPtr nmea_vtg_pub_; //!< NMEA-GP-VTG message
9191
rclcpp::Publisher<fpmsgs::NmeaZda>::SharedPtr nmea_zda_pub_; //!< NMEA-GP-ZDA message
9292
// - NOV_B messages
93-
rclcpp::Publisher<fpmsgs::NovbInspvax>::SharedPtr novb_inspvax_pub_; //!< NOV_B-INSPVAX message
93+
rclcpp::Publisher<fpmsgs::NovbInspvax>::SharedPtr novb_inspvax_pub_; //!< NOV_B-INSPVAX message
94+
rclcpp::Publisher<fpmsgs::NovbHeading2>::SharedPtr novb_heading2_pub_; //!< NOV_B-HEADING2 message
9495
// - Odometry
9596
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_ecef_pub_; //!< ECEF odometry
9697
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu_pub_; //!< ENU odometry

‎fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@
5757
#include <fixposition_driver_msgs/msg/nmea_vtg.hpp>
5858
#include <fixposition_driver_msgs/msg/nmea_zda.hpp>
5959
// - NOV-B
60+
#include <fixposition_driver_msgs/msg/novb_heading2.hpp>
6061
#include <fixposition_driver_msgs/msg/novb_inspvax.hpp>
6162

6263
// Shortcut

‎fixposition_driver_ros2/src/data_to_ros2.cpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -516,6 +516,50 @@ bool PublishNovbInspvax(const novb::NovbHeader* header, const novb::NovbInspvax*
516516

517517
// ---------------------------------------------------------------------------------------------------------------------
518518

519+
static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHeading2* payload,
520+
fpmsgs::NovbHeading2& msg) {
521+
if ((header != NULL) && (payload != NULL) && header->IsLongHeader()) {
522+
time::Time stamp;
523+
if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3,
524+
time::WnoTow::Sys::GPS})) {
525+
msg.header.stamp = ros2::utils::ConvTime(stamp);
526+
}
527+
528+
msg.sol_status = payload->sol_status;
529+
msg.pos_type = payload->pos_type;
530+
msg.length = payload->length;
531+
msg.heading = payload->heading;
532+
msg.pitch = payload->pitch;
533+
msg.reserved = payload->reserved;
534+
msg.heading_stdev = payload->heading_stdev;
535+
msg.pitch_stdev = payload->pitch_stdev;
536+
msg.rover_stn_id = fpsdk::common::string::BufToStr(
537+
{payload->rover_stn_id, payload->rover_stn_id + sizeof(payload->rover_stn_id)});
538+
msg.master_stn_id = fpsdk::common::string::BufToStr(
539+
{payload->master_stn_id, payload->master_stn_id + sizeof(payload->master_stn_id)});
540+
msg.num_sv_tracked = payload->num_sv_tracked;
541+
msg.num_sv_in_sol = payload->num_sv_in_sol;
542+
msg.num_sv_obs = payload->num_sv_obs;
543+
msg.num_sv_multi = payload->num_sv_multi;
544+
msg.sol_source = payload->sol_source;
545+
msg.ext_sol_status = payload->ext_sol_status;
546+
msg.galileo_beidou_sig_mask = payload->galileo_beidou_sig_mask;
547+
msg.gps_glonass_sig_mask = payload->gps_glonass_sig_mask;
548+
}
549+
}
550+
551+
bool PublishNovbHeading2(const novb::NovbHeader* header, const novb::NovbHeading2* payload,
552+
rclcpp::Publisher<fpmsgs::NovbHeading2>::SharedPtr& pub) {
553+
if (pub->get_subscription_count() > 0) {
554+
fpmsgs::NovbHeading2 msg;
555+
NovbHeading2ToMsg(header, payload, msg);
556+
pub->publish(msg);
557+
}
558+
return true;
559+
}
560+
561+
// ---------------------------------------------------------------------------------------------------------------------
562+
519563
void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload,
520564
rclcpp::Publisher<fpmsgs::NmeaGga>::SharedPtr& pub) {
521565
if (pub->get_subscription_count() > 0) {

0 commit comments

Comments
 (0)