Add speed filter zones to depot and warehouse maps.

nav2_bringup/launch/bringup_launch.py

@@ -36,6 +36,7 @@ def generate_launch_description() -> LaunchDescription:
     slam = LaunchConfiguration('slam')
     map_yaml_file = LaunchConfiguration('map')
     keepout_mask_yaml_file = LaunchConfiguration('keepout_mask')
+    speed_mask_yaml_file = LaunchConfiguration('speed_mask')
     graph_filepath = LaunchConfiguration('graph')
     use_sim_time = LaunchConfiguration('use_sim_time')
     params_file = LaunchConfiguration('params_file')
@@ -45,6 +46,7 @@ def generate_launch_description() -> LaunchDescription:
     log_level = LaunchConfiguration('log_level')
     use_localization = LaunchConfiguration('use_localization')
     use_keepout_zones = LaunchConfiguration('use_keepout_zones')
+    use_speed_zones = LaunchConfiguration('use_speed_zones')
 
     # Map fully qualified names to relative ones so the node's namespace can be prepended.
     remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
@@ -80,6 +82,11 @@ def generate_launch_description() -> LaunchDescription:
         description='Full path to keepout mask yaml file to load'
     )
 
+    declare_speed_mask_yaml_cmd = DeclareLaunchArgument(
+        'speed_mask', default_value='',
+        description='Full path to speed mask yaml file to load'
+    )
+
     declare_graph_file_cmd = DeclareLaunchArgument(
         'graph',
         default_value='', description='Path to the graph file to load'
@@ -95,6 +102,11 @@ def generate_launch_description() -> LaunchDescription:
         description='Whether to enable keepout zones or not'
     )
 
+    declare_use_speed_zones_cmd = DeclareLaunchArgument(
+        'use_speed_zones', default_value='True',
+        description='Whether to enable speed zones or not'
+    )
+
     declare_use_sim_time_cmd = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',
@@ -189,6 +201,22 @@ def generate_launch_description() -> LaunchDescription:
                 }.items(),
             ),
 
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(launch_dir, 'speed_zone_launch.py')
+                ),
+                condition=IfCondition(PythonExpression([use_speed_zones])),
+                launch_arguments={
+                    'namespace': namespace,
+                    'speed_mask': speed_mask_yaml_file,
+                    'use_sim_time': use_sim_time,
+                    'params_file': params_file,
+                    'use_composition': use_composition,
+                    'use_respawn': use_respawn,
+                    'container_name': 'nav2_container',
+                }.items(),
+            ),
+
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
                     os.path.join(launch_dir, 'navigation_launch.py')
@@ -218,6 +246,7 @@ def generate_launch_description() -> LaunchDescription:
     ld.add_action(declare_slam_cmd)
     ld.add_action(declare_map_yaml_cmd)
     ld.add_action(declare_keepout_mask_yaml_cmd)
+    ld.add_action(declare_speed_mask_yaml_cmd)
     ld.add_action(declare_graph_file_cmd)
     ld.add_action(declare_use_sim_time_cmd)
     ld.add_action(declare_params_file_cmd)
@@ -227,6 +256,7 @@ def generate_launch_description() -> LaunchDescription:
     ld.add_action(declare_log_level_cmd)
     ld.add_action(declare_use_localization_cmd)
     ld.add_action(declare_use_keepout_zones_cmd)
+    ld.add_action(declare_use_speed_zones_cmd)
 
     # Add the actions to launch all of the navigation nodes
     ld.add_action(bringup_cmd_group)

nav2_bringup/launch/keepout_zone_launch.py

@@ -40,7 +40,7 @@ def generate_launch_description() -> LaunchDescription:
     use_keepout_zones = LaunchConfiguration('use_keepout_zones')
     log_level = LaunchConfiguration('log_level')
 
-    lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server']
+    lifecycle_nodes = ['keepout_filter_mask_server', 'keepout_costmap_filter_info_server']
 
     # Map fully qualified names to relative ones so the node's namespace can be prepended.
     remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
@@ -116,7 +116,7 @@ def generate_launch_description() -> LaunchDescription:
                 condition=IfCondition(use_keepout_zones),
                 package='nav2_map_server',
                 executable='map_server',
-                name='filter_mask_server',
+                name='keepout_filter_mask_server',
                 output='screen',
                 respawn=use_respawn,
                 respawn_delay=2.0,
@@ -128,7 +128,7 @@ def generate_launch_description() -> LaunchDescription:
                 condition=IfCondition(use_keepout_zones),
                 package='nav2_map_server',
                 executable='costmap_filter_info_server',
-                name='costmap_filter_info_server',
+                name='keepout_costmap_filter_info_server',
                 output='screen',
                 respawn=use_respawn,
                 respawn_delay=2.0,
