Updated ekf and navsat

RoboJackets · Nov 1, 2024 · f3d2a83 · f3d2a83
1 parent fb8916a
commit f3d2a83
Show file tree

Hide file tree

Showing 6 changed files with 37 additions and 16 deletions.
diff --git a/.DS_Store b/.DS_Store
diff --git a/urc_bringup/.DS_Store b/urc_bringup/.DS_Store
diff --git a/urc_localization/.DS_Store b/urc_localization/.DS_Store
diff --git a/urc_localization/config/dual_ekf_navsat_params.yaml b/urc_localization/config/dual_ekf_navsat_params.yaml
@@ -12,7 +12,7 @@ ekf_filter_node_odom:
 
     odom_frame: odom
     base_link_frame: base_link
-    world_frame: odom
+    world_frame: map
 
     odom0: /rover_drivetrain_controller/odom
     odom0_config:

diff --git a/urc_localization/config/ekf.yaml b/urc_localization/config/ekf.yaml
@@ -23,22 +23,20 @@ ekf_filter_node:
     map_frame: map
     odom_frame: odom
     base_link_frame: base_link
-    world_frame: map
+    world_frame: odom
 
 
     odom0: /odometry/gps
     odom0_config: [true, true, false,
-                  false, false, false,
-                  false, false, false,
-                  false, false, false,
-                  false, false, false]
+                 false, false, false,
+                 false, false, false,
+                 false, false, false,
+                 false, false, false]
     odom0_covariance: [1.0, 0.0, 0.0,
-                    0.0, 1.0, 0.0,
-                    0.0, 0.0, 2.0]
+                   0.0, 1.0, 0.0,
+                   0.0, 0.0, 2.0]
 
     imu0: /imu/data
-
-
     imu0_config: [false, false, false,
                       true,  true,  true,
                       false, false, false,
@@ -72,6 +70,11 @@ ekf_filter_node:
                                    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]
 
     #Initial only - if we make square, then there will be rapid convergence for initial measurements of the variable in question
+    initial_state:  [0.0,  0.0,  0.0,
+                 0.0,  0.0,  0,
+                 0.0,  0.0,  0.0,
+                 0.0,  0.0,  0.0,
+                 0.0,  0.0,  0.0]
     initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9]
     use_control: false
 
diff --git a/urc_localization/launch/ekf.launch.py b/urc_localization/launch/ekf.launch.py
@@ -2,12 +2,15 @@
 from launch import LaunchDescription
 import launch_ros.actions
 import os
+from launch.actions import (
+    RegisterEventHandler
+)
+from launch.event_handlers import OnProcessStart
 
 
 def generate_launch_description():
-    return LaunchDescription(
-        [
-            launch_ros.actions.Node(
+
+    ekf = launch_ros.actions.Node(
                 package="robot_localization",
                 executable="navsat_transform_node",
                 name="navsat_transform_node",
@@ -21,9 +24,11 @@ def generate_launch_description():
                 ],
                 remappings=[
                     ("gps/fix", "/gps/data"),
+                    ("/imu", "/imu/data"),
                 ],
-            ),
-            launch_ros.actions.Node(
+            )
+
+    navsat = launch_ros.actions.Node(
                 package="robot_localization",
                 executable="ekf_node",
                 name="ekf_filter_node",
@@ -35,6 +40,19 @@ def generate_launch_description():
                         "ekf.yaml",
                     )
                 ],
-            ),
+            )
+
+    return LaunchDescription(
+        [
+          RegisterEventHandler(
+              event_handler = OnProcessStart(
+                    target_action = navsat,
+                    on_start=[ekf]
+
+              )
+          ),
+          #ekf,
+          navsat  
+
         ]
     )