From 24d77123c75961ffe6f02bca8daa5e5a2a4064bd Mon Sep 17 00:00:00 2001
From: SimonR99 <simonroy99@hotmail.ca>
Date: Wed, 15 May 2024 18:59:49 -0400
Subject: [PATCH] add navsat conversion and gps

---
 src/rove_bringup/launch/common.launch.py      | 40 +++++++++----------
 .../config/navsat_transform.yaml              |  5 ++-
 src/rove_slam/config/ekf.yaml                 |  5 +--
 3 files changed, 24 insertions(+), 26 deletions(-)

diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py
index b17562c..458e0fa 100644
--- a/src/rove_bringup/launch/common.launch.py
+++ b/src/rove_bringup/launch/common.launch.py
@@ -45,23 +45,24 @@ def generate_launch_description():
     # used tutorial: https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html
 
     robot_localization_node_local = Node(
-       package='robot_localization',
-       executable='ekf_node',
-       name='ekf_filter_node_local',
-       output='screen',
-       parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
-                   {'use_sim_time': True}]
-                   )
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_local',
+        output='screen',
+        parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
+                    {'use_sim_time': True}],
+        remappings=[('odometry/filtered', 'odometry/local')])
+    
 
     robot_localization_node_global = Node(
-       package='robot_localization',
-       executable='ekf_node',
-       name='ekf_filter_node_global',
-       output='screen',
-       parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
-                   {'use_sim_time': True}],
-       remappings=[("odometry/filtered", "odometry/filtered/global")],
-    )
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_global',
+        output='screen',
+        parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
+                    {'use_sim_time': True}],
+        remappings=[('odometry/filtered', 'odometry/global')])
+
 
     navsat_transform = Node(
         package="robot_localization",
@@ -74,10 +75,7 @@ def generate_launch_description():
             # Subscribed Topics
                     ("imu/data", "imu"),
                     ("gps/fix", "gps"),
-                    ("odometry/filtered", "odometry/filtered/global"),
-            # Published Topics
-                    ("gps/filtered", "gps/filtered"),
-                    ("odometry/gps", "odometry/gps"),
+                    ('odometry/filtered', 'odometry/global'),
                 ],
     )
 
@@ -101,8 +99,8 @@ def generate_launch_description():
     return LaunchDescription([
             robot_state_publisher,
             robot_localization_node_local,
-            #robot_localization_node_global,
-            #navsat_transform,
+            robot_localization_node_global,
+            navsat_transform,
             rviz,
             teleop,
             autonomy,
diff --git a/src/rove_description/config/navsat_transform.yaml b/src/rove_description/config/navsat_transform.yaml
index a6faa62..a7669e2 100644
--- a/src/rove_description/config/navsat_transform.yaml
+++ b/src/rove_description/config/navsat_transform.yaml
@@ -4,9 +4,10 @@ navsat_transform:
     frequency: 30.0
     delay: 3.0
     magnetic_declination_radians: 0.0
-    yaw_offset: 0.0
+    yaw_offset: pi
     zero_altitude: true
     broadcast_utm_transform: true
     publish_filtered_gps: true
     use_odometry_yaw: true
-    wait_for_datum: false 
\ No newline at end of file
+    wait_for_datum: false
+    broadcast_utm_transform_as_parent_frame: false
\ No newline at end of file
diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml
index 22568b8..ddfefe3 100644
--- a/src/rove_slam/config/ekf.yaml
+++ b/src/rove_slam/config/ekf.yaml
@@ -28,13 +28,11 @@ ekf_filter_node_global:
     ros__parameters:
         frequency: 30.0
         two_d_mode: false
-        publish_acceleration: true
         publish_tf: true
-
         map_frame: map              
         odom_frame: odom            
         base_link_frame: base_link
-        world_frame: map           
+        world_frame: gps
 
         odom0: /odom
         odom0_config: [true,  true,  false,
@@ -49,6 +47,7 @@ ekf_filter_node_global:
                       false,  false,  false, # vx vy vz
                       false,  false,  true, # vroll vpitch vyaw
                       true, true, false] # ax ay az
+        imu0_queue_size: 20
 
         # description from https://github.com/cra-ros-pkg/robot_localization/blob/ros2/params/ekf.yaml
         # Each sensor reading updates some or all of the filter's state. These options give you greater control over which