From 3577230a9a4dc16f8ad2c553c6a39034dbd0a8db Mon Sep 17 00:00:00 2001 From: bossEx Date: Tue, 7 Apr 2026 12:54:16 -0500 Subject: [PATCH 01/18] Gps launch files --- launch/example.launch.py | 21 -------------- launch/gps.launch.py | 60 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 60 insertions(+), 21 deletions(-) delete mode 100644 launch/example.launch.py create mode 100644 launch/gps.launch.py diff --git a/launch/example.launch.py b/launch/example.launch.py deleted file mode 100644 index 19de1d8..0000000 --- a/launch/example.launch.py +++ /dev/null @@ -1,21 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -import os -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - - config = os.path.join(get_package_share_directory('project-name'),'launch', 'example.yaml') - - example_node = Node( - package='project-name', - executable='project-name', - name = 'umrt_example_node', - parameters=[config] - ) - - return LaunchDescription([ - example_node - ]) - diff --git a/launch/gps.launch.py b/launch/gps.launch.py new file mode 100644 index 0000000..c82c587 --- /dev/null +++ b/launch/gps.launch.py @@ -0,0 +1,60 @@ +""" +UMRT Robot Launch File + +TO-DO: +- Work on QoS Profile for all cameras +- Define Launch Arguments for Cameras to keep a persistent way of getting /dev/video +""" + +""" +Imports +""" +from launch import LaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from launch_ros.actions import Node +import os +from ament_index_python.packages import get_package_share_directory + +""" +Generate Launch Description +""" +def generate_launch_description(): + + # Path to this package's launch dir + launch_dir = get_package_share_directory('ublox_dgnss') + launch_path = os.path.join(launch_dir, 'launch','ublox_rover_hpposllh_navsatfix.launch.py') + ''' + gps_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_path), + launch_arguments={ + 'namespace': 'gps_front', + 'DEVICE_FAMILY': 'F9P', + 'DEVICE_SERIAL_STRING': 'GPSF', # Replace with actual serial + 'frame_id': 'gps_top_link', + # 'device': '/dev/serial/by-path/platform-xhci-hcd.2.auto-usb-0:1.4:1.0', + }.items() + ) + ''' + gps_2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_path), + launch_arguments={ + 'namespace': 'gps_back', + 'DEVICE_FAMILY': 'F9P', + 'DEVICE_SERIAL_STRING': 'GPSB', # Replace with actual serial + 'frame_id': 'gps_back_link', + # 'device': '/dev/serial/by-path/platform-xhci-hcd.2.auto-usbv2-0:1.4:1.0', + }.items() + ) + + return LaunchDescription([ + + #gps_1, + gps_2, + + + + ]) + From 94be9c7ea45268bb72c005e8cf407b0ef81efa34 Mon Sep 17 00:00:00 2001 From: bossEx Date: Tue, 7 Apr 2026 15:23:13 -0500 Subject: [PATCH 02/18] Fixed frame id to gps_front_link --- launch/gps.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/gps.launch.py b/launch/gps.launch.py index c82c587..92888a7 100644 --- a/launch/gps.launch.py +++ b/launch/gps.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): 'namespace': 'gps_front', 'DEVICE_FAMILY': 'F9P', 'DEVICE_SERIAL_STRING': 'GPSF', # Replace with actual serial - 'frame_id': 'gps_top_link', + 'frame_id': 'gps_front_link', # 'device': '/dev/serial/by-path/platform-xhci-hcd.2.auto-usb-0:1.4:1.0', }.items() ) From 440bad2d8f96ade2fa8b95be4dce60aa0b2a70f5 Mon Sep 17 00:00:00 2001 From: bossEx Date: Tue, 7 Apr 2026 15:38:30 -0500 Subject: [PATCH 03/18] 2nd launch file --- launch/ublox.launch.py | 88 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 launch/ublox.launch.py diff --git a/launch/ublox.launch.py b/launch/ublox.launch.py new file mode 100644 index 0000000..dca6f25 --- /dev/null +++ b/launch/ublox.launch.py @@ -0,0 +1,88 @@ +""" Launch ublox_dgnss_node publishing high precision Lon/Lat messages""" +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + """Generate launch description for ublox_dgnss components.""" + + namespace = LaunchConfiguration('namespace') + device_family = LaunchConfiguration("device_family") + device_serial_string = LaunchConfiguration('device_serial_string') + frame_id = LaunchConfiguration('frame_id') + + log_level_arg = DeclareLaunchArgument( + "log_level", default_value=TextSubstitution(text="INFO") + ) + namespace_arg = DeclareLaunchArgument( + "namespace", default_value="" + ) + device_family_arg = DeclareLaunchArgument( + "device_family", default_value=TextSubstitution(text="F9P") + ) + device_serial_string_arg = DeclareLaunchArgument( + "device_serial_string", + default_value="", + description="Serial string of the device to use" + ) + frame_id_arg = DeclareLaunchArgument( + "frame_id", + default_value="ubx", + description="The frame_id to use in header of published messages" + ) + + params = [{"DEVICE_FAMILY": device_family}, + {'DEVICE_SERIAL_STRING': device_serial_string}, + {'FRAME_ID': frame_id}, + {'CFG_USBOUTPROT_NMEA': False}, + {'CFG_RATE_MEAS': 10}, + {'CFG_RATE_NAV': 100}, + {'CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB': 1}, + {'CFG_MSGOUT_UBX_NAV_STATUS_USB': 5}, + {'CFG_MSGOUT_UBX_NAV_COV_USB': 1}, + {'CFG_MSGOUT_UBX_RXM_RTCM_USB': 1}] + + container1 = ComposableNodeContainer( + name='ublox_dgnss_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_dgnss_node', + plugin='ublox_dgnss::UbloxDGNSSNode', + name='ublox_dgnss', + namespace=namespace, + parameters=params + ) + ] + ) + + container2 = ComposableNodeContainer( + name='ublox_nav_sat_fix_hp_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_nav_sat_fix_hp_node', + plugin='ublox_nav_sat_fix_hp::UbloxNavSatHpFixNode', + name='ublox_nav_sat_fix_hp', + namespace=namespace + ) + ] + ) + + return launch.LaunchDescription([ + log_level_arg, + device_family_arg, + namespace_arg, + device_serial_string_arg, + frame_id_arg, + container1, + container2, + ]) \ No newline at end of file From aad27732e91886af19f9cf67411e6261f8274653 Mon Sep 17 00:00:00 2001 From: bossEx Date: Tue, 7 Apr 2026 17:11:43 -0500 Subject: [PATCH 04/18] Fixed launch files --- launch/gps.launch.py | 24 +++++++++--------------- 1 file changed, 9 insertions(+), 15 deletions(-) diff --git a/launch/gps.launch.py b/launch/gps.launch.py index 92888a7..ae3e26a 100644 --- a/launch/gps.launch.py +++ b/launch/gps.launch.py @@ -24,37 +24,31 @@ def generate_launch_description(): # Path to this package's launch dir - launch_dir = get_package_share_directory('ublox_dgnss') - launch_path = os.path.join(launch_dir, 'launch','ublox_rover_hpposllh_navsatfix.launch.py') - ''' + launch_dir = get_package_share_directory('umrt-localization-ros') + launch_path = os.path.join(launch_dir, 'launch','ublox.launch.py') + gps_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_path), launch_arguments={ 'namespace': 'gps_front', - 'DEVICE_FAMILY': 'F9P', - 'DEVICE_SERIAL_STRING': 'GPSF', # Replace with actual serial + 'device_family': 'F9P', + 'device_serial_string': 'GPSF', # Replace with actual serial 'frame_id': 'gps_front_link', - # 'device': '/dev/serial/by-path/platform-xhci-hcd.2.auto-usb-0:1.4:1.0', }.items() ) - ''' + gps_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_path), launch_arguments={ 'namespace': 'gps_back', - 'DEVICE_FAMILY': 'F9P', - 'DEVICE_SERIAL_STRING': 'GPSB', # Replace with actual serial + 'device_family': 'F9P', + 'device_serial_string': 'GPSB', # Replace with actual serial 'frame_id': 'gps_back_link', - # 'device': '/dev/serial/by-path/platform-xhci-hcd.2.auto-usbv2-0:1.4:1.0', }.items() ) return LaunchDescription([ - - #gps_1, + gps_1, gps_2, - - - ]) From 21f78dcb11d9a7a807f41684f9766466e43022c0 Mon Sep 17 00:00:00 2001 From: Dev-Patel4 Date: Thu, 9 Apr 2026 16:15:27 -0500 Subject: [PATCH 05/18] Changes to node and cpp file --- CMakeLists.txt | 11 +- .../example_node.hpp | 0 include/umrt-localization-ros/gps_node.hpp | 47 +++++ launch/example.launch.py | 6 +- launch/example.yaml | 13 +- src/heading.cpp | 5 +- src/heading_node.cpp | 166 +++++++++++++++++- 7 files changed, 232 insertions(+), 16 deletions(-) rename include/{project-name => umrt-localization-ros}/example_node.hpp (100%) create mode 100644 include/umrt-localization-ros/gps_node.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 189cd35..f067f71 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle ublox_dgnss + sensor_msgs ) # find dependencies @@ -23,18 +24,18 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_executable(${PROJECT_NAME} +add_executable(gps_node src/heading.cpp src/heading_node.cpp ) -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) -target_include_directories(${PROJECT_NAME} PUBLIC +target_compile_features(gps_node PUBLIC cxx_std_17) +target_include_directories(gps_node PUBLIC $ $ ) ament_target_dependencies( - ${PROJECT_NAME} PUBLIC + gps_node PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} ) @@ -51,7 +52,7 @@ configure_file( # INSTALL install(TARGETS - ${PROJECT_NAME} + gps_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/include/project-name/example_node.hpp b/include/umrt-localization-ros/example_node.hpp similarity index 100% rename from include/project-name/example_node.hpp rename to include/umrt-localization-ros/example_node.hpp diff --git a/include/umrt-localization-ros/gps_node.hpp b/include/umrt-localization-ros/gps_node.hpp new file mode 100644 index 0000000..9aa01e0 --- /dev/null +++ b/include/umrt-localization-ros/gps_node.hpp @@ -0,0 +1,47 @@ +#ifndef GPS_NODE_HPP +#define GPS_NODE_HPP + +#include +#include + +/** + * This is an example node provided to demonstrate file structure and CMake configuration. + */ +class GpsNode: public rclcpp::Node { +public: + // Constructs a GpsNode. Runs when the node starts and is used to initialize subscriptions, publishers, etc. + GpsNode(); + +private: + + // Publisher: Sends messages to a topic. Other nodes can subscribe to it + rclcpp::Publisher::SharedPtr fix_pub_; + + // Subscriber: Listens to a topic (like /gps1/fix). Receives messages when published. + rclcpp::Subscription::SharedPtr gps1_sub_; + rclcpp::Subscription::SharedPtr gps2_sub_; + + // Stores the latest GPS data + sensor_msgs::msg::NavSatFix gps1_msg; + sensor_msgs::msg::NavSatFix gps2_msg; + + // Ensures both gps are producing an output before processing + bool gps1_received; + bool gps2_received; + bool prev_gps_valid; // tracks if previous GPS is valid + + // Store previous fused midpoint + sensor_msgs::msg::NavSatFix prev_gps_msg; // stores previous fused GPS midpoint + + // Callbacks: Runs automatically when a message arrives + void gps1Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + void gps2Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + + // Combines both gps readings + void processGps(); + + bool gpsDataAvailable() const; + +}; + +#endif //GPS_NODE_HPP \ No newline at end of file diff --git a/launch/example.launch.py b/launch/example.launch.py index 19de1d8..a6791fe 100644 --- a/launch/example.launch.py +++ b/launch/example.launch.py @@ -6,11 +6,11 @@ def generate_launch_description(): - config = os.path.join(get_package_share_directory('project-name'),'launch', 'example.yaml') + config = os.path.join(get_package_share_directory('umrt-localization-ros'),'launch', 'example.yaml') example_node = Node( - package='project-name', - executable='project-name', + package='umrt-localization-ros', + executable='umrt-localization-ros', name = 'umrt_example_node', parameters=[config] ) diff --git a/launch/example.yaml b/launch/example.yaml index 116ea5c..a0a60ee 100644 --- a/launch/example.yaml +++ b/launch/example.yaml @@ -1,3 +1,14 @@ umrt_example_node: ros__parameters: - example_param: 0 + gps1_topic: "/gps1/fix" + gps2_topic: "/gps2/fix" + + # Topic where the combined GPS data will be published + combined_topic: "/combined/fix" + + # Frame ID for the NavSatFix messages + frame_id: "map" + + # Optional parameters + example_param: 0 # Your previous example param + publish_rate: 10 # Hz, how often to publish combined GPS data \ No newline at end of file diff --git a/src/heading.cpp b/src/heading.cpp index c1599cb..38c043e 100644 --- a/src/heading.cpp +++ b/src/heading.cpp @@ -3,12 +3,11 @@ // #include - -#include "project-name/example_node.hpp" +#include "umrt-localization-ros/gps_node.hpp" int main(int argc, char* argv[]){ rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/src/heading_node.cpp b/src/heading_node.cpp index 0e2066b..0a7bb56 100644 --- a/src/heading_node.cpp +++ b/src/heading_node.cpp @@ -1,9 +1,167 @@ -#include "project-name/example_node.hpp" +#include "umrt-localization-ros/gps_node.hpp" #include -ExampleNode::ExampleNode() : Node("example") {} +// gpsNode::gpsNode() : Node("example") {} -void ExampleNode::example() { - std::cout << "Hello World" << std::endl; +/** + * Constructor: + * Initializes node name + * Initializes flags for message reception + * Creates publisher for fused GPS + * Creates subscriptions for GPS1 and GPS2 + */ +GpsNode::GpsNode() : Node("gps_node"), gps1_received(false), gps2_received(false){ + + // Publisher: creates a topic "gps/fix" to publish fused GPS messages + // 10 = size of the message queue. If messages come too fast, store up to 10 before dropping + fix_pub_ = this->create_publisher("gps/fix", 10); + + // Subscriber for GPS1: listens to "/gps1/fix" topic + // When a message arrives, gps1Callback() is called + gps1_sub_ = this->create_subscription("/gps1/fix", 10, std::bind(&GpsNode::gps1Callback, this, std::placeholders::_1)); + + // Subscriber for GPS2: listens to "/gps2/fix" topic + // When a message arrives, gps2Callback() is called + gps2_sub_ = this->create_subscription("/gps2/fix", 10,std::bind(&GpsNode::gps2Callback, this, std::placeholders::_1)); + + // Log to console that the node has started + RCLCPP_INFO(this->get_logger(), "Dual GPS node initialized."); +} + +/** + * Callback for GPS1 + * Stores the latest GPS1 message + * Sets gps1_received_ to true + * Calls processGps() to attempt fusion + */ +void GpsNode::gps1Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) +{ + gps1_msg = *msg; + gps1_received = true; + processGps(); +} + +/** + * Callback for GPS2 + * Stores the latest GPS2 message + * Sets gps2_received_ to true + * Calls processGps() to attempt fusion + */ +void GpsNode::gps2Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) +{ + gps2_msg = *msg; + gps2_received = true; + processGps(); +} + +/** + * Checks if both GPS messages have been received at least once + */ +bool GpsNode::gpsDataAvailable() const +{ + return gps1_received && gps2_received; } + +/** + * Fuse GPS messages + * Checks if both GPS messages are available + * Averages latitude, longitude, and altitude + * Publishes fused message + */ +void GpsNode::processGps() +{ + // Wait until both GPS messages are received + if (!gpsDataAvailable()) return; + + sensor_msgs::msg::NavSatFix gps_msg; + + // Set timestamp and frame + gps_msg.header.stamp = this->now(); + gps_msg.header.frame_id = "gps"; + + // Calculate midpoint for vehicle position + double mid_lat = (gps1_msg.latitude + gps2_msg.latitude) / 2.0; + double mid_lon = (gps1_msg.longitude + gps2_msg.longitude) / 2.0; + double mid_alt = (gps1_msg.altitude + gps2_msg.altitude) / 2.0; + + // Rover facing direction (GPS1 back → GPS2 front) + double dx_facing = (gps2_msg.longitude - gps1_msg.longitude) * cos(((gps2_msg.latitude + gps1_msg.latitude)/2.0) * M_PI / 180.0) * 111320.0; + double dy_facing = (gps2_msg.latitude - gps1_msg.latitude) * 111320.0; + + std::string facing_dir = "N/A"; + if (dx_facing != 0 || dy_facing != 0) { + double angle = atan2(dy_facing, dx_facing) * 180.0 / M_PI; + if (angle < 0) angle += 360.0; + + if ((angle >= 337.5 && angle <= 360) || (angle >= 0 && angle < 22.5)) facing_dir = "E"; + else if (angle >= 22.5 && angle < 67.5) facing_dir = "NE"; + else if (angle >= 67.5 && angle < 112.5) facing_dir = "N"; + else if (angle >= 112.5 && angle < 157.5) facing_dir = "NW"; + else if (angle >= 157.5 && angle < 202.5) facing_dir = "W"; + else if (angle >= 202.5 && angle < 247.5) facing_dir = "SW"; + else if (angle >= 247.5 && angle < 292.5) facing_dir = "S"; + else if (angle >= 292.5 && angle < 337.5) facing_dir = "SE"; + } + + std::string movement_dir = "N/A"; + if (prev_gps_valid) { + // Convert latitude/longitude to radians for heading calculation + // GPS coordinates are usually in degrees, but math functions like cos() and atan2() use radians. + double prev_lat_rad = prev_gps_msg.latitude * M_PI / 180.0; + double curr_lat_rad = mid_lat * M_PI / 180.0; + + /** + * Calculate differences in meters. Calculate how far apart the two GPS units are in the east (dx) and north (dy) directions. + * 1. dx is the East-West difference: + * Multiply the difference with cos to to convert longitude difference into actual east-west meters, + * accounting for the fact that the Earth is round and longitude lines get closer together near the poles. + * Multiply by 111320 to convert degrees to meters + * + * 2. dy is the north-south difference: + * Calculate the difference and multiply by 111320 to convert degrees to meters + * + * 3. Finally, calculate the difference in meters between previous and current midpoints + */ + double dx_move = (mid_lon - prev_gps_msg.longitude) * cos((prev_lat_rad + curr_lat_rad)/2.0) * 111320.0; + double dy_move = (mid_lat - prev_gps_msg.latitude) * 111320.0; + + /** + * Returns the angle (in radians) from the x-axis (east) to the y-axis (north) + * For example, if GPS1 is directly in front of GPS2, then the angle is 90 deg, east is 0 deg, west is 180 deg, south is 270 deg. + * If angle is negative (e.g., -90), add 360° → becomes 270. This is South. + */ + // Movement angle in degrees (0 = East, 90 = North) + if (dx_move != 0 || dy_move != 0) { + double angle = atan2(dy_move, dx_move) * 180.0 / M_PI; + if (angle < 0) angle += 360.0; + + if ((angle >= 337.5 && angle <= 360) || (angle >= 0 && angle < 22.5)) movement_dir = "E"; + else if (angle >= 22.5 && angle < 67.5) movement_dir = "NE"; + else if (angle >= 67.5 && angle < 112.5) movement_dir = "N"; + else if (angle >= 112.5 && angle < 157.5) movement_dir = "NW"; + else if (angle >= 157.5 && angle < 202.5) movement_dir = "W"; + else if (angle >= 202.5 && angle < 247.5) movement_dir = "SW"; + else if (angle >= 247.5 && angle < 292.5) movement_dir = "S"; + else if (angle >= 292.5 && angle < 337.5) movement_dir = "SE"; + } + } + + // Update previous Position + prev_gps_msg.latitude = mid_lat; + prev_gps_msg.longitude = mid_lon; + prev_gps_msg.altitude = mid_alt; + prev_gps_valid = true; + + // Prepare NavSatFix message + gps_msg.latitude = mid_lat; + gps_msg.longitude = mid_lon; + gps_msg.altitude = mid_alt; + gps_msg.status = gps1_msg.status; // copy status from GPS1 + + // Publish GPS midpoint + fix_pub_->publish(gps_msg); + + // --- Log both heading and movement --- + RCLCPP_INFO(this->get_logger(), "Lat: %.7f, Lon: %.7f, Alt: %.2f, Facing: %s, Moving: %s", mid_lat, mid_lon, mid_alt, facing_dir.c_str(), movement_dir.c_str()); +} \ No newline at end of file From 7a6da6dacc33b5c69b20c625643fbf7d415f3b16 Mon Sep 17 00:00:00 2001 From: Dev-Patel4 Date: Thu, 9 Apr 2026 17:13:00 -0500 Subject: [PATCH 06/18] Changed topic name and node output --- include/umrt-localization-ros/gps_node.hpp | 4 - src/heading_node.cpp | 111 ++++++--------------- 2 files changed, 33 insertions(+), 82 deletions(-) diff --git a/include/umrt-localization-ros/gps_node.hpp b/include/umrt-localization-ros/gps_node.hpp index 9aa01e0..580930b 100644 --- a/include/umrt-localization-ros/gps_node.hpp +++ b/include/umrt-localization-ros/gps_node.hpp @@ -28,10 +28,6 @@ class GpsNode: public rclcpp::Node { // Ensures both gps are producing an output before processing bool gps1_received; bool gps2_received; - bool prev_gps_valid; // tracks if previous GPS is valid - - // Store previous fused midpoint - sensor_msgs::msg::NavSatFix prev_gps_msg; // stores previous fused GPS midpoint // Callbacks: Runs automatically when a message arrives void gps1Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); diff --git a/src/heading_node.cpp b/src/heading_node.cpp index 0a7bb56..df0a25d 100644 --- a/src/heading_node.cpp +++ b/src/heading_node.cpp @@ -13,17 +13,20 @@ */ GpsNode::GpsNode() : Node("gps_node"), gps1_received(false), gps2_received(false){ + rclcpp::QoS qos(10); + qos.best_effort(); + // Publisher: creates a topic "gps/fix" to publish fused GPS messages // 10 = size of the message queue. If messages come too fast, store up to 10 before dropping - fix_pub_ = this->create_publisher("gps/fix", 10); + fix_pub_ = this->create_publisher("gps/fix", qos); // Subscriber for GPS1: listens to "/gps1/fix" topic // When a message arrives, gps1Callback() is called - gps1_sub_ = this->create_subscription("/gps1/fix", 10, std::bind(&GpsNode::gps1Callback, this, std::placeholders::_1)); + gps1_sub_ = this->create_subscription("/gps_front/fix", qos, std::bind(&GpsNode::gps1Callback, this, std::placeholders::_1)); // Subscriber for GPS2: listens to "/gps2/fix" topic // When a message arrives, gps2Callback() is called - gps2_sub_ = this->create_subscription("/gps2/fix", 10,std::bind(&GpsNode::gps2Callback, this, std::placeholders::_1)); + gps2_sub_ = this->create_subscription("/gps_back/fix", qos, std::bind(&GpsNode::gps2Callback, this, std::placeholders::_1)); // Log to console that the node has started RCLCPP_INFO(this->get_logger(), "Dual GPS node initialized."); @@ -81,87 +84,39 @@ void GpsNode::processGps() gps_msg.header.frame_id = "gps"; // Calculate midpoint for vehicle position - double mid_lat = (gps1_msg.latitude + gps2_msg.latitude) / 2.0; - double mid_lon = (gps1_msg.longitude + gps2_msg.longitude) / 2.0; - double mid_alt = (gps1_msg.altitude + gps2_msg.altitude) / 2.0; - - // Rover facing direction (GPS1 back → GPS2 front) - double dx_facing = (gps2_msg.longitude - gps1_msg.longitude) * cos(((gps2_msg.latitude + gps1_msg.latitude)/2.0) * M_PI / 180.0) * 111320.0; - double dy_facing = (gps2_msg.latitude - gps1_msg.latitude) * 111320.0; - - std::string facing_dir = "N/A"; - if (dx_facing != 0 || dy_facing != 0) { - double angle = atan2(dy_facing, dx_facing) * 180.0 / M_PI; - if (angle < 0) angle += 360.0; - - if ((angle >= 337.5 && angle <= 360) || (angle >= 0 && angle < 22.5)) facing_dir = "E"; - else if (angle >= 22.5 && angle < 67.5) facing_dir = "NE"; - else if (angle >= 67.5 && angle < 112.5) facing_dir = "N"; - else if (angle >= 112.5 && angle < 157.5) facing_dir = "NW"; - else if (angle >= 157.5 && angle < 202.5) facing_dir = "W"; - else if (angle >= 202.5 && angle < 247.5) facing_dir = "SW"; - else if (angle >= 247.5 && angle < 292.5) facing_dir = "S"; - else if (angle >= 292.5 && angle < 337.5) facing_dir = "SE"; - } - - std::string movement_dir = "N/A"; - if (prev_gps_valid) { - // Convert latitude/longitude to radians for heading calculation - // GPS coordinates are usually in degrees, but math functions like cos() and atan2() use radians. - double prev_lat_rad = prev_gps_msg.latitude * M_PI / 180.0; - double curr_lat_rad = mid_lat * M_PI / 180.0; - - /** - * Calculate differences in meters. Calculate how far apart the two GPS units are in the east (dx) and north (dy) directions. - * 1. dx is the East-West difference: - * Multiply the difference with cos to to convert longitude difference into actual east-west meters, - * accounting for the fact that the Earth is round and longitude lines get closer together near the poles. - * Multiply by 111320 to convert degrees to meters - * - * 2. dy is the north-south difference: - * Calculate the difference and multiply by 111320 to convert degrees to meters - * - * 3. Finally, calculate the difference in meters between previous and current midpoints - */ - double dx_move = (mid_lon - prev_gps_msg.longitude) * cos((prev_lat_rad + curr_lat_rad)/2.0) * 111320.0; - double dy_move = (mid_lat - prev_gps_msg.latitude) * 111320.0; - - /** - * Returns the angle (in radians) from the x-axis (east) to the y-axis (north) - * For example, if GPS1 is directly in front of GPS2, then the angle is 90 deg, east is 0 deg, west is 180 deg, south is 270 deg. - * If angle is negative (e.g., -90), add 360° → becomes 270. This is South. - */ - // Movement angle in degrees (0 = East, 90 = North) - if (dx_move != 0 || dy_move != 0) { - double angle = atan2(dy_move, dx_move) * 180.0 / M_PI; - if (angle < 0) angle += 360.0; - - if ((angle >= 337.5 && angle <= 360) || (angle >= 0 && angle < 22.5)) movement_dir = "E"; - else if (angle >= 22.5 && angle < 67.5) movement_dir = "NE"; - else if (angle >= 67.5 && angle < 112.5) movement_dir = "N"; - else if (angle >= 112.5 && angle < 157.5) movement_dir = "NW"; - else if (angle >= 157.5 && angle < 202.5) movement_dir = "W"; - else if (angle >= 202.5 && angle < 247.5) movement_dir = "SW"; - else if (angle >= 247.5 && angle < 292.5) movement_dir = "S"; - else if (angle >= 292.5 && angle < 337.5) movement_dir = "SE"; - } + // double mid_lat = (gps1_msg.latitude + gps2_msg.latitude) / 2.0; + // double mid_lon = (gps1_msg.longitude + gps2_msg.longitude) / 2.0; + // double mid_alt = (gps1_msg.altitude + gps2_msg.altitude) / 2.0; + + // Rover heading direction (GPS1 back → GPS2 front) + double dx_heading = (gps1_msg.longitude - gps2_msg.longitude) * cos(((gps2_msg.latitude + gps1_msg.latitude)/2.0) * M_PI / 180.0) * 111320.0; + double dy_heading = (gps1_msg.latitude - gps2_msg.latitude) * 111320.0; + + double heading_angle = 0; + std::string heading_dir = "N/A"; + if (dx_heading != 0 || dy_heading != 0) { + heading_angle = atan2(dy_heading, dx_heading) * 180.0 / M_PI; + if (heading_angle < 0) heading_angle += 360.0; + + if ((heading_angle >= 337.5 && heading_angle <= 360) || (heading_angle >= 0 && heading_angle < 22.5)) heading_dir = "E"; + else if (heading_angle >= 22.5 && heading_angle < 67.5) heading_dir = "NE"; + else if (heading_angle >= 67.5 && heading_angle < 112.5) heading_dir = "N"; + else if (heading_angle >= 112.5 && heading_angle < 157.5) heading_dir = "NW"; + else if (heading_angle >= 157.5 && heading_angle < 202.5) heading_dir = "W"; + else if (heading_angle >= 202.5 && heading_angle < 247.5) heading_dir = "SW"; + else if (heading_angle >= 247.5 && heading_angle < 292.5) heading_dir = "S"; + else if (heading_angle >= 292.5 && heading_angle < 337.5) heading_dir = "SE"; } - // Update previous Position - prev_gps_msg.latitude = mid_lat; - prev_gps_msg.longitude = mid_lon; - prev_gps_msg.altitude = mid_alt; - prev_gps_valid = true; - // Prepare NavSatFix message - gps_msg.latitude = mid_lat; - gps_msg.longitude = mid_lon; - gps_msg.altitude = mid_alt; + // gps_msg.latitude = mid_lat; + // gps_msg.longitude = mid_lon; + // gps_msg.altitude = mid_alt; gps_msg.status = gps1_msg.status; // copy status from GPS1 // Publish GPS midpoint fix_pub_->publish(gps_msg); - // --- Log both heading and movement --- - RCLCPP_INFO(this->get_logger(), "Lat: %.7f, Lon: %.7f, Alt: %.2f, Facing: %s, Moving: %s", mid_lat, mid_lon, mid_alt, facing_dir.c_str(), movement_dir.c_str()); + // Output + RCLCPP_INFO(this->get_logger(), "Heading Direction: %s, Heading Angle: %.2f", heading_dir.c_str(), heading_angle); } \ No newline at end of file From 9d2d00f9822ec312f37d4fd55cbd0c70bd551c77 Mon Sep 17 00:00:00 2001 From: Dev-Patel4 Date: Thu, 9 Apr 2026 17:33:42 -0500 Subject: [PATCH 07/18] Corrected the angle calculation --- src/heading_node.cpp | 32 ++++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/heading_node.cpp b/src/heading_node.cpp index df0a25d..d6d11a4 100644 --- a/src/heading_node.cpp +++ b/src/heading_node.cpp @@ -88,24 +88,28 @@ void GpsNode::processGps() // double mid_lon = (gps1_msg.longitude + gps2_msg.longitude) / 2.0; // double mid_alt = (gps1_msg.altitude + gps2_msg.altitude) / 2.0; - // Rover heading direction (GPS1 back → GPS2 front) - double dx_heading = (gps1_msg.longitude - gps2_msg.longitude) * cos(((gps2_msg.latitude + gps1_msg.latitude)/2.0) * M_PI / 180.0) * 111320.0; + // Rover heading direction (GPS2 back → GPS1 front) + double dx_heading = (gps1_msg.longitude - gps2_msg.longitude) * cos(((gps1_msg.latitude + gps2_msg.latitude) / 2.0) * M_PI / 180.0) * 111320.0; double dy_heading = (gps1_msg.latitude - gps2_msg.latitude) * 111320.0; - double heading_angle = 0; + double heading_angle = 0.0; std::string heading_dir = "N/A"; - if (dx_heading != 0 || dy_heading != 0) { + if (dx_heading != 0 || dy_heading != 0) + { + // Standard compass heading: 0° = North, 90° = East heading_angle = atan2(dy_heading, dx_heading) * 180.0 / M_PI; - if (heading_angle < 0) heading_angle += 360.0; - - if ((heading_angle >= 337.5 && heading_angle <= 360) || (heading_angle >= 0 && heading_angle < 22.5)) heading_dir = "E"; - else if (heading_angle >= 22.5 && heading_angle < 67.5) heading_dir = "NE"; - else if (heading_angle >= 67.5 && heading_angle < 112.5) heading_dir = "N"; - else if (heading_angle >= 112.5 && heading_angle < 157.5) heading_dir = "NW"; - else if (heading_angle >= 157.5 && heading_angle < 202.5) heading_dir = "W"; - else if (heading_angle >= 202.5 && heading_angle < 247.5) heading_dir = "SW"; - else if (heading_angle >= 247.5 && heading_angle < 292.5) heading_dir = "S"; - else if (heading_angle >= 292.5 && heading_angle < 337.5) heading_dir = "SE"; + + if (heading_angle < 0) + heading_angle += 360.0; + + if ((heading_angle >= 337.5 || heading_angle < 22.5)) heading_dir = "N"; + else if (heading_angle < 67.5) heading_dir = "NE"; + else if (heading_angle < 112.5) heading_dir = "E"; + else if (heading_angle < 157.5) heading_dir = "SE"; + else if (heading_angle < 202.5) heading_dir = "S"; + else if (heading_angle < 247.5) heading_dir = "SW"; + else if (heading_angle < 292.5) heading_dir = "W"; + else if (heading_angle < 337.5) heading_dir = "NW"; } // Prepare NavSatFix message From 1bb129835be4afbd27ff4f5f8be957736fa7c9ce Mon Sep 17 00:00:00 2001 From: bossEx Date: Fri, 24 Apr 2026 21:05:07 -0500 Subject: [PATCH 08/18] Added delay when launching 2nd gps and changed the gps' namespace/frameid to left and right instead of front and back --- launch/gps.launch.py | 25 ++++++++++++++++++++----- launch/ublox.launch.py | 1 + 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/launch/gps.launch.py b/launch/gps.launch.py index ae3e26a..4b91501 100644 --- a/launch/gps.launch.py +++ b/launch/gps.launch.py @@ -17,6 +17,8 @@ from launch_ros.actions import Node import os from ament_index_python.packages import get_package_share_directory +from launch.actions import RegisterEventHandler, TimerAction, LogInfo +from launch.event_handlers import OnProcessStart """ Generate Launch Description @@ -30,25 +32,38 @@ def generate_launch_description(): gps_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_path), launch_arguments={ - 'namespace': 'gps_front', + 'namespace': 'gps_left', 'device_family': 'F9P', 'device_serial_string': 'GPSF', # Replace with actual serial - 'frame_id': 'gps_front_link', + 'frame_id': 'gps_left_link', }.items() ) gps_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_path), launch_arguments={ - 'namespace': 'gps_back', + 'namespace': 'gps_right', 'device_family': 'F9P', 'device_serial_string': 'GPSB', # Replace with actual serial - 'frame_id': 'gps_back_link', + 'frame_id': 'gps_right_link', }.items() ) + + gps_2_delayed = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=gps_1, + on_start=[ + LogInfo(msg='GPS_LEFT started. Waiting 5s for USB to claim before starting GPS_RIGHT...'), + TimerAction( + period=5.0, + actions=[gps_2], + ) + ] + ) + ) return LaunchDescription([ gps_1, - gps_2, + gps_2_delayed, ]) diff --git a/launch/ublox.launch.py b/launch/ublox.launch.py index dca6f25..0d7926f 100644 --- a/launch/ublox.launch.py +++ b/launch/ublox.launch.py @@ -12,6 +12,7 @@ def generate_launch_description(): device_family = LaunchConfiguration("device_family") device_serial_string = LaunchConfiguration('device_serial_string') frame_id = LaunchConfiguration('frame_id') + log_level_arg = DeclareLaunchArgument( "log_level", default_value=TextSubstitution(text="INFO") From 1734e96f985b8078b0323cea0ae6912da8c3a508 Mon Sep 17 00:00:00 2001 From: Dev-Patel4 Date: Sat, 25 Apr 2026 23:03:07 -0500 Subject: [PATCH 09/18] Change heading calculation from front-back to left-right gps position --- include/umrt-localization-ros/gps_node.hpp | 24 +++---- src/heading_node.cpp | 73 ++++++++++------------ 2 files changed, 45 insertions(+), 52 deletions(-) diff --git a/include/umrt-localization-ros/gps_node.hpp b/include/umrt-localization-ros/gps_node.hpp index 580930b..3275ce7 100644 --- a/include/umrt-localization-ros/gps_node.hpp +++ b/include/umrt-localization-ros/gps_node.hpp @@ -15,25 +15,25 @@ class GpsNode: public rclcpp::Node { private: // Publisher: Sends messages to a topic. Other nodes can subscribe to it - rclcpp::Publisher::SharedPtr fix_pub_; + rclcpp::Publisher::SharedPtr fix_pub; - // Subscriber: Listens to a topic (like /gps1/fix). Receives messages when published. - rclcpp::Subscription::SharedPtr gps1_sub_; - rclcpp::Subscription::SharedPtr gps2_sub_; + // Subscriber: Listens to a topic (like /gps_left/fix). Receives messages when published. + rclcpp::Subscription::SharedPtr gpsLeft_sub; + rclcpp::Subscription::SharedPtr gpsRight_sub; // Stores the latest GPS data - sensor_msgs::msg::NavSatFix gps1_msg; - sensor_msgs::msg::NavSatFix gps2_msg; + sensor_msgs::msg::NavSatFix gpsLeft_msg; + sensor_msgs::msg::NavSatFix gpsRight_msg; // Ensures both gps are producing an output before processing - bool gps1_received; - bool gps2_received; + bool gpsLeft_active; + bool gpsRight_active; - // Callbacks: Runs automatically when a message arrives - void gps1Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); - void gps2Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + // Stores the latest message automatically. + void gpsLeftLatestMsg(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + void gpsRightLatestMsg(const sensor_msgs::msg::NavSatFix::SharedPtr msg); - // Combines both gps readings + // Process the message and give output void processGps(); bool gpsDataAvailable() const; diff --git a/src/heading_node.cpp b/src/heading_node.cpp index d6d11a4..cd83fbd 100644 --- a/src/heading_node.cpp +++ b/src/heading_node.cpp @@ -9,52 +9,50 @@ * Initializes node name * Initializes flags for message reception * Creates publisher for fused GPS - * Creates subscriptions for GPS1 and GPS2 + * Creates subscriptions for gps left and right */ -GpsNode::GpsNode() : Node("gps_node"), gps1_received(false), gps2_received(false){ +GpsNode::GpsNode() : Node("gps_node"), gpsLeft_active(false), gpsRight_active(false){ rclcpp::QoS qos(10); qos.best_effort(); // Publisher: creates a topic "gps/fix" to publish fused GPS messages // 10 = size of the message queue. If messages come too fast, store up to 10 before dropping - fix_pub_ = this->create_publisher("gps/fix", qos); + fix_pub = this->create_publisher("gps/fix", qos); - // Subscriber for GPS1: listens to "/gps1/fix" topic - // When a message arrives, gps1Callback() is called - gps1_sub_ = this->create_subscription("/gps_front/fix", qos, std::bind(&GpsNode::gps1Callback, this, std::placeholders::_1)); + // Subscriber for gpsLeft: listens to "/gps_left/fix" topic + // When a message arrives, gpsLeftLatestMsg() is called + gpsLeft_sub = this->create_subscription("/gps_left/fix", qos, std::bind(&GpsNode::gpsLeftLatestMsg, this, std::placeholders::_1)); - // Subscriber for GPS2: listens to "/gps2/fix" topic - // When a message arrives, gps2Callback() is called - gps2_sub_ = this->create_subscription("/gps_back/fix", qos, std::bind(&GpsNode::gps2Callback, this, std::placeholders::_1)); + // Subscriber for gpsRight: listens to "/gps_right/fix" topic + // When a message arrives, gpsRightLatestMsg() is called + gpsRight_sub = this->create_subscription("/gps_right/fix", qos, std::bind(&GpsNode::gpsRightLatestMsg, this, std::placeholders::_1)); // Log to console that the node has started RCLCPP_INFO(this->get_logger(), "Dual GPS node initialized."); } /** - * Callback for GPS1 - * Stores the latest GPS1 message - * Sets gps1_received_ to true + * Stores the latest message for GPS on the left + * Sets gpsLeft_active to true * Calls processGps() to attempt fusion */ -void GpsNode::gps1Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) +void GpsNode::gpsLeftLatestMsg(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - gps1_msg = *msg; - gps1_received = true; + gpsLeft_msg = *msg; + gpsLeft_active = true; processGps(); } /** - * Callback for GPS2 - * Stores the latest GPS2 message - * Sets gps2_received_ to true + * Stores the latest message for GPS on the right + * Sets gpsRight_active to true * Calls processGps() to attempt fusion */ -void GpsNode::gps2Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) +void GpsNode::gpsRightLatestMsg(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - gps2_msg = *msg; - gps2_received = true; + gpsRight_msg = *msg; + gpsRight_active = true; processGps(); } @@ -63,13 +61,13 @@ void GpsNode::gps2Callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) */ bool GpsNode::gpsDataAvailable() const { - return gps1_received && gps2_received; + return gpsLeft_active && gpsRight_active; } /** * Fuse GPS messages * Checks if both GPS messages are available - * Averages latitude, longitude, and altitude + * Get the rover heading in degrees and compass direction * Publishes fused message */ void GpsNode::processGps() @@ -83,21 +81,19 @@ void GpsNode::processGps() gps_msg.header.stamp = this->now(); gps_msg.header.frame_id = "gps"; - // Calculate midpoint for vehicle position - // double mid_lat = (gps1_msg.latitude + gps2_msg.latitude) / 2.0; - // double mid_lon = (gps1_msg.longitude + gps2_msg.longitude) / 2.0; - // double mid_alt = (gps1_msg.altitude + gps2_msg.altitude) / 2.0; - - // Rover heading direction (GPS2 back → GPS1 front) - double dx_heading = (gps1_msg.longitude - gps2_msg.longitude) * cos(((gps1_msg.latitude + gps2_msg.latitude) / 2.0) * M_PI / 180.0) * 111320.0; - double dy_heading = (gps1_msg.latitude - gps2_msg.latitude) * 111320.0; - + // Calculate difference between left and right GPS in meters + // dx = east-west movement, dy = north-south movement + // This gives the sideways direction of the rover + double dx = (gpsRight_msg.longitude - gpsLeft_msg.longitude) * cos(((gpsRight_msg.latitude + gpsLeft_msg.latitude) / 2.0) * M_PI / 180.0) * 111320.0; + double dy = (gpsRight_msg.latitude - gpsLeft_msg.latitude) * 111320.0; double heading_angle = 0.0; std::string heading_dir = "N/A"; - if (dx_heading != 0 || dy_heading != 0) + if (dx != 0 || dy != 0) { - // Standard compass heading: 0° = North, 90° = East - heading_angle = atan2(dy_heading, dx_heading) * 180.0 / M_PI; + // atan2 gives angle where 0° is East + // Subtract 90° to convert it so 0° is North (just like a compass) + heading_angle = atan2(dy, dx) * 180.0 / M_PI; + heading_angle -= 90.0; if (heading_angle < 0) heading_angle += 360.0; @@ -113,13 +109,10 @@ void GpsNode::processGps() } // Prepare NavSatFix message - // gps_msg.latitude = mid_lat; - // gps_msg.longitude = mid_lon; - // gps_msg.altitude = mid_alt; - gps_msg.status = gps1_msg.status; // copy status from GPS1 + gps_msg.status = gpsLeft_msg.status; // copy status from the left GPS // Publish GPS midpoint - fix_pub_->publish(gps_msg); + fix_pub->publish(gps_msg); // Output RCLCPP_INFO(this->get_logger(), "Heading Direction: %s, Heading Angle: %.2f", heading_dir.c_str(), heading_angle); From 16b6cb32c14ee21e6d0d65faf5865d03c4f77e3d Mon Sep 17 00:00:00 2001 From: Dev-Patel4 Date: Fri, 1 May 2026 16:07:03 -0500 Subject: [PATCH 10/18] Change the heading calculation --- launch/gps.launch.py | 17 ++++++----------- src/heading_node.cpp | 11 +++++++---- umrt-localization-ros | 1 + 3 files changed, 14 insertions(+), 15 deletions(-) create mode 160000 umrt-localization-ros diff --git a/launch/gps.launch.py b/launch/gps.launch.py index 4b91501..fbb06df 100644 --- a/launch/gps.launch.py +++ b/launch/gps.launch.py @@ -49,17 +49,12 @@ def generate_launch_description(): }.items() ) - gps_2_delayed = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=gps_1, - on_start=[ - LogInfo(msg='GPS_LEFT started. Waiting 5s for USB to claim before starting GPS_RIGHT...'), - TimerAction( - period=5.0, - actions=[gps_2], - ) - ] - ) + gps_2_delayed = TimerAction( + period=5.0, + actions=[ + LogInfo(msg='GPS_LEFT started. Waiting 5s for USB to claim before starting GPS_RIGHT...'), + gps_2 + ] ) return LaunchDescription([ diff --git a/src/heading_node.cpp b/src/heading_node.cpp index cd83fbd..28f4a53 100644 --- a/src/heading_node.cpp +++ b/src/heading_node.cpp @@ -90,10 +90,13 @@ void GpsNode::processGps() std::string heading_dir = "N/A"; if (dx != 0 || dy != 0) { - // atan2 gives angle where 0° is East - // Subtract 90° to convert it so 0° is North (just like a compass) - heading_angle = atan2(dy, dx) * 180.0 / M_PI; - heading_angle -= 90.0; + + // For a left/right antenna setup, the forward direction is perpendicular to the baseline. + double forward_x = -dy; + double forward_y = dx; + + // atan2(x, y) here gives: 0° = North, 90° = East, 180° = South, 270° = West + heading_angle = atan2(forward_x, forward_y) * 180.0 / M_PI; if (heading_angle < 0) heading_angle += 360.0; diff --git a/umrt-localization-ros b/umrt-localization-ros new file mode 160000 index 0000000..89a0b3e --- /dev/null +++ b/umrt-localization-ros @@ -0,0 +1 @@ +Subproject commit 89a0b3ed0f084c2aa9ad84b90a0fa0e500bd77a4 From 34e9a1a944d78780d6b250a97049bfdad558ecc8 Mon Sep 17 00:00:00 2001 From: senay Date: Thu, 28 May 2026 19:36:23 -0500 Subject: [PATCH 11/18] Documentation for heading project Expanded project overview and detailed hardware setup, heading calculation, and usage instructions. Signed-off-by: senay --- README.md | 79 ++++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 75 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 2da0cd1..daf154a 100644 --- a/README.md +++ b/README.md @@ -17,9 +17,80 @@ trickling down). Each new project must: 9. Replace example files with real code, add source files to CMake targets, document it with Doxygen, and proceed --- -# Project Name +# Heading -This library/executable/project implements XYZ functionality for the University of Manitoba Robotics Team's -rover/robotic arm. +This project implements heading for the University of Manitoba Robotics Team's +rover. -[See the documentation](https://umroboticsteam.github.io/********** project-name **********/) \ No newline at end of file +**Overview** +Dual GPS localization and heading calculation package for the University of Manitoba. +This package uses 2 SparkFun GNSS Combo Breakout to: + 1. Get rover position + 2. Compute rover heading + 3. Publish a combined GPS topic. +This combo breakout pairs the u-blox ZED-F9P multi-band high-precision GNSS module with the NEO-D9S L-band GNSS correction data receiver. In this setup, only ZED-F9P is used for position and heading estimation. Website to find the details about this board: datasheet. + +**Hardware Setup** +Both GPS receivers are connected to Jetson through the USB-A Ports. Each receiver appears as its own serial USB device inside Linux. +Each GPS receiver is connected to its own external GNSS antenna. The antennas receive satellite signals while the receivers process the GNSS data and publish GPS coordinates. +The antennas are mounted left and right on the rover, connected to their respective GPS receivers. This creates a horizontal baseline and the package uses this setup to estimate heading. + +**Header Files and Heading calculation** +The header file, gps_node.hpp defines: + *Publishers: Sends messages to a topic. Other nodes can subscribe to it. + *Subscribers: Listens to a topic. Receives messages when published. + *Some variables and functions: Store and process the GPS data. + +The heading_node.cpp file uses the header file to: + *Initialize publisher, subscriber, variables and functions + *Wait until the GPS actually receives data to start processing it. + *Use the latitude and longitude positions of both GPS to calculate the heading. + +**Heading Estimation** +First, use the latitude and longitude coordinates to calculate the difference between left and right GPS in meters. Since the gps is mounted on the left and right of the rover, its forward is perpendicular to the baseline. Thus, we rotate the vector 90° to make the rover point forward: +double dx = (gpsRight_msg.longitude - gpsLeft_msg.longitude) * cos(((gpsRight_msg.latitude + gpsLeft_msg.latitude) / 2.0) * M_PI / 180.0) * 111320.0; +double dy = (gpsRight_msg.latitude - gpsLeft_msg.latitude) * 111320.0; +double forward_x = -dy; +double forward_y = dx; + +Now, to get the heading angle, just use atan2(x , y) to convert the forward vector into an angle. +heading_angle = atan2(forward_x, forward_y) * 180.0 / M_PI; + +Then, compare this angle to the angle from a compass to get the heading direction. Finally, publish all this data to the topic. + +**Node** +The *gps_node* created in the heading_node.cpp file, is the only node created in this package to take in the coordinates from the 2 GPS receivers and publish an output. + +Topics +There are 3 main topics: + */gps_left/fix : This collects and processes the GPS coordinates from the left GPS. + */gps_right/fix : This collects and processes the GPS coordinates from the right GPS. + *gps/fix : This publishes the combined GPS data. + +Steps to run the package: +| Terminal Steps | Details | Example Command | +|---|---|---| +| | Go inside the directory | `cd Documents/GPS` | +| | Build the Image file (You need to do this every time you make changes to your files) | `./gpsImage.sh` | +| | Run the container with the name ‘gps’ | `./gpsContainer.sh gpsImage` | +| | (Every time) source the file | `source install/setup.bash` | +| | Launch the rosbridge server | `ros2 launch rosbridge_server rosbridge_websocket_launch.xml` | +| | (New terminal) Connect the container to this new terminal using: | `docker exec -it gpsImage bash` | +| | Run the launch file | `ros2 launch umrt-localization-ros gps.launch.py` | +| | (New terminal) Connect the container to this new terminal using: | `docker exec -it gpsImage bash` | +| | Run the node | `ros2 run umrt-localization-ros gps_node` | +| | (New Terminal) Connect one terminal’s container to another terminal | `ros2 topic echo /gps/fix` | +| **In Foxglove** | Run the topic | `ros2 topic echo /gps/fix` | +| | Go to Dashboard | | +| | Click on open connection | | +| | On the pop-up window, select rosbridge | | +| | Click on open | | +| | Click on the dot, and then on the taskbar on the left select topic, and then choose /fix | | + +**Overview on Launch File:** +Overview: The launch file distinguishes the 2 gps via their serial string, which we were able to modify with ublox’s software U-center. + +Once the launch file is ran, our 2 ublox dgnss nodes are created, and they will then output a nav-sat fix, which the heading node (gps_node) will use to calculate a bearing. +finalfinal + +[See the documentation](https://umroboticsteam.github.io/********** project-name **********/) From 3d27b53ee8d5bf2e687a9ff579d49808e50176f6 Mon Sep 17 00:00:00 2001 From: senay Date: Thu, 28 May 2026 19:43:59 -0500 Subject: [PATCH 12/18] Fix formatting and consistency in README.md Corrected formatting issues in the README file, ensuring consistent bullet points and headings. Signed-off-by: senay --- README.md | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index daf154a..8e159c8 100644 --- a/README.md +++ b/README.md @@ -37,14 +37,14 @@ The antennas are mounted left and right on the rover, connected to their respect **Header Files and Heading calculation** The header file, gps_node.hpp defines: - *Publishers: Sends messages to a topic. Other nodes can subscribe to it. - *Subscribers: Listens to a topic. Receives messages when published. - *Some variables and functions: Store and process the GPS data. +* Publishers: Sends messages to a topic. Other nodes can subscribe to it. +* Subscribers: Listens to a topic. Receives messages when published. +* Some variables and functions: Store and process the GPS data. The heading_node.cpp file uses the header file to: - *Initialize publisher, subscriber, variables and functions - *Wait until the GPS actually receives data to start processing it. - *Use the latitude and longitude positions of both GPS to calculate the heading. +* Initialize publisher, subscriber, variables and functions +* Wait until the GPS actually receives data to start processing it. +* Use the latitude and longitude positions of both GPS to calculate the heading. **Heading Estimation** First, use the latitude and longitude coordinates to calculate the difference between left and right GPS in meters. Since the gps is mounted on the left and right of the rover, its forward is perpendicular to the baseline. Thus, we rotate the vector 90° to make the rover point forward: @@ -61,11 +61,11 @@ Then, compare this angle to the angle from a compass to get the heading directio **Node** The *gps_node* created in the heading_node.cpp file, is the only node created in this package to take in the coordinates from the 2 GPS receivers and publish an output. -Topics +**Topics** There are 3 main topics: - */gps_left/fix : This collects and processes the GPS coordinates from the left GPS. - */gps_right/fix : This collects and processes the GPS coordinates from the right GPS. - *gps/fix : This publishes the combined GPS data. +* /gps_left/fix : This collects and processes the GPS coordinates from the left GPS. +* /gps_right/fix : This collects and processes the GPS coordinates from the right GPS. +* gps/fix : This publishes the combined GPS data. Steps to run the package: | Terminal Steps | Details | Example Command | @@ -88,9 +88,10 @@ Steps to run the package: | | Click on the dot, and then on the taskbar on the left select topic, and then choose /fix | | **Overview on Launch File:** -Overview: The launch file distinguishes the 2 gps via their serial string, which we were able to modify with ublox’s software U-center. +The launch file distinguishes the 2 gps via their serial string, which we were able to modify with ublox’s software U-center. + +Once the launch file is ran, our 2 ublox dgnss nodes are created, and they will then output a nav-sat fix, which the heading node (gps_node) will use to calculate a bearing. Below is a diagram to illustrate the process. +finalfinalfinal drawio -Once the launch file is ran, our 2 ublox dgnss nodes are created, and they will then output a nav-sat fix, which the heading node (gps_node) will use to calculate a bearing. -finalfinal [See the documentation](https://umroboticsteam.github.io/********** project-name **********/) From 3b75706daab84782b30c49df75d9c16678d6c006 Mon Sep 17 00:00:00 2001 From: senay Date: Thu, 28 May 2026 19:51:26 -0500 Subject: [PATCH 13/18] Fixed readme Removed template and edited the table Signed-off-by: senay --- README.md | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 8e159c8..deaf7be 100644 --- a/README.md +++ b/README.md @@ -1,22 +1,3 @@ -# UMRT ROS Template -ROS project repository template for the University of Manitoba Robotics Team. - -New projects should be **forked** from this repo (not using this as a template, as that prevents template changes from -trickling down). Each new project must: -1. Fill in missing fields in package.in.xml -2. Fill in project name in CMakeLists.txt and Doxyfile -3. Go into `umrt-build` package settings and give the new repo read permission -4. Go into `umrt-apt-image` package settings and give the new repo read permission -5. Go into `UMRoboticsTeam` organisation secrets and add the new repo to: - - `APT_DEPLOY_KEY` - - `APT_SIGNING_KEY` -6. Copy the rulesets (branch protection rules) from a mature repository like - [umrt-arm-firmware-lib](https://github.com/UMRoboticsTeam/umrt-arm-firmware-lib/) -7. Remove this notice and fill in below README template -8. Write something in mainpage.dox -9. Replace example files with real code, add source files to CMake targets, document it with Doxygen, and proceed - ---- # Heading This project implements heading for the University of Manitoba Robotics Team's @@ -80,8 +61,8 @@ Steps to run the package: | | (New terminal) Connect the container to this new terminal using: | `docker exec -it gpsImage bash` | | | Run the node | `ros2 run umrt-localization-ros gps_node` | | | (New Terminal) Connect one terminal’s container to another terminal | `ros2 topic echo /gps/fix` | -| **In Foxglove** | Run the topic | `ros2 topic echo /gps/fix` | -| | Go to Dashboard | | +| | Run the topic | `ros2 topic echo /gps/fix` | +| **In Foxglove** | Go to Dashboard | | | | Click on open connection | | | | On the pop-up window, select rosbridge | | | | Click on open | | From 3abc28e0057085c3c3fdf1a59518ba08c013078d Mon Sep 17 00:00:00 2001 From: bossEx Date: Thu, 18 Jun 2026 18:53:04 -0500 Subject: [PATCH 14/18] Fixed the delay and version bump. --- launch/gps.launch.py | 14 +++++--------- version | 2 +- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/launch/gps.launch.py b/launch/gps.launch.py index fbb06df..6691ca2 100644 --- a/launch/gps.launch.py +++ b/launch/gps.launch.py @@ -1,9 +1,5 @@ """ -UMRT Robot Launch File - -TO-DO: -- Work on QoS Profile for all cameras -- Define Launch Arguments for Cameras to keep a persistent way of getting /dev/video +UMRT Gps launch file """ """ @@ -50,10 +46,10 @@ def generate_launch_description(): ) gps_2_delayed = TimerAction( - period=5.0, - actions=[ - LogInfo(msg='GPS_LEFT started. Waiting 5s for USB to claim before starting GPS_RIGHT...'), - gps_2 + period = 5.0, + actions = [ + LogInfo(msg='GPS_LEFT started. Waiting 5s for USB to claim before starting GPS_RIGHT...'), + gps_2 ] ) diff --git a/version b/version index 7bcd0e3..6812f81 100644 --- a/version +++ b/version @@ -1 +1 @@ -0.0.2 \ No newline at end of file +0.0.3 \ No newline at end of file From 77fac27ff5c62874aa367d78ff5c5ddcf39d5351 Mon Sep 17 00:00:00 2001 From: bossEx Date: Thu, 18 Jun 2026 18:58:26 -0500 Subject: [PATCH 15/18] Removed empty folder --- umrt-localization-ros | 1 - 1 file changed, 1 deletion(-) delete mode 160000 umrt-localization-ros diff --git a/umrt-localization-ros b/umrt-localization-ros deleted file mode 160000 index 89a0b3e..0000000 --- a/umrt-localization-ros +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 89a0b3ed0f084c2aa9ad84b90a0fa0e500bd77a4 From 21b6dc7f63b30257ea442af82aa9094d1268b6c5 Mon Sep 17 00:00:00 2001 From: bossEx Date: Sat, 4 Jul 2026 06:33:20 -0500 Subject: [PATCH 16/18] Changes for moving base RTK --- CMakeLists.txt | 8 +- include/umrt-localization-ros/gps_node.hpp | 38 +---- launch/gps.launch.py | 62 ++++---- launch/oldgps.launch.py | 59 ++++++++ launch/ublox_base_readonly.launch.py | 83 +++++++++++ launch/ublox_mb+r_base.launch.py | 126 ++++++++++++++++ launch/ublox_mb+r_rover.launch.py | 121 +++++++++++++++ src/heading_node.cpp | 165 ++++++++------------- 8 files changed, 496 insertions(+), 166 deletions(-) create mode 100644 launch/oldgps.launch.py create mode 100644 launch/ublox_base_readonly.launch.py create mode 100644 launch/ublox_mb+r_base.launch.py create mode 100644 launch/ublox_mb+r_rover.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f067f71..d212984 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) # find dependencies +find_package(ublox_ubx_msgs REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -34,9 +35,10 @@ target_include_directories(gps_node PUBLIC $ $ ) -ament_target_dependencies( - gps_node PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} +ament_target_dependencies(gps_node + rclcpp + sensor_msgs + ublox_ubx_msgs ) # Parse version number from the file diff --git a/include/umrt-localization-ros/gps_node.hpp b/include/umrt-localization-ros/gps_node.hpp index 3275ce7..584bfa9 100644 --- a/include/umrt-localization-ros/gps_node.hpp +++ b/include/umrt-localization-ros/gps_node.hpp @@ -2,42 +2,20 @@ #define GPS_NODE_HPP #include -#include +#include -/** - * This is an example node provided to demonstrate file structure and CMake configuration. - */ -class GpsNode: public rclcpp::Node { +class GpsNode : public rclcpp::Node { public: - // Constructs a GpsNode. Runs when the node starts and is used to initialize subscriptions, publishers, etc. GpsNode(); private: + rclcpp::Subscription::SharedPtr relpos_sub; - // Publisher: Sends messages to a topic. Other nodes can subscribe to it - rclcpp::Publisher::SharedPtr fix_pub; + void relposCallback( + const ublox_ubx_msgs::msg::UBXNavRelPosNED::SharedPtr msg + ); - // Subscriber: Listens to a topic (like /gps_left/fix). Receives messages when published. - rclcpp::Subscription::SharedPtr gpsLeft_sub; - rclcpp::Subscription::SharedPtr gpsRight_sub; - - // Stores the latest GPS data - sensor_msgs::msg::NavSatFix gpsLeft_msg; - sensor_msgs::msg::NavSatFix gpsRight_msg; - - // Ensures both gps are producing an output before processing - bool gpsLeft_active; - bool gpsRight_active; - - // Stores the latest message automatically. - void gpsLeftLatestMsg(const sensor_msgs::msg::NavSatFix::SharedPtr msg); - void gpsRightLatestMsg(const sensor_msgs::msg::NavSatFix::SharedPtr msg); - - // Process the message and give output - void processGps(); - - bool gpsDataAvailable() const; - + double normalizeHeading(double heading_deg) const; }; -#endif //GPS_NODE_HPP \ No newline at end of file +#endif // GPS_NODE_HPP \ No newline at end of file diff --git a/launch/gps.launch.py b/launch/gps.launch.py index 6691ca2..7da1912 100644 --- a/launch/gps.launch.py +++ b/launch/gps.launch.py @@ -1,5 +1,5 @@ """ -UMRT Gps launch file +UMRT Moving Base RTK GPS launch file """ """ @@ -7,54 +7,56 @@ """ from launch import LaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction, LogInfo from launch.substitutions import LaunchConfiguration -from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy -from launch_ros.actions import Node import os from ament_index_python.packages import get_package_share_directory -from launch.actions import RegisterEventHandler, TimerAction, LogInfo -from launch.event_handlers import OnProcessStart -""" -Generate Launch Description -""" -def generate_launch_description(): - # Path to this package's launch dir +def generate_launch_description(): + launch_dir = get_package_share_directory('umrt-localization-ros') - launch_path = os.path.join(launch_dir, 'launch','ublox.launch.py') - - gps_1 = IncludeLaunchDescription( - PythonLaunchDescriptionSource(launch_path), + + base_launch_path = os.path.join( + launch_dir, + 'launch', + 'ublox_mb+r_base.launch.py' + ) + rover_launch_path = os.path.join( + launch_dir, + 'launch', + 'ublox_mb+r_rover.launch.py' + ) + + gps_base = IncludeLaunchDescription( + PythonLaunchDescriptionSource(base_launch_path), launch_arguments={ 'namespace': 'gps_left', 'device_family': 'F9P', - 'device_serial_string': 'GPSF', # Replace with actual serial + 'device_serial_string': 'GPSF', # left/base receiver serial 'frame_id': 'gps_left_link', }.items() ) - - gps_2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource(launch_path), + + gps_rover = IncludeLaunchDescription( + PythonLaunchDescriptionSource(rover_launch_path), launch_arguments={ 'namespace': 'gps_right', 'device_family': 'F9P', - 'device_serial_string': 'GPSB', # Replace with actual serial + 'device_serial_string': 'GPSB', # right/rover receiver serial 'frame_id': 'gps_right_link', }.items() ) - gps_2_delayed = TimerAction( - period = 5.0, - actions = [ - LogInfo(msg='GPS_LEFT started. Waiting 5s for USB to claim before starting GPS_RIGHT...'), - gps_2 + gps_rover_delayed = TimerAction( + period=5.0, + actions=[ + LogInfo(msg='Moving base started. Waiting 5s before starting rover...'), + gps_rover ] ) - - return LaunchDescription([ - gps_1, - gps_2_delayed, - ]) + return LaunchDescription([ + gps_base, + gps_rover_delayed, + ]) \ No newline at end of file diff --git a/launch/oldgps.launch.py b/launch/oldgps.launch.py new file mode 100644 index 0000000..9f82e8f --- /dev/null +++ b/launch/oldgps.launch.py @@ -0,0 +1,59 @@ +""" +UMRT Gps launch file +""" + +""" +Imports +""" +from launch import LaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from launch_ros.actions import Node +import os +from ament_index_python.packages import get_package_share_directory +from launch.actions import RegisterEventHandler, TimerAction, LogInfo +from launch.event_handlers import OnProcessStart + +""" +Generate Launch Description +""" +def generate_launch_description(): + + # Path to this package's launch dir + launch_dir = get_package_share_directory('umrt-localization-ros') + launch_path = os.path.join(launch_dir, 'launch','ublox.launch.py') + + gps_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_path), + launch_arguments={ + 'namespace': 'gps_left', + 'device_family': 'F9P', + 'device_serial_string': 'GPSF', # Replace with actual serial + 'frame_id': 'gps_left_link', + }.items() + ) + + gps_2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_path), + launch_arguments={ + 'namespace': 'gps_right', + 'device_family': 'F9P', + 'device_serial_string': 'GPSB', # Replace with actual serial + 'frame_id': 'gps_right_link', + }.items() + ) + + gps_2_delayed = TimerAction( + period = 5.0, + actions = [ + LogInfo(msg='GPS_LEFT started. Waiting 5s for USB to claim before starting GPS_RIGHT...'), + gps_2 + ] + ) + + return LaunchDescription([ + gps_1, + gps_2_delayed, + ]) diff --git a/launch/ublox_base_readonly.launch.py b/launch/ublox_base_readonly.launch.py new file mode 100644 index 0000000..eafdf0e --- /dev/null +++ b/launch/ublox_base_readonly.launch.py @@ -0,0 +1,83 @@ +"""Launch base ublox_dgnss_node without configuring CFG_* settings.""" + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import TextSubstitution, LaunchConfiguration + + +def generate_launch_description(): + device_serial_string = LaunchConfiguration("device_serial_string") + device_family = LaunchConfiguration("device_family") + frame_id = LaunchConfiguration("frame_id") + + log_level_arg = DeclareLaunchArgument( + "log_level", + default_value=TextSubstitution(text="INFO") + ) + + device_family_arg = DeclareLaunchArgument( + "device_family", + default_value=TextSubstitution(text="F9P") + ) + + device_serial_string_arg = DeclareLaunchArgument( + "device_serial_string", + default_value="GPSF", + description="Serial string of the base receiver" + ) + + frame_id_arg = DeclareLaunchArgument( + "frame_id", + default_value="gps_left_link", + description="Frame ID for base receiver messages" + ) + + params_base = [ + {"DEVICE_FAMILY": device_family}, + {"DEVICE_SERIAL_STRING": device_serial_string}, + {"FRAME_ID": frame_id}, + ] + + container_base = ComposableNodeContainer( + name="ublox_dgnss_moving_base", + namespace="", + package="rclcpp_components", + executable="component_container_mt", + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + composable_node_descriptions=[ + ComposableNode( + package="ublox_dgnss_node", + plugin="ublox_dgnss::UbloxDGNSSNode", + name="ublox_dgnss", + namespace="base", + parameters=params_base + ) + ] + ) + + container_navsatfix = ComposableNodeContainer( + name="ublox_nav_sat_fix_hp_base_container", + namespace="", + package="rclcpp_components", + executable="component_container_mt", + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + composable_node_descriptions=[ + ComposableNode( + package="ublox_nav_sat_fix_hp_node", + plugin="ublox_nav_sat_fix_hp::UbloxNavSatHpFixNode", + namespace="base", + name="ublox_nav_sat_fix_hp" + ) + ] + ) + + return launch.LaunchDescription([ + log_level_arg, + device_family_arg, + device_serial_string_arg, + frame_id_arg, + container_base, + container_navsatfix, + ]) \ No newline at end of file diff --git a/launch/ublox_mb+r_base.launch.py b/launch/ublox_mb+r_base.launch.py new file mode 100644 index 0000000..22d3a23 --- /dev/null +++ b/launch/ublox_mb+r_base.launch.py @@ -0,0 +1,126 @@ +""" Launch ublox_dgnss_node publishing high precision Lon/Lat messages""" +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import TextSubstitution +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + """Generate launch description for ublox_dgnss components.""" + + namespace = LaunchConfiguration('namespace') + device_serial_string = LaunchConfiguration('device_serial_string') + device_family = LaunchConfiguration("device_family") + frame_id = LaunchConfiguration('frame_id') + + log_level_arg = DeclareLaunchArgument( + "log_level", default_value=TextSubstitution(text="INFO") + ) + device_family_arg = DeclareLaunchArgument( + "device_family", default_value=TextSubstitution(text="F9P") + ) + device_serial_string_arg = DeclareLaunchArgument( + "device_serial_string", + default_value="", + description="Serial string of the device to use" + ) + frame_id_arg = DeclareLaunchArgument( + "frame_id", + default_value="ubx", + description="The frame_id to use in header of published messages" + ) + + params_base= [ + {"DEVICE_FAMILY": device_family}, + {'DEVICE_SERIAL_STRING': device_serial_string}, + {'FRAME_ID': frame_id}, + + # config measurement interval to 200 ms (ie 5 Hz) and nav update rate to once per measurement + {'CFG_RATE_MEAS': 0xc8}, + {'CFG_RATE_NAV': 0x1}, + + # disable all messages on UART1 + {'CFG_UART1INPROT_NMEA': False}, + {'CFG_UART1INPROT_RTCM3X': False}, + {'CFG_UART1INPROT_UBX': False}, + {'CFG_UART1OUTPROT_NMEA': False}, + {'CFG_UART1OUTPROT_RTCM3X': False}, + {'CFG_UART1OUTPROT_UBX': False}, + + # set UART2 baud rate to 460800 + {'CFG-UART2-BAUDRATE': 0x70800}, + + + # send RTCM messages only (to rover) on UART2 + {'CFG_UART2INPROT_NMEA': False}, + {'CFG_UART2INPROT_RTCM3X': False}, + {'CFG_UART2INPROT_UBX': False}, + {'CFG_UART2OUTPROT_NMEA': False}, + {'CFG_UART2OUTPROT_RTCM3X': True}, + {'CFG_UART2OUTPROT_UBX': False}, + + # RTCM and UBX messages as required on USB + {'CFG_USBINPROT_NMEA': False}, + {'CFG_USBINPROT_RTCM3X': True}, + {'CFG_USBINPROT_UBX': True}, + {'CFG_USBOUTPROT_NMEA': False}, + {'CFG_USBOUTPROT_RTCM3X': False},#set this back to false + {'CFG_USBOUTPROT_UBX': True}, + + # output RTCM messages required for moving base+rover mode on UART2 + {'CFG_MSGOUT_RTCM_3X_TYPE4072_0_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1074_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1084_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1094_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1124_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1230_UART2': 0x1}, + + # messages required for navsatfix calcs by ROS node + {'CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_COV_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_STATUS_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_PVT_USB': 0x1}, + + ] + + container_base = ComposableNodeContainer( + name='ublox_dgnss_moving_base', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_dgnss_node', + plugin='ublox_dgnss::UbloxDGNSSNode', + name='ublox_dgnss', + namespace='base', + parameters=params_base + ) + ] + ) + + container_navsatfix = ComposableNodeContainer( + name='ublox_nav_sat_fix_hp_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_nav_sat_fix_hp_node', + plugin='ublox_nav_sat_fix_hp::UbloxNavSatHpFixNode', + namespace='base', + name='ublox_nav_sat_fix_hp' + ) + ] + ) + + return launch.LaunchDescription([ + log_level_arg, + device_family_arg, + device_serial_string_arg, + container_base, + container_navsatfix, + ]) \ No newline at end of file diff --git a/launch/ublox_mb+r_rover.launch.py b/launch/ublox_mb+r_rover.launch.py new file mode 100644 index 0000000..35b8f18 --- /dev/null +++ b/launch/ublox_mb+r_rover.launch.py @@ -0,0 +1,121 @@ +""" Launch ublox_dgnss_node publishing high precision Lon/Lat messages""" +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import TextSubstitution +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + """Generate launch description for ublox_dgnss components.""" + + namespace = LaunchConfiguration('namespace') + device_family = LaunchConfiguration("device_family") + device_serial_string = LaunchConfiguration('device_serial_string') + frame_id = LaunchConfiguration('frame_id') + + log_level_arg = DeclareLaunchArgument( + "log_level", default_value=TextSubstitution(text="INFO") + ) + device_family_arg = DeclareLaunchArgument( + "device_family", default_value=TextSubstitution(text="F9P") + ) + device_serial_string_arg = DeclareLaunchArgument( + "device_serial_string", + default_value="", + description="Serial string of the device to use" + ) + frame_id_arg = DeclareLaunchArgument( + "frame_id", + default_value="ubx", + description="The frame_id to use in header of published messages" + ) + + params_rover = [ + {"DEVICE_FAMILY": device_family}, + {'DEVICE_SERIAL_STRING': device_serial_string}, + {'FRAME_ID': frame_id}, + + # config measurement interval to 200 ms (ie 5 Hz) and nav update rate to once per measurement + {'CFG_RATE_MEAS': 0xc8}, + {'CFG_RATE_NAV': 0x1}, + + # disable all messages on UART1 + {'CFG_UART1INPROT_NMEA': False}, + {'CFG_UART1INPROT_RTCM3X': True}, + {'CFG_UART1INPROT_UBX': False}, + {'CFG_UART1OUTPROT_NMEA': False}, + {'CFG_UART1OUTPROT_RTCM3X': False}, + {'CFG_UART1OUTPROT_UBX': False}, + + # set UART2 baud rate to 460800 + {'CFG_UART1_BAUDRATE': 0x70800}, + {'CFG_UART2_BAUDRATE': 0x70800}, + + # receive RTCM messages only (from base) on UART2 + {'CFG_UART2INPROT_NMEA': False}, + {'CFG_UART2INPROT_RTCM3X': True}, + {'CFG_UART2INPROT_UBX': False}, + {'CFG_UART2OUTPROT_NMEA': False}, + {'CFG_UART2OUTPROT_RTCM3X': False}, + {'CFG_UART2OUTPROT_UBX': False}, + + # send/receive UBX messages only on USB + {'CFG_USBINPROT_NMEA': False}, + {'CFG_USBINPROT_RTCM3X': False}, + {'CFG_USBINPROT_UBX': True}, + {'CFG_USBOUTPROT_NMEA': False}, + {'CFG_USBOUTPROT_RTCM3X': False}, + {'CFG_USBOUTPROT_UBX': True}, + + # messages required for navsatfix calcs by ROS node + {'CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_COV_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_STATUS_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_PVT_USB': 0x1}, + + # output relative position messages + {'CFG_MSGOUT_UBX_NAV_RELPOSNED_USB': 0x1}, + {'CFG_MSGOUT_UBX_RXM_RTCM_USB': 0x1}, + ] + + container_rover = ComposableNodeContainer( + name='ublox_dgnss_rover', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_dgnss_node', + plugin='ublox_dgnss::UbloxDGNSSNode', + name='ublox_dgnss', + namespace='rover', + parameters=params_rover + ) + ] + ) + + container_navsatfix = ComposableNodeContainer( + name='ublox_nav_sat_fix_hp_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_nav_sat_fix_hp_node', + plugin='ublox_nav_sat_fix_hp::UbloxNavSatHpFixNode', + namespace='rover', + name='ublox_nav_sat_fix_hp' + ) + ] + ) + + return launch.LaunchDescription([ + log_level_arg, + device_family_arg, + device_serial_string_arg, + container_rover, + container_navsatfix, + ]) \ No newline at end of file diff --git a/src/heading_node.cpp b/src/heading_node.cpp index 28f4a53..6a7f338 100644 --- a/src/heading_node.cpp +++ b/src/heading_node.cpp @@ -1,122 +1,81 @@ #include "umrt-localization-ros/gps_node.hpp" -#include - -// gpsNode::gpsNode() : Node("example") {} - -/** - * Constructor: - * Initializes node name - * Initializes flags for message reception - * Creates publisher for fused GPS - * Creates subscriptions for gps left and right - */ -GpsNode::GpsNode() : Node("gps_node"), gpsLeft_active(false), gpsRight_active(false){ +#include +#include +GpsNode::GpsNode() : Node("gps_node") +{ rclcpp::QoS qos(10); - qos.best_effort(); + qos.reliable(); + qos.transient_local(); - // Publisher: creates a topic "gps/fix" to publish fused GPS messages - // 10 = size of the message queue. If messages come too fast, store up to 10 before dropping - fix_pub = this->create_publisher("gps/fix", qos); - - // Subscriber for gpsLeft: listens to "/gps_left/fix" topic - // When a message arrives, gpsLeftLatestMsg() is called - gpsLeft_sub = this->create_subscription("/gps_left/fix", qos, std::bind(&GpsNode::gpsLeftLatestMsg, this, std::placeholders::_1)); + relpos_sub = this->create_subscription( + "/rover/ubx_nav_rel_pos_ned", + qos, + std::bind(&GpsNode::relposCallback, this, std::placeholders::_1) + ); - // Subscriber for gpsRight: listens to "/gps_right/fix" topic - // When a message arrives, gpsRightLatestMsg() is called - gpsRight_sub = this->create_subscription("/gps_right/fix", qos, std::bind(&GpsNode::gpsRightLatestMsg, this, std::placeholders::_1)); - - // Log to console that the node has started - RCLCPP_INFO(this->get_logger(), "Dual GPS node initialized."); + RCLCPP_INFO(this->get_logger(), "Moving-base RTK heading node initialized."); } -/** - * Stores the latest message for GPS on the left - * Sets gpsLeft_active to true - * Calls processGps() to attempt fusion - */ -void GpsNode::gpsLeftLatestMsg(const sensor_msgs::msg::NavSatFix::SharedPtr msg) +double GpsNode::normalizeHeading(double heading_deg) const { - gpsLeft_msg = *msg; - gpsLeft_active = true; - processGps(); -} + while (heading_deg < 0.0) { + heading_deg += 360.0; + } -/** - * Stores the latest message for GPS on the right - * Sets gpsRight_active to true - * Calls processGps() to attempt fusion - */ -void GpsNode::gpsRightLatestMsg(const sensor_msgs::msg::NavSatFix::SharedPtr msg) -{ - gpsRight_msg = *msg; - gpsRight_active = true; - processGps(); -} + while (heading_deg >= 360.0) { + heading_deg -= 360.0; + } -/** - * Checks if both GPS messages have been received at least once - */ -bool GpsNode::gpsDataAvailable() const -{ - return gpsLeft_active && gpsRight_active; + return heading_deg; } -/** - * Fuse GPS messages - * Checks if both GPS messages are available - * Get the rover heading in degrees and compass direction - * Publishes fused message - */ -void GpsNode::processGps() +void GpsNode::relposCallback( + const ublox_ubx_msgs::msg::UBXNavRelPosNED::SharedPtr msg +) { - // Wait until both GPS messages are received - if (!gpsDataAvailable()) return; - - sensor_msgs::msg::NavSatFix gps_msg; - - // Set timestamp and frame - gps_msg.header.stamp = this->now(); - gps_msg.header.frame_id = "gps"; - - // Calculate difference between left and right GPS in meters - // dx = east-west movement, dy = north-south movement - // This gives the sideways direction of the rover - double dx = (gpsRight_msg.longitude - gpsLeft_msg.longitude) * cos(((gpsRight_msg.latitude + gpsLeft_msg.latitude) / 2.0) * M_PI / 180.0) * 111320.0; - double dy = (gpsRight_msg.latitude - gpsLeft_msg.latitude) * 111320.0; - double heading_angle = 0.0; - std::string heading_dir = "N/A"; - if (dx != 0 || dy != 0) - { - - // For a left/right antenna setup, the forward direction is perpendicular to the baseline. - double forward_x = -dy; - double forward_y = dx; - - // atan2(x, y) here gives: 0° = North, 90° = East, 180° = South, 270° = West - heading_angle = atan2(forward_x, forward_y) * 180.0 / M_PI; - - if (heading_angle < 0) - heading_angle += 360.0; - - if ((heading_angle >= 337.5 || heading_angle < 22.5)) heading_dir = "N"; - else if (heading_angle < 67.5) heading_dir = "NE"; - else if (heading_angle < 112.5) heading_dir = "E"; - else if (heading_angle < 157.5) heading_dir = "SE"; - else if (heading_angle < 202.5) heading_dir = "S"; - else if (heading_angle < 247.5) heading_dir = "SW"; - else if (heading_angle < 292.5) heading_dir = "W"; - else if (heading_angle < 337.5) heading_dir = "NW"; + /* + * RELPOSNED gives the vector from the moving base antenna + * to the rover antenna. + * + * Your setup: + * left antenna = base + * right antenna = rover + * + * So this vector points left -> right, which is sideways. + * Robot forward heading = baseline heading - 90 degrees. + */ + + double rel_n = static_cast(msg->rel_pos_n); + double rel_e = static_cast(msg->rel_pos_e); + + if (rel_n == 0.0 && rel_e == 0.0) { + RCLCPP_WARN(this->get_logger(), "RELPOSNED baseline is zero; cannot compute heading."); + return; } - // Prepare NavSatFix message - gps_msg.status = gpsLeft_msg.status; // copy status from the left GPS + double baseline_heading = std::atan2(rel_e, rel_n) * 180.0 / M_PI; + baseline_heading = normalizeHeading(baseline_heading); - // Publish GPS midpoint - fix_pub->publish(gps_msg); + double heading_angle = normalizeHeading(baseline_heading - 90.0); + + std::string heading_dir = "N/A"; - // Output - RCLCPP_INFO(this->get_logger(), "Heading Direction: %s, Heading Angle: %.2f", heading_dir.c_str(), heading_angle); + if ((heading_angle >= 337.5 || heading_angle < 22.5)) heading_dir = "N"; + else if (heading_angle < 67.5) heading_dir = "NE"; + else if (heading_angle < 112.5) heading_dir = "E"; + else if (heading_angle < 157.5) heading_dir = "SE"; + else if (heading_angle < 202.5) heading_dir = "S"; + else if (heading_angle < 247.5) heading_dir = "SW"; + else if (heading_angle < 292.5) heading_dir = "W"; + else if (heading_angle < 337.5) heading_dir = "NW"; + + RCLCPP_INFO( + this->get_logger(), + "Heading Direction: %s, Heading Angle: %.2f, Baseline Heading: %.2f", + heading_dir.c_str(), + heading_angle, + baseline_heading + ); } \ No newline at end of file From 89e4c14fd199dd49b0dd3fe686215fbaff57d9ff Mon Sep 17 00:00:00 2001 From: bossEx Date: Sat, 4 Jul 2026 06:40:14 -0500 Subject: [PATCH 17/18] Removed a file I forgot to delete --- launch/ublox_base_readonly.launch.py | 83 ---------------------------- 1 file changed, 83 deletions(-) delete mode 100644 launch/ublox_base_readonly.launch.py diff --git a/launch/ublox_base_readonly.launch.py b/launch/ublox_base_readonly.launch.py deleted file mode 100644 index eafdf0e..0000000 --- a/launch/ublox_base_readonly.launch.py +++ /dev/null @@ -1,83 +0,0 @@ -"""Launch base ublox_dgnss_node without configuring CFG_* settings.""" - -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch.actions import DeclareLaunchArgument -from launch.substitutions import TextSubstitution, LaunchConfiguration - - -def generate_launch_description(): - device_serial_string = LaunchConfiguration("device_serial_string") - device_family = LaunchConfiguration("device_family") - frame_id = LaunchConfiguration("frame_id") - - log_level_arg = DeclareLaunchArgument( - "log_level", - default_value=TextSubstitution(text="INFO") - ) - - device_family_arg = DeclareLaunchArgument( - "device_family", - default_value=TextSubstitution(text="F9P") - ) - - device_serial_string_arg = DeclareLaunchArgument( - "device_serial_string", - default_value="GPSF", - description="Serial string of the base receiver" - ) - - frame_id_arg = DeclareLaunchArgument( - "frame_id", - default_value="gps_left_link", - description="Frame ID for base receiver messages" - ) - - params_base = [ - {"DEVICE_FAMILY": device_family}, - {"DEVICE_SERIAL_STRING": device_serial_string}, - {"FRAME_ID": frame_id}, - ] - - container_base = ComposableNodeContainer( - name="ublox_dgnss_moving_base", - namespace="", - package="rclcpp_components", - executable="component_container_mt", - arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], - composable_node_descriptions=[ - ComposableNode( - package="ublox_dgnss_node", - plugin="ublox_dgnss::UbloxDGNSSNode", - name="ublox_dgnss", - namespace="base", - parameters=params_base - ) - ] - ) - - container_navsatfix = ComposableNodeContainer( - name="ublox_nav_sat_fix_hp_base_container", - namespace="", - package="rclcpp_components", - executable="component_container_mt", - arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], - composable_node_descriptions=[ - ComposableNode( - package="ublox_nav_sat_fix_hp_node", - plugin="ublox_nav_sat_fix_hp::UbloxNavSatHpFixNode", - namespace="base", - name="ublox_nav_sat_fix_hp" - ) - ] - ) - - return launch.LaunchDescription([ - log_level_arg, - device_family_arg, - device_serial_string_arg, - frame_id_arg, - container_base, - container_navsatfix, - ]) \ No newline at end of file From 8bcdf8758644f32d3cc9d49e08b921642a59dbf6 Mon Sep 17 00:00:00 2001 From: bossEx Date: Wed, 15 Jul 2026 01:13:09 -0500 Subject: [PATCH 18/18] The node file was rewritten with different logging, validity/carr_soln gating, HP position components, and a baseline-length sanity check. Also fixed a line in one of the launch files --- launch/ublox_mb+r_base.launch.py | 2 +- src/heading_node.cpp | 102 +++++++++++++++++++++++++------ 2 files changed, 83 insertions(+), 21 deletions(-) diff --git a/launch/ublox_mb+r_base.launch.py b/launch/ublox_mb+r_base.launch.py index 22d3a23..9f9ca99 100644 --- a/launch/ublox_mb+r_base.launch.py +++ b/launch/ublox_mb+r_base.launch.py @@ -49,7 +49,7 @@ def generate_launch_description(): {'CFG_UART1OUTPROT_UBX': False}, # set UART2 baud rate to 460800 - {'CFG-UART2-BAUDRATE': 0x70800}, + {'CFG_UART2_BAUDRATE': 0x70800}, # send RTCM messages only (to rover) on UART2 diff --git a/src/heading_node.cpp b/src/heading_node.cpp index 6a7f338..5e02ebf 100644 --- a/src/heading_node.cpp +++ b/src/heading_node.cpp @@ -1,5 +1,7 @@ #include "umrt-localization-ros/gps_node.hpp" +#include + #include #include @@ -23,11 +25,9 @@ double GpsNode::normalizeHeading(double heading_deg) const while (heading_deg < 0.0) { heading_deg += 360.0; } - while (heading_deg >= 360.0) { heading_deg -= 360.0; } - return heading_deg; } @@ -39,43 +39,105 @@ void GpsNode::relposCallback( * RELPOSNED gives the vector from the moving base antenna * to the rover antenna. * - * Your setup: + * Antenna mounting on this robot: * left antenna = base * right antenna = rover * - * So this vector points left -> right, which is sideways. + * So the baseline vector points left -> right (sideways). * Robot forward heading = baseline heading - 90 degrees. */ - double rel_n = static_cast(msg->rel_pos_n); - double rel_e = static_cast(msg->rel_pos_e); + // --- Gate on solution validity before trusting anything --- + // + // Field layout per ublox_ubx_msgs: validity flags are flat bools on the + // message, and carr_soln is a CarrSoln sub-message with a .status field + // plus named constants. Confirm with: + // ros2 interface show ublox_ubx_msgs/msg/UBXNavRelPosNED + + if (!msg->gnss_fix_ok || !msg->rel_pos_valid) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "RELPOSNED not valid yet (gnss_fix_ok=%d, rel_pos_valid=%d); skipping.", + static_cast(msg->gnss_fix_ok), + static_cast(msg->rel_pos_valid) + ); + return; + } + + const uint8_t carr_soln = msg->carr_soln.status; + if (carr_soln != + ublox_ubx_msgs::msg::CarrSoln::CARRIER_SOLUTION_PHASE_WITH_FIXED_AMBIGUITIES) + { + // 0 = no carrier solution, 1 = RTK float, 2 = RTK fixed. + // Float headings can be off by several degrees; log but don't trust. + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "RTK not fixed (carr_soln=%d); heading unreliable, skipping.", + static_cast(carr_soln) + ); + return; + } + + // --- Build the baseline vector in meters, including HP components --- + // rel_pos_n/e are in cm; rel_pos_hp_n/hp_e are in 0.1 mm. + // Full HP value in cm = rel_pos_n + rel_pos_hp_n * 1e-2, then * 1e-2 for m. + const double rel_n = + static_cast(msg->rel_pos_n) * 1e-2 + + static_cast(msg->rel_pos_hp_n) * 1e-4; + const double rel_e = + static_cast(msg->rel_pos_e) * 1e-2 + + static_cast(msg->rel_pos_hp_e) * 1e-4; if (rel_n == 0.0 && rel_e == 0.0) { - RCLCPP_WARN(this->get_logger(), "RELPOSNED baseline is zero; cannot compute heading."); + RCLCPP_WARN(this->get_logger(), + "RELPOSNED baseline is zero; cannot compute heading."); return; } - double baseline_heading = std::atan2(rel_e, rel_n) * 180.0 / M_PI; + // --- Sanity check: baseline length vs. known antenna separation --- + // Set this to your measured antenna-to-antenna distance in meters. + // If the RTK length disagrees, the ambiguities fixed wrongly and the + // heading is garbage (see u-blox app note UBX-19009093, section 2.4). + constexpr double kExpectedBaselineM = 1.0; // <-- EDIT to your robot + constexpr double kBaselineToleranceM = 0.05; // +/- 5 cm + + const double baseline_len = std::sqrt(rel_n * rel_n + rel_e * rel_e); + if (std::fabs(baseline_len - kExpectedBaselineM) > kBaselineToleranceM) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Baseline length %.3f m differs from expected %.2f m; " + "possible wrong ambiguity fix, skipping.", + baseline_len, kExpectedBaselineM + ); + return; + } + + // --- Compute heading --- + double baseline_heading = + std::atan2(rel_e, rel_n) * 180.0 / M_PI; baseline_heading = normalizeHeading(baseline_heading); - double heading_angle = normalizeHeading(baseline_heading - 90.0); + const double heading_angle = normalizeHeading(baseline_heading - 90.0); std::string heading_dir = "N/A"; - - if ((heading_angle >= 337.5 || heading_angle < 22.5)) heading_dir = "N"; - else if (heading_angle < 67.5) heading_dir = "NE"; - else if (heading_angle < 112.5) heading_dir = "E"; - else if (heading_angle < 157.5) heading_dir = "SE"; - else if (heading_angle < 202.5) heading_dir = "S"; - else if (heading_angle < 247.5) heading_dir = "SW"; - else if (heading_angle < 292.5) heading_dir = "W"; - else if (heading_angle < 337.5) heading_dir = "NW"; + if (heading_angle >= 337.5 || heading_angle < 22.5) heading_dir = "N"; + else if (heading_angle < 67.5) heading_dir = "NE"; + else if (heading_angle < 112.5) heading_dir = "E"; + else if (heading_angle < 157.5) heading_dir = "SE"; + else if (heading_angle < 202.5) heading_dir = "S"; + else if (heading_angle < 247.5) heading_dir = "SW"; + else if (heading_angle < 292.5) heading_dir = "W"; + else if (heading_angle < 337.5) heading_dir = "NW"; RCLCPP_INFO( this->get_logger(), - "Heading Direction: %s, Heading Angle: %.2f, Baseline Heading: %.2f", + "Heading: %s, angle: %.2f deg, baseline heading: %.2f deg, " + "rel_n: %.3f m, rel_e: %.3f m, len: %.3f m", heading_dir.c_str(), heading_angle, - baseline_heading + baseline_heading, + rel_n, + rel_e, + baseline_len ); } \ No newline at end of file