Add enable_ekf launch parameter to platform -> localization launch …

…files. Disable the EKF node if enable_ekf is false. (#133)
clearpathrobotics · Dec 18, 2024 · 13d9c03 · 13d9c03
1 parent 2ac65c7
commit 13d9c03
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 27 deletions.
diff --git a/clearpath_common/launch/platform.launch.py b/clearpath_common/launch/platform.launch.py
@@ -66,10 +66,18 @@ def generate_launch_description():
         description='Robot namespace'
     )
 
+    arg_enable_ekf = DeclareLaunchArgument(
+        'enable_ekf',
+        default_value='true',
+        choices=['true', 'false'],
+        description='Enable localization via EKF node'
+    )
+
     # Launch Configurations
     setup_path = LaunchConfiguration('setup_path')
     use_sim_time = LaunchConfiguration('use_sim_time')
     namespace = LaunchConfiguration('namespace')
+    enable_ekf = LaunchConfiguration('enable_ekf')
 
     # Launch files
     launch_file_platform_description = PathJoinSubstitution([
@@ -104,45 +112,49 @@ def generate_launch_description():
             IncludeLaunchDescription(
               PythonLaunchDescriptionSource(launch_file_platform_description),
               launch_arguments=[
-                  ('setup_path', setup_path),
-                  ('use_sim_time', use_sim_time),
-                  ('namespace', namespace),
+                ('setup_path', setup_path),
+                ('use_sim_time', use_sim_time),
+                ('namespace', namespace),
               ]
             ),
 
             # Launch clearpath_control/control.launch.py which is just robot_localization.
             IncludeLaunchDescription(
               PythonLaunchDescriptionSource(launch_file_control),
               launch_arguments=[
-                  ('setup_path', setup_path),
-                  ('use_sim_time', use_sim_time),
+                ('setup_path', setup_path),
+                ('use_sim_time', use_sim_time),
               ]
             ),
 
             # Launch localization (ekf node)
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(launch_file_localization),
                 launch_arguments=[
-                    ('setup_path', setup_path),
-                    ('use_sim_time', use_sim_time)]
+                  ('setup_path', setup_path),
+                  ('use_sim_time', use_sim_time),
+                  ('enable_ekf', enable_ekf)
+                ]
             ),
 
             # Launch clearpath_control/teleop_base.launch.py which is various ways to tele-op
             # the robot but does not include the joystick. Also, has a twist mux.
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(launch_file_teleop_base),
                 launch_arguments=[
-                    ('setup_path', setup_path),
-                    ('use_sim_time', use_sim_time)]
+                  ('setup_path', setup_path),
+                  ('use_sim_time', use_sim_time),
+                ]
             ),
 
             # Launch clearpath_control/teleop_joy.launch.py which is tele-operation using a
             # physical joystick.
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(launch_file_teleop_joy),
                 launch_arguments=[
-                    ('setup_path', setup_path),
-                    ('use_sim_time', use_sim_time)]
+                  ('setup_path', setup_path),
+                  ('use_sim_time', use_sim_time),
+                ]
             ),
         ]
     )
@@ -151,5 +163,6 @@ def generate_launch_description():
     ld.add_action(arg_setup_path)
     ld.add_action(arg_use_sim_time)
     ld.add_action(arg_namespace)
+    ld.add_action(arg_enable_ekf)
     ld.add_action(group_platform_action)
     return ld
diff --git a/clearpath_control/launch/localization.launch.py b/clearpath_control/launch/localization.launch.py
@@ -34,22 +34,29 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
 
     # Launch Configurations
+    enable_ekf = LaunchConfiguration('enable_ekf')
     setup_path = LaunchConfiguration('setup_path')
     use_sim_time = LaunchConfiguration('use_sim_time')
 
     # Launch Arguments
+    arg_enable_ekf = DeclareLaunchArgument(
+        'enable_ekf',
+        default_value='true',
+        choices=['true', 'false'],
+        description='Enable localization via EKF node'
+    )
     arg_setup_path = DeclareLaunchArgument(
         'setup_path',
         default_value='/etc/clearpath/'
     )
-
     arg_use_sim_time = DeclareLaunchArgument(
         'use_sim_time',
         choices=['true', 'false'],
@@ -69,20 +76,22 @@ def generate_launch_description():
 
     # Localization
     node_localization = Node(
-            package='robot_localization',
-            executable='ekf_node',
-            name='ekf_node',
-            output='screen',
-            parameters=[config_localization],
-            remappings=[
-              ('odometry/filtered', 'platform/odom/filtered'),
-              ('/diagnostics', 'diagnostics'),
-              ('/tf', 'tf'),
-              ('/tf_static', 'tf_static'),
-            ]
-        )
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_node',
+        output='screen',
+        parameters=[config_localization],
+        remappings=[
+            ('odometry/filtered', 'platform/odom/filtered'),
+            ('/diagnostics', 'diagnostics'),
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ],
+        condition=IfCondition(enable_ekf),
+    )
 
     ld = LaunchDescription()
+    ld.add_action(arg_enable_ekf)
     ld.add_action(arg_setup_path)
     ld.add_action(arg_use_sim_time)
     ld.add_action(node_localization)

diff --git a/clearpath_generator_common/clearpath_generator_common/launch/generator.py b/clearpath_generator_common/clearpath_generator_common/launch/generator.py
@@ -68,9 +68,10 @@ def __init__(self,
             name='platform',
             package=self.pkg_clearpath_common,
             args=[
-              ('setup_path', self.setup_path),
-              ('use_sim_time', 'false'),
-              ('namespace', self.namespace),
+                ('setup_path', self.setup_path),
+                ('use_sim_time', 'false'),
+                ('namespace', self.namespace),
+                ('enable_ekf', str(self.clearpath_config.platform.enable_ekf).lower()),
             ])
 
         self.manipulators_launch_file = LaunchFile(