From d1acfcc0960944bcb628f1cb08c22ae113cfaaa8 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Tue, 24 Mar 2026 19:22:13 +0100
Subject: [PATCH] Add GPS

---
 docker/Dockerfile      |  2 +-
 launch/a4wd3_launch.py | 29 +++++++++++++++++++++++++++++
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/docker/Dockerfile b/docker/Dockerfile
index 21ec223..d20b209 100644
--- a/docker/Dockerfile
+++ b/docker/Dockerfile
@@ -3,4 +3,4 @@ FROM ros:jazzy-ros-base
 RUN apt-get update && \
         apt-get install -y ros-jazzy-rplidar-ros less libi2c-dev ros-jazzy-robot-localization \
 	ros-jazzy-slam-toolbox ros-jazzy-navigation2 ros-jazzy-nav2-bringup \
-	ros-jazzy-demo-nodes-cpp ros-jazzy-teleop-twist-keyboard ros-jazzy-gpsd-client libgdal-dev ros-jazzy-teleop-twist-joy ros-jazzy-twist-mux
+	ros-jazzy-demo-nodes-cpp ros-jazzy-teleop-twist-keyboard ros-jazzy-gpsd-client libgdal-dev ros-jazzy-teleop-twist-joy ros-jazzy-twist-mux ros-jazzy-gpsd-client
diff --git a/launch/a4wd3_launch.py b/launch/a4wd3_launch.py
index ad40653..0a55de4 100644
--- a/launch/a4wd3_launch.py
+++ b/launch/a4wd3_launch.py
@@ -3,11 +3,17 @@
 
 import os
 from ament_index_python.packages import get_package_share_directory
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
 from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
 
 def generate_launch_description():
+    with_gps = LaunchConfiguration('with_gps', default=os.path.exists("/dev/serial/by-id/usb-u-blox_AG_-_www.u-blox.com_u-blox_GNSS_receiver-if00"))
     return LaunchDescription([
+        DeclareLaunchArgument('with_gps', default_value=with_gps, description='Enable GPS'),
         Node(
             package='a4wd3',
             executable='hw_node',
@@ -35,6 +41,14 @@ def generate_launch_description():
 	    arguments = ['--x', '-0.02', '--y', '-0.013', '--roll', '3.1416', '--yaw', '0.0', '--frame-id', 'base_link', '--child-frame-id', 'imu'],
 	    output="screen"
         ),
+        Node(
+            condition=IfCondition(with_gps),
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='tf_base_gps',
+	    arguments = ['--x', '-0.04', '--y', '-0.035', '--frame-id', 'base_link', '--child-frame-id', 'gps'],
+	    output="screen"
+        ),
         Node(
             package='robot_localization',
             executable='ekf_node',
@@ -42,4 +56,19 @@ def generate_launch_description():
             output='screen',
 	    parameters = [os.path.join(get_package_share_directory('a4wd3'), 'params', 'robot_localization.yaml')],
         ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('gpsd_client'), 'launch'),'/gpsd_client-launch.py']),
+            condition=IfCondition(with_gps)
+        ),
+        Node(
+            condition=IfCondition(with_gps),
+            package='robot_localization',
+            executable='navsat_transform_node',
+            name='navsat_transform',
+	    parameters=[{"delay": 3.0, "magnetic_declination_radians": 0.047, "zero_altitude": True, "broadcast_cartesian_transform": True, "use_odometry_yaw": False}],
+	    output="screen",
+            remappings=[
+                    ("gps/fix", "fix"),
+            ],
+        ),
     ])
-- 
2.39.5