From 9eb50200b23ea412bd103f2481c8d2182f420d3b Mon Sep 17 00:00:00 2001
From: Mohammad Durrani <mdurrani808@gmail.com>
Date: Fri, 8 May 2026 23:07:28 -0400
Subject: [PATCH] removed bad heading publsiher

---
 .../navigation/athena_gps/CMakeLists.txt          |  2 --
 .../navigation/athena_gps/src/PixhawkNode.cpp     | 15 ---------------
 2 files changed, 17 deletions(-)

diff --git a/src/subsystems/navigation/athena_gps/CMakeLists.txt b/src/subsystems/navigation/athena_gps/CMakeLists.txt
index ea72dfae..33850750 100644
--- a/src/subsystems/navigation/athena_gps/CMakeLists.txt
+++ b/src/subsystems/navigation/athena_gps/CMakeLists.txt
@@ -14,7 +14,6 @@ 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)
 
@@ -27,7 +26,6 @@ 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 545129b5..c7d3d1b7 100644
--- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
+++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
@@ -7,7 +7,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "sensor_msgs/msg/nav_sat_fix.hpp"
-#include "msgs/msg/heading.hpp"
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
@@ -21,7 +20,6 @@ class PixhawkNode : public rclcpp::Node
     : Node("athena_gps"),
       last_gps_info_{},
       last_attitude_quaternion_{},
-      last_heading_deg_{},
       is_initialized_(false),
       connection_added_(false)
     {
@@ -33,7 +31,6 @@ class PixhawkNode : public rclcpp::Node
         this->declare_parameter("gps_frame_id", "gps_link");
         this->declare_parameter("imu_topic", "imu/data");
         this->declare_parameter("gps_topic", "gps/fix");
-        this->declare_parameter("heading_topic", "heading");
         this->declare_parameter("mavsdk_system_id", 10);
         this->declare_parameter("mavsdk_component_id", 1);
         this->declare_parameter("mavsdk_always_send_heartbeats", false);
@@ -46,7 +43,6 @@ class PixhawkNode : public rclcpp::Node
         gps_frame_id_ = this->get_parameter("gps_frame_id").as_string();
         std::string imu_topic = this->get_parameter("imu_topic").as_string();
         std::string gps_topic = this->get_parameter("gps_topic").as_string();
-        std::string heading_topic = this->get_parameter("heading_topic").as_string();
         int mavsdk_system_id = this->get_parameter("mavsdk_system_id").as_int();
         int mavsdk_component_id = this->get_parameter("mavsdk_component_id").as_int();
         bool mavsdk_always_send_heartbeats = this->get_parameter("mavsdk_always_send_heartbeats").as_bool();
@@ -54,7 +50,6 @@ class PixhawkNode : public rclcpp::Node
         // Create publishers unconditionally so they're always valid
         imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_topic, 10);
         gps_publisher_ = this->create_publisher<sensor_msgs::msg::NavSatFix>(gps_topic, 10);
-        heading_publisher_ = this->create_publisher<msgs::msg::Heading>(heading_topic, 10);
 
         // Create MAVSDK instance
         mavsdk_ = std::make_unique<Mavsdk>(Mavsdk::Configuration{
@@ -78,12 +73,10 @@ class PixhawkNode : public rclcpp::Node
 
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_publisher_;
-    rclcpp::Publisher<msgs::msg::Heading>::SharedPtr heading_publisher_;
     rclcpp::TimerBase::SharedPtr init_timer_;
 
     Telemetry::GpsInfo last_gps_info_;
     Telemetry::Quaternion last_attitude_quaternion_;
-    double last_heading_deg_;
 
     std::string imu_frame_id_;
     std::string gps_frame_id_;
@@ -272,14 +265,6 @@ class PixhawkNode : public rclcpp::Node
         } else {
             gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
         }
-        auto heading_msg = msgs::msg::Heading();
-        heading_msg.header.stamp = this->now();
-        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;
-
-        heading_publisher_->publish(heading_msg);
         gps_publisher_->publish(gps_msg);
     }
 };