From b73fdaf208527ddf686bcc5d59fed7d625cb1628 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Wed, 28 Jan 2026 14:01:55 -0500 Subject: [PATCH 1/4] added gps node --- dependencies.sh | 20 +- .../navigation/athena_gps/CMakeLists.txt | 32 +++ .../navigation/athena_gps/package.xml | 19 ++ .../navigation/athena_gps/src/PixhawkNode.cpp | 212 ++++++++++++++++++ 4 files changed, 282 insertions(+), 1 deletion(-) create mode 100644 src/subsystems/navigation/athena_gps/CMakeLists.txt create mode 100644 src/subsystems/navigation/athena_gps/package.xml create mode 100644 src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp diff --git a/dependencies.sh b/dependencies.sh index 40b470f..1be24a4 100755 --- a/dependencies.sh +++ b/dependencies.sh @@ -1,13 +1,31 @@ #!/bin/bash set -euo pipefail +ARCH=$(dpkg --print-architecture) +VERSION_ID=$(grep '^VERSION_ID=' /etc/os-release | cut -d'=' -f2 | tr -d '"') +MAVSDK_VERSION="3.14.0" + +echo "Architecture: $ARCH" +echo "Version ID: $VERSION_ID" pkgs=( python3-colcon-common-extensions libgeographic-dev geographiclib-tools + wget ) for pkg in "${pkgs[@]}"; do + echo "Installing $pkg" apt-get install -y "$pkg" -done \ No newline at end of file +done + +echo "Installing MAVSDK version $MAVSDK_VERSION" +if [[ "$ARCH" == "amd64" ]]; then + PACKAGE="libmavsdk-dev_${MAVSDK_VERSION}_ubuntu${VERSION_ID}_amd64.deb" +else + PACKAGE="libmavsdk-dev_${MAVSDK_VERSION}_debian12_${ARCH}.deb" +fi + +wget https://github.com/mavlink/MAVSDK/releases/download/v${MAVSDK_VERSION}/${PACKAGE} +sudo dpkg -i ${PACKAGE} \ No newline at end of file diff --git a/src/subsystems/navigation/athena_gps/CMakeLists.txt b/src/subsystems/navigation/athena_gps/CMakeLists.txt new file mode 100644 index 0000000..cbd84de --- /dev/null +++ b/src/subsystems/navigation/athena_gps/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(athena_gps) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(MAVSDK REQUIRED) +add_executable(athena_gps src/PixhawkNode.cpp) + +ament_target_dependencies(athena_gps + rclcpp + std_msgs + sensor_msgs +) +target_link_libraries(athena_gps + MAVSDK::mavsdk +) +install(TARGETS + athena_gps + DESTINATION lib/${PROJECT_NAME}) + +ament_package() \ No newline at end of file diff --git a/src/subsystems/navigation/athena_gps/package.xml b/src/subsystems/navigation/athena_gps/package.xml new file mode 100644 index 0000000..8b38c76 --- /dev/null +++ b/src/subsystems/navigation/athena_gps/package.xml @@ -0,0 +1,19 @@ + + + + athena_gps + 0.0.0 + TODO: Package description + mdurrani + TODO: License declaration + + ament_cmake + rclcpp + std_msgs + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp new file mode 100644 index 0000000..035bd62 --- /dev/null +++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp @@ -0,0 +1,212 @@ +#include +#include +#include +#include +#include + +#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 +#include + +using namespace std::chrono_literals; +using namespace mavsdk; + +class PixhawkNode : public rclcpp::Node +{ +public: + PixhawkNode() + : Node("minimal_publisher"), last_gps_info_{}, last_attitude_quaternion_{}, last_heading_deg_{} + { + mavsdk_ = std::make_unique(Mavsdk::Configuration{10, 1, false}); + // this must be configured in the launch file + this->declare_parameter("usb_path", "/dev/ttyACM0"); + ConnectionResult conn_result = mavsdk_->add_any_connection(this->get_parameter("usb_path").as_string()); + if (conn_result != ConnectionResult::Success) { + RCLCPP_ERROR(this->get_logger(), "Failed to connect to Pixhawk! Please check physical connection."); + return; + } + RCLCPP_INFO(this->get_logger(), "Connection to pixhawk successful."); + + while (mavsdk_->systems().empty()) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Waiting for system connection..."); + rclcpp::sleep_for(std::chrono::seconds(1)); + } + + auto system = mavsdk_->systems().at(0); + telemetry_ = std::make_shared(system); + + Telemetry::Result imu_result = telemetry_->set_rate_imu(10.0); + Telemetry::Result gps_info_result = telemetry_->set_rate_gps_info(1.0); + Telemetry::Result attitude_result = telemetry_->set_rate_attitude_quaternion(10.0); + + if (imu_result != Telemetry::Result::Success) { + RCLCPP_ERROR(this->get_logger(), "Failed to set IMU rate"); + return; + } + + if (gps_info_result != Telemetry::Result::Success) { + RCLCPP_ERROR(this->get_logger(), "Failed to set GPS info rate"); + return; + } + + if (attitude_result != Telemetry::Result::Success) { + RCLCPP_ERROR(this->get_logger(), "Failed to set attitude quaternion rate"); + return; + } + + // Create publishers + imu_publisher_ = this->create_publisher("imu/data", 10); + gps_publisher_ = this->create_publisher("gps/fix", 10); + heading_publisher_ = this->create_publisher("heading", 10); + + // Subscribe to IMU data + telemetry_->subscribe_imu([this](const Telemetry::Imu &imu) { + imu_callback(imu); + }); + + // Subscribe to attitude quaternion + telemetry_->subscribe_attitude_quaternion([this](const Telemetry::Quaternion &quaternion) { + last_attitude_quaternion_ = quaternion; + }); + + // Subscribe to heading + telemetry_->subscribe_heading([this](Telemetry::Heading heading) { + last_heading_deg_ = heading.heading_deg; + publish_heading(); + }); + + // Subscribe to raw GPS + telemetry_->subscribe_raw_gps([this](const Telemetry::RawGps &raw_gps) { + raw_gps_callback(raw_gps); + }); + + // Subscribe to GPS info for fix type + telemetry_->subscribe_gps_info([this](const Telemetry::GpsInfo &gps_info) { + last_gps_info_ = gps_info; + }); + } + +private: + std::unique_ptr mavsdk_; + std::shared_ptr telemetry_; + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp::Publisher::SharedPtr gps_publisher_; + rclcpp::Publisher::SharedPtr heading_publisher_; + Telemetry::GpsInfo last_gps_info_; + Telemetry::Quaternion last_attitude_quaternion_; + double last_heading_deg_; + + void publish_heading() { + auto msg = std_msgs::msg::Float64(); + msg.data = last_heading_deg_; + heading_publisher_->publish(msg); + } + + void imu_callback(const Telemetry::Imu imu) { + auto msg = sensor_msgs::msg::Imu(); + msg.header.stamp = this->now(); + msg.header.frame_id = "imu_link"; + + // Use the full quaternion from attitude + msg.orientation.w = last_attitude_quaternion_.w; + msg.orientation.x = last_attitude_quaternion_.x; + msg.orientation.y = last_attitude_quaternion_.y; + msg.orientation.z = last_attitude_quaternion_.z; + + msg.angular_velocity.x = imu.angular_velocity_frd.forward_rad_s; + msg.angular_velocity.y = imu.angular_velocity_frd.right_rad_s; + msg.angular_velocity.z = imu.angular_velocity_frd.down_rad_s; + + msg.linear_acceleration.x = imu.acceleration_frd.forward_m_s2; + msg.linear_acceleration.y = imu.acceleration_frd.right_m_s2; + msg.linear_acceleration.z = imu.acceleration_frd.down_m_s2; + + // Set covariances + for(int i = 0; i < 9; i++) { + msg.angular_velocity_covariance[i] = 0.0; + msg.linear_acceleration_covariance[i] = 0.0; + // Only set orientation covariance to -1 if we haven't received quaternion data yet + msg.orientation_covariance[i] = (last_attitude_quaternion_.w == 0.0 && + last_attitude_quaternion_.x == 0.0 && + last_attitude_quaternion_.y == 0.0 && + last_attitude_quaternion_.z == 0.0) ? -1 : 0.0; + } + + imu_publisher_->publish(msg); + } + + void raw_gps_callback(const Telemetry::RawGps raw_gps) { + auto msg = sensor_msgs::msg::NavSatFix(); + + auto unix_epoch_time = std::chrono::microseconds(raw_gps.timestamp_us); + auto ros_time = rclcpp::Time(unix_epoch_time.count() * 1000); // Convert to nanoseconds + + msg.header.stamp = ros_time; + msg.header.frame_id = "gps_link"; + + msg.latitude = raw_gps.latitude_deg; + msg.longitude = raw_gps.longitude_deg; + msg.altitude = raw_gps.altitude_ellipsoid_m; // Using ellipsoid altitude + + // Set the status based on GPS info + 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; + break; + case Telemetry::FixType::Fix2D: + 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; + break; + default: + msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + } + } else { + msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + } + + msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + + // Set position covariance using actual uncertainties + if (!std::isnan(raw_gps.horizontal_uncertainty_m) && + !std::isnan(raw_gps.vertical_uncertainty_m)) { + // Convert uncertainties to variances + 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; // xx + msg.position_covariance[4] = h_variance; // yy (assuming isotropic horizontal uncertainty) + msg.position_covariance[8] = v_variance; // zz + + // If we have HDOP and VDOP, we can make the horizontal covariance more accurate + if (!std::isnan(raw_gps.hdop) && !std::isnan(raw_gps.vdop)) { + msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0); // xx + msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0); // yy + msg.position_covariance[8] = v_variance * raw_gps.vdop; // zz + } + + 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_publisher_->publish(msg); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 6593feb82baa68634e74d6275eac360e40afda1f Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 29 Jan 2026 14:00:38 -0500 Subject: [PATCH 2/4] cleaned up GPS code, added parameterization, moved sim bridge to gps launch --- src/simulation/launch/bridge.launch.py | 20 --- .../navigation/athena_gps/CMakeLists.txt | 5 + .../athena_gps/config/pixhawk_params.yaml | 18 +++ .../athena_gps/launch/gps_launch.py | 63 ++++++++ .../navigation/athena_gps/package.xml | 3 + .../navigation/athena_gps/src/PixhawkNode.cpp | 150 ++++++++++++------ 6 files changed, 187 insertions(+), 72 deletions(-) create mode 100644 src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml create mode 100644 src/subsystems/navigation/athena_gps/launch/gps_launch.py diff --git a/src/simulation/launch/bridge.launch.py b/src/simulation/launch/bridge.launch.py index 6a7df09..9db637b 100644 --- a/src/simulation/launch/bridge.launch.py +++ b/src/simulation/launch/bridge.launch.py @@ -3,26 +3,6 @@ def generate_launch_description(): return LaunchDescription([ - Node( - package='ros_gz_bridge', - executable='parameter_bridge', - name='imu_bridge', - output='screen', - arguments=[ - '/imu@sensor_msgs/msg/Imu[gz.msgs.IMU', - ], - ), - - Node( - package='ros_gz_bridge', - executable='parameter_bridge', - name='gps_bridge', - output='screen', - arguments=[ - '/gps/fix@sensor_msgs/msg/NavSatFix@gz.msgs.NavSat', - ], - ), - Node( package='ros_gz_bridge', executable='parameter_bridge', diff --git a/src/subsystems/navigation/athena_gps/CMakeLists.txt b/src/subsystems/navigation/athena_gps/CMakeLists.txt index cbd84de..3385075 100644 --- a/src/subsystems/navigation/athena_gps/CMakeLists.txt +++ b/src/subsystems/navigation/athena_gps/CMakeLists.txt @@ -17,6 +17,11 @@ find_package(sensor_msgs REQUIRED) find_package(MAVSDK REQUIRED) add_executable(athena_gps src/PixhawkNode.cpp) +install( + DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + ament_target_dependencies(athena_gps rclcpp std_msgs diff --git a/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml b/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml new file mode 100644 index 0000000..0513f21 --- /dev/null +++ b/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml @@ -0,0 +1,18 @@ +athena_gps: + ros__parameters: + usb_path: "/dev/ttyACM0" + + imu_rate: 10.0 + gps_info_rate: 1.0 + attitude_rate: 10.0 + + imu_frame_id: "imu_link" + gps_frame_id: "gps_link" + + imu_topic: "imu/data" + gps_topic: "gps/fix" + heading_topic: "heading" + + mavsdk_system_id: 10 + mavsdk_component_id: 1 + mavsdk_always_send_heartbeats: false diff --git a/src/subsystems/navigation/athena_gps/launch/gps_launch.py b/src/subsystems/navigation/athena_gps/launch/gps_launch.py new file mode 100644 index 0000000..9b682b6 --- /dev/null +++ b/src/subsystems/navigation/athena_gps/launch/gps_launch.py @@ -0,0 +1,63 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + + pkg_dir = get_package_share_directory('athena_gps') + config_file = os.path.join(pkg_dir, 'config', 'pixhawk_params.yaml') + + sim_arg = DeclareLaunchArgument( + 'sim', + default_value='false', + choices=['true', 'false'], + description='Use simulation bridge instead of real hardware' + ) + + pixhawk_node = Node( + package='athena_gps', + executable='athena_gps', + name='athena_gps', + output='screen', + parameters=[ + config_file, + ], + respawn=True, + respawn_delay=2.0, + condition=UnlessCondition(LaunchConfiguration('sim')) + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + arguments=[ + '/imu@sensor_msgs/msg/Imu[gz.msgs.IMU', + ], + condition=IfCondition(LaunchConfiguration('sim')) + ) + + gps_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='gps_bridge', + output='screen', + arguments=[ + '/gps/fix@sensor_msgs/msg/NavSatFix@gz.msgs.NavSat', + ], + condition=IfCondition(LaunchConfiguration('sim')) + ) + + + return LaunchDescription([ + sim_arg, + pixhawk_node, + imu_bridge, + gps_bridge, + ]) diff --git a/src/subsystems/navigation/athena_gps/package.xml b/src/subsystems/navigation/athena_gps/package.xml index 8b38c76..2138e4c 100644 --- a/src/subsystems/navigation/athena_gps/package.xml +++ b/src/subsystems/navigation/athena_gps/package.xml @@ -8,8 +8,11 @@ TODO: License declaration ament_cmake + rclcpp std_msgs + sensor_msgs + ament_lint_auto ament_lint_common diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp index 035bd62..4ad96c2 100644 --- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp +++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp @@ -18,17 +18,46 @@ class PixhawkNode : public rclcpp::Node { public: PixhawkNode() - : Node("minimal_publisher"), last_gps_info_{}, last_attitude_quaternion_{}, last_heading_deg_{} + : Node("athena_gps"), last_gps_info_{}, last_attitude_quaternion_{}, last_heading_deg_{} { - mavsdk_ = std::make_unique(Mavsdk::Configuration{10, 1, false}); - // this must be configured in the launch file this->declare_parameter("usb_path", "/dev/ttyACM0"); - ConnectionResult conn_result = mavsdk_->add_any_connection(this->get_parameter("usb_path").as_string()); + this->declare_parameter("imu_rate", 10.0); + this->declare_parameter("gps_info_rate", 1.0); + this->declare_parameter("attitude_rate", 10.0); + this->declare_parameter("imu_frame_id", "imu_link"); + 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); + + std::string usb_path = this->get_parameter("usb_path").as_string(); + double imu_rate = this->get_parameter("imu_rate").as_double(); + double gps_info_rate = this->get_parameter("gps_info_rate").as_double(); + double attitude_rate = this->get_parameter("attitude_rate").as_double(); + imu_frame_id_ = this->get_parameter("imu_frame_id").as_string(); + 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(); + + mavsdk_ = std::make_unique(Mavsdk::Configuration{ + static_cast(mavsdk_system_id), + static_cast(mavsdk_component_id), + mavsdk_always_send_heartbeats + }); + + ConnectionResult conn_result = mavsdk_->add_any_connection(usb_path); if (conn_result != ConnectionResult::Success) { - RCLCPP_ERROR(this->get_logger(), "Failed to connect to Pixhawk! Please check physical connection."); + RCLCPP_ERROR(this->get_logger(), "Failed to connect to Pixhawk at %s", usb_path.c_str()); return; } - RCLCPP_INFO(this->get_logger(), "Connection to pixhawk successful."); + RCLCPP_INFO(this->get_logger(), "Connected to Pixhawk at %s", usb_path.c_str()); while (mavsdk_->systems().empty()) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Waiting for system connection..."); @@ -38,101 +67,118 @@ class PixhawkNode : public rclcpp::Node auto system = mavsdk_->systems().at(0); telemetry_ = std::make_shared(system); - Telemetry::Result imu_result = telemetry_->set_rate_imu(10.0); - Telemetry::Result gps_info_result = telemetry_->set_rate_gps_info(1.0); - Telemetry::Result attitude_result = telemetry_->set_rate_attitude_quaternion(10.0); + Telemetry::Result imu_result = telemetry_->set_rate_imu(imu_rate); + Telemetry::Result gps_info_result = telemetry_->set_rate_gps_info(gps_info_rate); + Telemetry::Result attitude_result = telemetry_->set_rate_attitude_quaternion(attitude_rate); if (imu_result != Telemetry::Result::Success) { - RCLCPP_ERROR(this->get_logger(), "Failed to set IMU rate"); + RCLCPP_ERROR(this->get_logger(), "Failed to set IMU rate to %.1f Hz", imu_rate); return; } if (gps_info_result != Telemetry::Result::Success) { - RCLCPP_ERROR(this->get_logger(), "Failed to set GPS info rate"); + RCLCPP_ERROR(this->get_logger(), "Failed to set GPS info rate to %.1f Hz", gps_info_rate); return; } if (attitude_result != Telemetry::Result::Success) { - RCLCPP_ERROR(this->get_logger(), "Failed to set attitude quaternion rate"); + RCLCPP_ERROR(this->get_logger(), "Failed to set attitude rate to %.1f Hz", attitude_rate); return; } - // Create publishers - imu_publisher_ = this->create_publisher("imu/data", 10); - gps_publisher_ = this->create_publisher("gps/fix", 10); - heading_publisher_ = this->create_publisher("heading", 10); + RCLCPP_INFO(this->get_logger(), "Telemetry rates configured successfully"); + + imu_publisher_ = this->create_publisher(imu_topic, 10); + gps_publisher_ = this->create_publisher(gps_topic, 10); + heading_publisher_ = this->create_publisher(heading_topic, 10); - // Subscribe to IMU data telemetry_->subscribe_imu([this](const Telemetry::Imu &imu) { imu_callback(imu); }); - // Subscribe to attitude quaternion telemetry_->subscribe_attitude_quaternion([this](const Telemetry::Quaternion &quaternion) { last_attitude_quaternion_ = quaternion; }); - // Subscribe to heading telemetry_->subscribe_heading([this](Telemetry::Heading heading) { last_heading_deg_ = heading.heading_deg; publish_heading(); }); - // Subscribe to raw GPS telemetry_->subscribe_raw_gps([this](const Telemetry::RawGps &raw_gps) { raw_gps_callback(raw_gps); }); - // Subscribe to GPS info for fix type telemetry_->subscribe_gps_info([this](const Telemetry::GpsInfo &gps_info) { last_gps_info_ = gps_info; }); + + RCLCPP_INFO(this->get_logger(), "Pixhawk GPS node initialized successfully"); } private: std::unique_ptr mavsdk_; std::shared_ptr telemetry_; + rclcpp::Publisher::SharedPtr imu_publisher_; rclcpp::Publisher::SharedPtr gps_publisher_; rclcpp::Publisher::SharedPtr heading_publisher_; + Telemetry::GpsInfo last_gps_info_; Telemetry::Quaternion last_attitude_quaternion_; double last_heading_deg_; + std::string imu_frame_id_; + std::string gps_frame_id_; + void publish_heading() { auto msg = std_msgs::msg::Float64(); msg.data = last_heading_deg_; heading_publisher_->publish(msg); } + /** + * convert quaternion from FRD (Forward-Right-Down) to FLU (Forward-Left-Up) frame. + */ + void quaternion_frd_to_flu(const Telemetry::Quaternion &frd, + double &w_out, double &x_out, double &y_out, double &z_out) { + w_out = -frd.x; + x_out = frd.w; + y_out = -frd.z; + z_out = frd.y; + } + void imu_callback(const Telemetry::Imu imu) { auto msg = sensor_msgs::msg::Imu(); msg.header.stamp = this->now(); - msg.header.frame_id = "imu_link"; + msg.header.frame_id = imu_frame_id_; - // Use the full quaternion from attitude - msg.orientation.w = last_attitude_quaternion_.w; - msg.orientation.x = last_attitude_quaternion_.x; - msg.orientation.y = last_attitude_quaternion_.y; - msg.orientation.z = last_attitude_quaternion_.z; + quaternion_frd_to_flu(last_attitude_quaternion_, + msg.orientation.w, + msg.orientation.x, + msg.orientation.y, + msg.orientation.z); + // convert angular velocity from FRD to FLU frame + // FLU: X=forward, Y=left, Z=up + // FRD: X=forward, Y=right, Z=down msg.angular_velocity.x = imu.angular_velocity_frd.forward_rad_s; - msg.angular_velocity.y = imu.angular_velocity_frd.right_rad_s; - msg.angular_velocity.z = imu.angular_velocity_frd.down_rad_s; + msg.angular_velocity.y = -imu.angular_velocity_frd.right_rad_s; + msg.angular_velocity.z = -imu.angular_velocity_frd.down_rad_s; msg.linear_acceleration.x = imu.acceleration_frd.forward_m_s2; - msg.linear_acceleration.y = imu.acceleration_frd.right_m_s2; - msg.linear_acceleration.z = imu.acceleration_frd.down_m_s2; + msg.linear_acceleration.y = -imu.acceleration_frd.right_m_s2; + msg.linear_acceleration.z = -imu.acceleration_frd.down_m_s2; - // Set covariances - for(int i = 0; i < 9; i++) { + bool has_orientation = (last_attitude_quaternion_.w != 0.0 || + last_attitude_quaternion_.x != 0.0 || + last_attitude_quaternion_.y != 0.0 || + last_attitude_quaternion_.z != 0.0); + + for (int i = 0; i < 9; i++) { msg.angular_velocity_covariance[i] = 0.0; msg.linear_acceleration_covariance[i] = 0.0; - // Only set orientation covariance to -1 if we haven't received quaternion data yet - msg.orientation_covariance[i] = (last_attitude_quaternion_.w == 0.0 && - last_attitude_quaternion_.x == 0.0 && - last_attitude_quaternion_.y == 0.0 && - last_attitude_quaternion_.z == 0.0) ? -1 : 0.0; + msg.orientation_covariance[i] = has_orientation ? 0.0 : -1.0; } imu_publisher_->publish(msg); @@ -141,17 +187,17 @@ class PixhawkNode : public rclcpp::Node void raw_gps_callback(const Telemetry::RawGps raw_gps) { auto msg = sensor_msgs::msg::NavSatFix(); + // convert microsections to nanoseconds for ros time auto unix_epoch_time = std::chrono::microseconds(raw_gps.timestamp_us); - auto ros_time = rclcpp::Time(unix_epoch_time.count() * 1000); // Convert to nanoseconds - + auto ros_time = rclcpp::Time(unix_epoch_time.count() * 1000); msg.header.stamp = ros_time; - msg.header.frame_id = "gps_link"; + 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; // Using ellipsoid altitude + msg.altitude = raw_gps.altitude_ellipsoid_m; - // Set the status based on GPS info + // Map GPS fix to ROS fix if (last_gps_info_.num_satellites > 0) { switch (last_gps_info_.fix_type) { case Telemetry::FixType::NoGps: @@ -176,22 +222,22 @@ class PixhawkNode : public rclcpp::Node msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - // Set position covariance using actual uncertainties + // calcilate variances if (!std::isnan(raw_gps.horizontal_uncertainty_m) && !std::isnan(raw_gps.vertical_uncertainty_m)) { - // Convert uncertainties to variances + 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; // xx - msg.position_covariance[4] = h_variance; // yy (assuming isotropic horizontal uncertainty) - msg.position_covariance[8] = v_variance; // zz + msg.position_covariance[0] = h_variance; + msg.position_covariance[4] = h_variance; + msg.position_covariance[8] = v_variance; - // If we have HDOP and VDOP, we can make the horizontal covariance more accurate + // 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); // xx - msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0); // yy - msg.position_covariance[8] = v_variance * raw_gps.vdop; // zz + 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; } msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; From 34a47d8b7a9b745db0e5d4e57816ade0e57c74b7 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sun, 1 Feb 2026 15:59:57 -0500 Subject: [PATCH 3/4] fix maybe idk wy it segfaults so fast --- .../navigation/athena_gps/src/PixhawkNode.cpp | 129 ++++++++++++------ 1 file changed, 86 insertions(+), 43 deletions(-) diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp index 4ad96c2..cc818a6 100644 --- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp +++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp @@ -18,9 +18,14 @@ class PixhawkNode : public rclcpp::Node { public: PixhawkNode() - : Node("athena_gps"), last_gps_info_{}, last_attitude_quaternion_{}, last_heading_deg_{} + : Node("athena_gps"), + last_gps_info_{}, + last_attitude_quaternion_{}, + last_heading_deg_{}, + is_initialized_(false), + connection_added_(false) { - this->declare_parameter("usb_path", "/dev/ttyACM0"); + this->declare_parameter("usb_path", "serial:///dev/ttyACM0:57600"); this->declare_parameter("imu_rate", 10.0); this->declare_parameter("gps_info_rate", 1.0); this->declare_parameter("attitude_rate", 10.0); @@ -33,10 +38,10 @@ class PixhawkNode : public rclcpp::Node this->declare_parameter("mavsdk_component_id", 1); this->declare_parameter("mavsdk_always_send_heartbeats", false); - std::string usb_path = this->get_parameter("usb_path").as_string(); - double imu_rate = this->get_parameter("imu_rate").as_double(); - double gps_info_rate = this->get_parameter("gps_info_rate").as_double(); - double attitude_rate = this->get_parameter("attitude_rate").as_double(); + usb_path_ = this->get_parameter("usb_path").as_string(); + imu_rate_ = this->get_parameter("imu_rate").as_double(); + gps_info_rate_ = this->get_parameter("gps_info_rate").as_double(); + attitude_rate_ = this->get_parameter("attitude_rate").as_double(); imu_frame_id_ = this->get_parameter("imu_frame_id").as_string(); gps_frame_id_ = this->get_parameter("gps_frame_id").as_string(); std::string imu_topic = this->get_parameter("imu_topic").as_string(); @@ -46,52 +51,107 @@ class PixhawkNode : public rclcpp::Node 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(); + // 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); + + // Create MAVSDK instance mavsdk_ = std::make_unique(Mavsdk::Configuration{ static_cast(mavsdk_system_id), static_cast(mavsdk_component_id), mavsdk_always_send_heartbeats }); - ConnectionResult conn_result = mavsdk_->add_any_connection(usb_path); - if (conn_result != ConnectionResult::Success) { - RCLCPP_ERROR(this->get_logger(), "Failed to connect to Pixhawk at %s", usb_path.c_str()); + RCLCPP_INFO(this->get_logger(), "Pixhawk GPS node starting, will attempt to connect to %s", usb_path_.c_str()); + + // Start initialization timer to retry connection until successful + init_timer_ = this->create_wall_timer( + std::chrono::seconds(2), + std::bind(&PixhawkNode::try_initialize, this) + ); + } + +private: + std::unique_ptr mavsdk_; + std::shared_ptr telemetry_; + + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp::Publisher::SharedPtr gps_publisher_; + rclcpp::Publisher::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_; + std::string usb_path_; + double imu_rate_; + double gps_info_rate_; + double attitude_rate_; + 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; } - RCLCPP_INFO(this->get_logger(), "Connected to Pixhawk at %s", usb_path.c_str()); - while (mavsdk_->systems().empty()) { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Waiting for system connection..."); - rclcpp::sleep_for(std::chrono::seconds(1)); + // Try to add connection (only once) + if (!connection_added_) { + ConnectionResult conn_result = mavsdk_->add_any_connection(usb_path_); + if (conn_result != ConnectionResult::Success) { + RCLCPP_WARN(this->get_logger(), + "Failed to add connection at %s: error %d. Will keep trying...", + usb_path_.c_str(), + static_cast(conn_result)); + return; + } + connection_added_ = true; + RCLCPP_INFO(this->get_logger(), "Connection added at %s, waiting for system...", usb_path_.c_str()); + } + + // Wait for a system to be discovered + if (mavsdk_->systems().empty()) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Waiting for Pixhawk system to appear at %s...", usb_path_.c_str()); + return; } auto system = mavsdk_->systems().at(0); telemetry_ = std::make_shared(system); - Telemetry::Result imu_result = telemetry_->set_rate_imu(imu_rate); - Telemetry::Result gps_info_result = telemetry_->set_rate_gps_info(gps_info_rate); - Telemetry::Result attitude_result = telemetry_->set_rate_attitude_quaternion(attitude_rate); + // Configure telemetry rates + Telemetry::Result imu_result = telemetry_->set_rate_imu(imu_rate_); + Telemetry::Result gps_info_result = telemetry_->set_rate_gps_info(gps_info_rate_); + Telemetry::Result attitude_result = telemetry_->set_rate_attitude_quaternion(attitude_rate_); if (imu_result != Telemetry::Result::Success) { - RCLCPP_ERROR(this->get_logger(), "Failed to set IMU rate to %.1f Hz", imu_rate); + RCLCPP_ERROR(this->get_logger(), "Failed to set IMU rate to %.1f Hz, will retry", imu_rate_); return; } if (gps_info_result != Telemetry::Result::Success) { - RCLCPP_ERROR(this->get_logger(), "Failed to set GPS info rate to %.1f Hz", gps_info_rate); + RCLCPP_ERROR(this->get_logger(), "Failed to set GPS info rate to %.1f Hz, will retry", gps_info_rate_); return; } if (attitude_result != Telemetry::Result::Success) { - RCLCPP_ERROR(this->get_logger(), "Failed to set attitude rate to %.1f Hz", attitude_rate); + RCLCPP_ERROR(this->get_logger(), "Failed to set attitude rate to %.1f Hz, will retry", attitude_rate_); return; } RCLCPP_INFO(this->get_logger(), "Telemetry rates configured successfully"); - imu_publisher_ = this->create_publisher(imu_topic, 10); - gps_publisher_ = this->create_publisher(gps_topic, 10); - heading_publisher_ = this->create_publisher(heading_topic, 10); - + // Subscribe to telemetry streams telemetry_->subscribe_imu([this](const Telemetry::Imu &imu) { imu_callback(imu); }); @@ -114,27 +174,10 @@ class PixhawkNode : public rclcpp::Node }); RCLCPP_INFO(this->get_logger(), "Pixhawk GPS node initialized successfully"); - } - -private: - std::unique_ptr mavsdk_; - std::shared_ptr telemetry_; - - rclcpp::Publisher::SharedPtr imu_publisher_; - rclcpp::Publisher::SharedPtr gps_publisher_; - rclcpp::Publisher::SharedPtr heading_publisher_; - - Telemetry::GpsInfo last_gps_info_; - Telemetry::Quaternion last_attitude_quaternion_; - double last_heading_deg_; + is_initialized_ = true; - std::string imu_frame_id_; - std::string gps_frame_id_; - - void publish_heading() { - auto msg = std_msgs::msg::Float64(); - msg.data = last_heading_deg_; - heading_publisher_->publish(msg); + // Stop the initialization timer + init_timer_->cancel(); } /** From 2414c58040d762dd73994046eddef67604d43b2d Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sun, 1 Feb 2026 16:29:04 -0500 Subject: [PATCH 4/4] updated config file --- src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml b/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml index 0513f21..de013f9 100644 --- a/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml +++ b/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml @@ -1,6 +1,6 @@ athena_gps: ros__parameters: - usb_path: "/dev/ttyACM0" + usb_path: "serial:///dev/ttyACM0:57600" imu_rate: 10.0 gps_info_rate: 1.0