Add GPS

diff --git a/docker/Dockerfile b/docker/Dockerfile
index 21ec22399c525a9294836aaf8e0ac8a3df020fea..d20b2094d6e2dd0678c8d1efd5cab9260899faa5 100644 (file)
--- 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 ad406530420d5054258c437a3d161090a068e944..0a55de408705506de3d5120fff89663d5d04f47b 100644 (file)
--- 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"),
+            ],
+        ),
     ])