@@ -162,7 +162,7 @@ def generate_launch_description() -> LaunchDescription:
                     ComposableNode(
                         package='nav2_map_server',
                         plugin='nav2_map_server::MapServer',
-                        name='filter_mask_server',
+                        name='keepout_filter_mask_server',
                         parameters=[
                             configured_params,
                             {'yaml_filename': keepout_mask_yaml_file}
@@ -172,7 +172,7 @@ def generate_launch_description() -> LaunchDescription:
                     ComposableNode(
                         package='nav2_map_server',
                         plugin='nav2_map_server::CostmapFilterInfoServer',
-                        name='costmap_filter_info_server',
+                        name='keepout_costmap_filter_info_server',
                         parameters=[configured_params],
                         remappings=remappings,
                     ),

nav2_bringup/launch/speed_zone_launch.py

@@ -0,0 +1,219 @@
+# Copyright (c) 2025 Leander Stephen Desouza
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes, Node, SetParameter
+from launch_ros.descriptions import ComposableNode, ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description() -> LaunchDescription:
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+
+    namespace = LaunchConfiguration('namespace')
+    speed_mask_yaml_file = LaunchConfiguration('speed_mask')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    autostart = LaunchConfiguration('autostart')
+    params_file = LaunchConfiguration('params_file')
+    use_composition = LaunchConfiguration('use_composition')
+    container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
+    use_respawn = LaunchConfiguration('use_respawn')
+    use_speed_zones = LaunchConfiguration('use_speed_zones')
+    log_level = LaunchConfiguration('log_level')
+
+    lifecycle_nodes = ['speed_filter_mask_server', 'speed_costmap_filter_info_server']
+
+    # Map fully qualified names to relative ones so the node's namespace can be prepended.
+    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites={},
+            convert_types=True,
+        ),
+        allow_substs=True,
+    )
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1'
+    )
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace', default_value='', description='Top-level namespace'
+    )
+
+    declare_speed_mask_yaml_cmd = DeclareLaunchArgument(
+        'speed_mask',
+        default_value='',
+        description='Full path to speed mask yaml file to load',
+    )
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true',
+    )
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes',
+    )
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition',
+        default_value='False',
+        description='Use composed bringup if True',
+    )
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name',
+        default_value='nav2_container',
+        description='the name of container that nodes will load in if use composition',
+    )
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn',
+        default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.',
+    )
+
+    declare_use_speed_zones_cmd = DeclareLaunchArgument(
+        'use_speed_zones', default_value='True',
+        description='Whether to enable speed zones or not'
+    )
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level', default_value='info', description='log level'
+    )
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            SetParameter('use_sim_time', use_sim_time),
+            Node(
+                condition=IfCondition(use_speed_zones),
+                package='nav2_map_server',
+                executable='map_server',
+                name='speed_filter_mask_server',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params, {'yaml_filename': speed_mask_yaml_file}],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings,
+            ),
+            Node(
+                condition=IfCondition(use_speed_zones),
+                package='nav2_map_server',
+                executable='costmap_filter_info_server',
+                name='speed_costmap_filter_info_server',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings,
+            ),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_speed_zone',
+                output='screen',
+                arguments=['--ros-args', '--log-level', log_level],
+                parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}],
+            ),
+        ],
+    )
+    # LoadComposableNode for map server twice depending if we should use the
+    # value of map from a CLI or launch default or user defined value in the
+    # yaml configuration file. They are separated since the conditions
+    # currently only work on the LoadComposableNodes commands and not on the
+    # ComposableNode node function itself
+    load_composable_nodes = GroupAction(
+        condition=IfCondition(use_composition),
+        actions=[
+            SetParameter('use_sim_time', use_sim_time),
+            LoadComposableNodes(
+                target_container=container_name_full,
+                condition=IfCondition(use_speed_zones),
+                composable_node_descriptions=[
+                    ComposableNode(
+                        package='nav2_map_server',
+                        plugin='nav2_map_server::MapServer',
+                        name='speed_filter_mask_server',
+                        parameters=[
+                            configured_params,
+                            {'yaml_filename': speed_mask_yaml_file}
+                        ],
+                        remappings=remappings,
+                    ),
+                    ComposableNode(
+                        package='nav2_map_server',
+                        plugin='nav2_map_server::CostmapFilterInfoServer',
+                        name='speed_costmap_filter_info_server',
+                        parameters=[configured_params],
+                        remappings=remappings,
+                    ),
+                ],
+            ),
+
+            LoadComposableNodes(
+                target_container=container_name_full,
+                composable_node_descriptions=[
+                    ComposableNode(
+                        package='nav2_lifecycle_manager',
+                        plugin='nav2_lifecycle_manager::LifecycleManager',
+                        name='lifecycle_manager_speed_zone',
+                        parameters=[
+                            {'autostart': autostart, 'node_names': lifecycle_nodes}
+                        ],
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_speed_mask_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_use_speed_zones_cmd)
+    ld.add_action(declare_log_level_cmd)
+
+    # Add the actions to launch all of the map modifier nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld