From 0b07cec85d8fc578a25718ad31fd1467320ee844 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 17 Mar 2026 13:45:10 -0400 Subject: [PATCH 1/5] fix heading publishing' --- src/msgs/CMakeLists.txt | 1 + src/msgs/msg/Heading.msg | 2 + .../navigation/athena_gps/CMakeLists.txt | 2 + .../navigation/athena_gps/src/PixhawkNode.cpp | 64 ++++++++----------- 4 files changed, 33 insertions(+), 36 deletions(-) create mode 100644 src/msgs/msg/Heading.msg diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 2b9df55b..83a12abd 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Currentdraw.msg" "msg/JointReceiver.msg" "msg/SystemInfo.msg" + "msg/Heading.msg" "srv/SetController.srv" "action/NavigateToGPS.action" DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs diff --git a/src/msgs/msg/Heading.msg b/src/msgs/msg/Heading.msg new file mode 100644 index 00000000..ad2f6b28 --- /dev/null +++ b/src/msgs/msg/Heading.msg @@ -0,0 +1,2 @@ +double heading +double heading_acc diff --git a/src/subsystems/navigation/athena_gps/CMakeLists.txt b/src/subsystems/navigation/athena_gps/CMakeLists.txt index 33850750..ea72dfae 100644 --- a/src/subsystems/navigation/athena_gps/CMakeLists.txt +++ b/src/subsystems/navigation/athena_gps/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(msgs REQUIRED) find_package(MAVSDK REQUIRED) add_executable(athena_gps src/PixhawkNode.cpp) @@ -26,6 +27,7 @@ ament_target_dependencies(athena_gps rclcpp std_msgs sensor_msgs + msgs ) target_link_libraries(athena_gps MAVSDK::mavsdk diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp index c6d9a8e9..87beec27 100644 --- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp +++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp @@ -7,7 +7,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "std_msgs/msg/float64.hpp" +#include "msgs/msg/heading.hpp" #include #include @@ -54,7 +54,7 @@ class PixhawkNode : public rclcpp::Node // Create publishers unconditionally so they're always valid imu_publisher_ = this->create_publisher(imu_topic, 10); gps_publisher_ = this->create_publisher(gps_topic, 10); - heading_publisher_ = this->create_publisher(heading_topic, 10); + heading_publisher_ = this->create_publisher(heading_topic, 10); // Create MAVSDK instance mavsdk_ = std::make_unique(Mavsdk::Configuration{ @@ -78,7 +78,7 @@ class PixhawkNode : public rclcpp::Node rclcpp::Publisher::SharedPtr imu_publisher_; rclcpp::Publisher::SharedPtr gps_publisher_; - rclcpp::Publisher::SharedPtr heading_publisher_; + rclcpp::Publisher::SharedPtr heading_publisher_; rclcpp::TimerBase::SharedPtr init_timer_; Telemetry::GpsInfo last_gps_info_; @@ -94,12 +94,6 @@ class PixhawkNode : public rclcpp::Node bool is_initialized_; bool connection_added_; - void publish_heading() { - auto msg = std_msgs::msg::Float64(); - msg.data = last_heading_deg_; - heading_publisher_->publish(msg); - } - void try_initialize() { if (is_initialized_) { return; @@ -151,7 +145,6 @@ class PixhawkNode : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Telemetry rates configured successfully"); - // Subscribe to telemetry streams telemetry_->subscribe_imu([this](const Telemetry::Imu &imu) { imu_callback(imu); }); @@ -160,11 +153,6 @@ class PixhawkNode : public rclcpp::Node last_attitude_quaternion_ = quaternion; }); - telemetry_->subscribe_heading([this](Telemetry::Heading heading) { - last_heading_deg_ = heading.heading_deg; - publish_heading(); - }); - telemetry_->subscribe_raw_gps([this](const Telemetry::RawGps &raw_gps) { raw_gps_callback(raw_gps); }); @@ -228,39 +216,39 @@ class PixhawkNode : public rclcpp::Node } void raw_gps_callback(const Telemetry::RawGps raw_gps) { - auto msg = sensor_msgs::msg::NavSatFix(); + auto gps_msg = sensor_msgs::msg::NavSatFix(); - msg.header.stamp = this->now(); - msg.header.frame_id = gps_frame_id_; + gps_msg.header.stamp = this->now(); + gps_msg.header.frame_id = gps_frame_id_; - msg.latitude = raw_gps.latitude_deg; - msg.longitude = raw_gps.longitude_deg; - msg.altitude = raw_gps.altitude_ellipsoid_m; + gps_msg.latitude = raw_gps.latitude_deg; + gps_msg.longitude = raw_gps.longitude_deg; + gps_msg.altitude = raw_gps.altitude_ellipsoid_m; // Map GPS fix to ROS fix if (last_gps_info_.num_satellites > 0) { switch (last_gps_info_.fix_type) { case Telemetry::FixType::NoGps: case Telemetry::FixType::NoFix: - msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; break; case Telemetry::FixType::Fix2D: - msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; break; case Telemetry::FixType::Fix3D: case Telemetry::FixType::FixDgps: case Telemetry::FixType::RtkFloat: case Telemetry::FixType::RtkFixed: - msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX; + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX; break; default: - msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; } } else { - msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; } - msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; // calcilate variances if (!std::isnan(raw_gps.horizontal_uncertainty_m) && @@ -269,23 +257,27 @@ class PixhawkNode : public rclcpp::Node double h_variance = raw_gps.horizontal_uncertainty_m * raw_gps.horizontal_uncertainty_m; double v_variance = raw_gps.vertical_uncertainty_m * raw_gps.vertical_uncertainty_m; - msg.position_covariance[0] = h_variance; - msg.position_covariance[4] = h_variance; - msg.position_covariance[8] = v_variance; + gps_msg.position_covariance[0] = h_variance; + gps_msg.position_covariance[4] = h_variance; + gps_msg.position_covariance[8] = v_variance; // refine estimate using doppler if (!std::isnan(raw_gps.hdop) && !std::isnan(raw_gps.vdop)) { - msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0); - msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0); - msg.position_covariance[8] = v_variance * raw_gps.vdop; + gps_msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0); + gps_msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0); + gps_msg.position_covariance[8] = v_variance * raw_gps.vdop; } - msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; } else { - msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } + auto heading_msg = msgs::msg::Heading(); + heading_msg.heading = raw_gps.yaw_deg; + heading_msg.heading_acc = raw_gps.heading_uncertainty_deg; - gps_publisher_->publish(msg); + heading_publisher_->publish(heading_msg); + gps_publisher_->publish(gps_msg); } }; From c10caffde83bc13ec1e0da045ebdea98577cd32c Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 17 Mar 2026 13:52:42 -0400 Subject: [PATCH 2/5] misread the docs on float64 --- src/msgs/msg/Heading.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/msgs/msg/Heading.msg b/src/msgs/msg/Heading.msg index ad2f6b28..ce6d9213 100644 --- a/src/msgs/msg/Heading.msg +++ b/src/msgs/msg/Heading.msg @@ -1,2 +1,2 @@ -double heading -double heading_acc +float64 heading +float64 heading_acc From 836b0ce88397e4431240838b55ab61064ffd4bc2 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 17 Mar 2026 13:59:11 -0400 Subject: [PATCH 3/5] adhere to ros gps convention --- src/msgs/msg/Heading.msg | 5 +++++ src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp | 6 ++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/msgs/msg/Heading.msg b/src/msgs/msg/Heading.msg index ce6d9213..ca315b47 100644 --- a/src/msgs/msg/Heading.msg +++ b/src/msgs/msg/Heading.msg @@ -1,2 +1,7 @@ +# Heading in ROS convention: 0 = East, increases counter-clockwise, radians. +# Note: this differs from compass bearing (0 = North, clockwise, degrees). +# The driver converts from compass bearing via: heading = (90 - bearing_deg) * pi / 180 + +std_msgs/Header header float64 heading float64 heading_acc diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp index 87beec27..86717791 100644 --- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp +++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp @@ -273,8 +273,10 @@ class PixhawkNode : public rclcpp::Node gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } auto heading_msg = msgs::msg::Heading(); - heading_msg.heading = raw_gps.yaw_deg; - heading_msg.heading_acc = raw_gps.heading_uncertainty_deg; + heading_msg.header.stamp = this->now(); + // Convert from compass bearing (0=North, CW, degrees) to ROS convention (0=East, CCW, radians) + heading_msg.heading = (90.0 - raw_gps.yaw_deg) * M_PI / 180.0; + heading_msg.heading_acc = raw_gps.heading_uncertainty_deg * M_PI / 180.0; heading_publisher_->publish(heading_msg); gps_publisher_->publish(gps_msg); From 9a00b445c27201f6be04662c25cc739dd4590969 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 23 Mar 2026 22:35:10 -0400 Subject: [PATCH 4/5] add raw compass bearing --- src/msgs/msg/Heading.msg | 7 ++++--- src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp | 3 ++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/msgs/msg/Heading.msg b/src/msgs/msg/Heading.msg index ca315b47..7f2007c9 100644 --- a/src/msgs/msg/Heading.msg +++ b/src/msgs/msg/Heading.msg @@ -1,7 +1,8 @@ -# Heading in ROS convention: 0 = East, increases counter-clockwise, radians. -# Note: this differs from compass bearing (0 = North, clockwise, degrees). -# The driver converts from compass bearing via: heading = (90 - bearing_deg) * pi / 180 +# heading: ROS convention (0=East, CCW, radians). Converted from compass bearing, +# but does not account for magnetic declination. +# compass_bearing: raw magnetometer reading (0=magnetic North, CW, degrees). std_msgs/Header header float64 heading float64 heading_acc +float64 compass_bearing diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp index 86717791..545129b5 100644 --- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp +++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp @@ -274,7 +274,8 @@ class PixhawkNode : public rclcpp::Node } auto heading_msg = msgs::msg::Heading(); heading_msg.header.stamp = this->now(); - // Convert from compass bearing (0=North, CW, degrees) to ROS convention (0=East, CCW, radians) + heading_msg.compass_bearing = raw_gps.yaw_deg; + // Convert from compass bearing (0=magnetic North, CW, degrees) to ROS convention (0=East, CCW, radians) heading_msg.heading = (90.0 - raw_gps.yaw_deg) * M_PI / 180.0; heading_msg.heading_acc = raw_gps.heading_uncertainty_deg * M_PI / 180.0; From d945ab767cfd20e2b5b5ee48eb8e232b8c19dd48 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 24 Mar 2026 01:24:50 -0400 Subject: [PATCH 5/5] add to package.xml --- src/subsystems/navigation/athena_gps/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/src/subsystems/navigation/athena_gps/package.xml b/src/subsystems/navigation/athena_gps/package.xml index 2138e4c3..2cd9b998 100644 --- a/src/subsystems/navigation/athena_gps/package.xml +++ b/src/subsystems/navigation/athena_gps/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs sensor_msgs + msgs ament_lint_auto ament_lint_common