diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 4f60682632c..05f54d67d8e 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -35,6 +35,7 @@ def generate_launch_description() -> LaunchDescription: namespace = LaunchConfiguration('namespace') slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') + keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -43,6 +44,7 @@ def generate_launch_description() -> LaunchDescription: use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') use_localization = LaunchConfiguration('use_localization') + use_keepout_zones = LaunchConfiguration('use_keepout_zones') # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -73,6 +75,11 @@ def generate_launch_description() -> LaunchDescription: 'map', default_value='', description='Full path to map yaml file to load' ) + declare_keepout_mask_yaml_cmd = DeclareLaunchArgument( + 'keepout_mask', default_value='', + description='Full path to keepout mask yaml file to load' + ) + declare_graph_file_cmd = DeclareLaunchArgument( 'graph', default_value='', description='Path to the graph file to load' @@ -83,6 +90,11 @@ def generate_launch_description() -> LaunchDescription: description='Whether to enable localization or not' ) + declare_use_keepout_zones_cmd = DeclareLaunchArgument( + 'use_keepout_zones', default_value='True', + description='Whether to enable keepout zones or not' + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -160,6 +172,23 @@ def generate_launch_description() -> LaunchDescription: 'container_name': 'nav2_container', }.items(), ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'keepout_zone_launch.py') + ), + condition=IfCondition(PythonExpression([use_keepout_zones])), + launch_arguments={ + 'namespace': namespace, + 'keepout_mask': keepout_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') @@ -188,6 +217,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_namespace_cmd) 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_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) @@ -196,6 +226,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) ld.add_action(declare_use_localization_cmd) + ld.add_action(declare_use_keepout_zones_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/keepout_zone_launch.py b/nav2_bringup/launch/keepout_zone_launch.py new file mode 100644 index 00000000000..58d5a5987bf --- /dev/null +++ b/nav2_bringup/launch/keepout_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') + keepout_mask_yaml_file = LaunchConfiguration('keepout_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_keepout_zones = LaunchConfiguration('use_keepout_zones') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['filter_mask_server', '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_keepout_mask_yaml_cmd = DeclareLaunchArgument( + 'keepout_mask', + default_value='', + description='Full path to keepout 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_keepout_zones_cmd = DeclareLaunchArgument( + 'use_keepout_zones', default_value='True', + description='Whether to enable keepout 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_keepout_zones), + package='nav2_map_server', + executable='map_server', + name='filter_mask_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'yaml_filename': keepout_mask_yaml_file}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + condition=IfCondition(use_keepout_zones), + package='nav2_map_server', + executable='costmap_filter_info_server', + name='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_keepout_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_keepout_zones), + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='filter_mask_server', + parameters=[ + configured_params, + {'yaml_filename': keepout_mask_yaml_file} + ], + remappings=remappings, + ), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::CostmapFilterInfoServer', + name='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_keepout_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_keepout_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_keepout_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 diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index 0dc98654ae8..ba78b556ddc 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -27,6 +27,19 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node +# Define local map types +MAP_POSES_DICT = { + 'depot': { + 'x': -8.00, 'y': 0.00, 'z': 0.01, + 'R': 0.00, 'P': 0.00, 'Y': 0.00 + }, + 'warehouse': { + 'x': 2.12, 'y': -21.3, 'z': 0.01, + 'R': 0.00, 'P': 0.00, 'Y': 1.57 + } +} +MAP_TYPE = 'depot' # Change this to 'warehouse' for warehouse map + def generate_launch_description() -> LaunchDescription: # Get the launch directory @@ -41,12 +54,14 @@ def generate_launch_description() -> LaunchDescription: slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') + keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') + use_keepout_zones = LaunchConfiguration('use_keepout_zones') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -56,12 +71,12 @@ def generate_launch_description() -> LaunchDescription: headless = LaunchConfiguration('headless') world = LaunchConfiguration('world') pose = { - 'x': LaunchConfiguration('x_pose', default='-8.00'), # Warehouse: 2.12 - 'y': LaunchConfiguration('y_pose', default='0.00'), # Warehouse: -21.3 - 'z': LaunchConfiguration('z_pose', default='0.01'), - 'R': LaunchConfiguration('roll', default='0.00'), - 'P': LaunchConfiguration('pitch', default='0.00'), - 'Y': LaunchConfiguration('yaw', default='0.00'), # Warehouse: 1.57 + 'x': LaunchConfiguration('x_pose', default=MAP_POSES_DICT[MAP_TYPE]['x']), + 'y': LaunchConfiguration('y_pose', default=MAP_POSES_DICT[MAP_TYPE]['y']), + 'z': LaunchConfiguration('z_pose', default=MAP_POSES_DICT[MAP_TYPE]['z']), + 'R': LaunchConfiguration('roll', default=MAP_POSES_DICT[MAP_TYPE]['R']), + 'P': LaunchConfiguration('pitch', default=MAP_POSES_DICT[MAP_TYPE]['P']), + 'Y': LaunchConfiguration('yaw', default=MAP_POSES_DICT[MAP_TYPE]['Y']), } robot_name = LaunchConfiguration('robot_name') robot_sdf = LaunchConfiguration('robot_sdf') @@ -79,10 +94,16 @@ def generate_launch_description() -> LaunchDescription: declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value=os.path.join(bringup_dir, 'maps', 'depot.yaml'), # Try warehouse.yaml! + default_value=os.path.join(bringup_dir, 'maps', f'{MAP_TYPE}.yaml'), description='Full path to map file to load', ) + declare_keepout_mask_yaml_cmd = DeclareLaunchArgument( + 'keepout_mask', + default_value=os.path.join(bringup_dir, 'maps', f'{MAP_TYPE}_keepout.yaml'), + description='Full path to keepout mask file to load', + ) + declare_graph_file_cmd = DeclareLaunchArgument( 'graph', default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'), @@ -118,6 +139,11 @@ def generate_launch_description() -> LaunchDescription: description='Whether to respawn if a node crashes. Applied when composition is disabled.', ) + declare_use_keepout_zones_cmd = DeclareLaunchArgument( + 'use_keepout_zones', default_value='True', + description='Whether to enable keepout zones or not' + ) + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -146,7 +172,7 @@ def generate_launch_description() -> LaunchDescription: declare_world_cmd = DeclareLaunchArgument( 'world', - default_value=os.path.join(sim_dir, 'worlds', 'depot.sdf'), # Try warehouse.sdf! + default_value=os.path.join(sim_dir, 'worlds', f'{MAP_TYPE}.sdf'), description='Full path to world model file to load', ) @@ -189,12 +215,14 @@ def generate_launch_description() -> LaunchDescription: 'namespace': namespace, 'slam': slam, 'map': map_yaml_file, + 'keepout_mask': keepout_mask_yaml_file, 'graph': graph_filepath, 'use_sim_time': use_sim_time, 'params_file': params_file, 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, + 'use_keepout_zones': use_keepout_zones, }.items(), ) @@ -253,6 +281,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_namespace_cmd) 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_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) @@ -268,6 +297,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_robot_name_cmd) ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_use_keepout_zones_cmd) ld.add_action(set_env_vars_resources) ld.add_action(world_sdf_xacro) diff --git a/nav2_bringup/maps/depot_keepout.pgm b/nav2_bringup/maps/depot_keepout.pgm new file mode 100644 index 00000000000..5d1aeea4dbb Binary files /dev/null and b/nav2_bringup/maps/depot_keepout.pgm differ diff --git a/nav2_bringup/maps/depot_keepout.yaml b/nav2_bringup/maps/depot_keepout.yaml new file mode 100644 index 00000000000..31c69b3d93d --- /dev/null +++ b/nav2_bringup/maps/depot_keepout.yaml @@ -0,0 +1,7 @@ +image: depot_keepout.pgm +mode: trinary +resolution: 0.05 +origin: [0.0, 0.0, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/nav2_bringup/maps/warehouse_keepout.pgm b/nav2_bringup/maps/warehouse_keepout.pgm new file mode 100644 index 00000000000..c654e209c42 Binary files /dev/null and b/nav2_bringup/maps/warehouse_keepout.pgm differ diff --git a/nav2_bringup/maps/warehouse_keepout.yaml b/nav2_bringup/maps/warehouse_keepout.yaml new file mode 100644 index 00000000000..4c14f4fe957 --- /dev/null +++ b/nav2_bringup/maps/warehouse_keepout.yaml @@ -0,0 +1,7 @@ +image: warehouse_keepout.pgm +mode: trinary +resolution: 0.03 +origin: [-15.1, -25, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.1 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 1a268565663..bf9ee77364e 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -224,6 +224,11 @@ local_costmap: resolution: 0.05 robot_radius: 0.22 plugins: ["voxel_layer", "inflation_layer"] + filters: ["keepout_filter"] + keepout_filter: + plugin: "nav2_costmap_2d::KeepoutFilter" + enabled: True + filter_info_topic: "costmap_filter_info" inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -271,6 +276,11 @@ global_costmap: resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + filters: ["keepout_filter"] + keepout_filter: + plugin: "nav2_costmap_2d::KeepoutFilter" + enabled: True + filter_info_topic: "costmap_filter_info" obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -309,6 +319,19 @@ map_server: # yaml_filename: "" service_introspection_mode: "disabled" +filter_mask_server: + ros__parameters: + topic_name: "keepout_filter_mask" + # yaml_filename: "" + +costmap_filter_info_server: + ros__parameters: + type: 0 + filter_info_topic: "costmap_filter_info" + mask_topic: "keepout_filter_mask" + base: 0.0 + multiplier: 1.0 + map_saver: ros__parameters: save_map_timeout: 5.0 diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp index 34f0314b76d..dbbfabbbfb6 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp @@ -194,24 +194,6 @@ class CostmapLayer : public Layer, public Costmap2D */ CombinationMethod combination_method_from_int(const int value); - /** - * Joins the specified topic with the parent namespace of the costmap node. - * If the topic has an absolute path, it is returned instead. - * - * This is necessary for user defined relative topics to work as expected since costmap layers - * add a an additional `costmap_name` namespace to the topic. - * For example: - * * User chosen namespace is `tb4`. - * * User chosen topic is `scan`. - * * Costmap node namespace will be `/tb4/global_costmap`. - * * Without this function, the topic would be `/tb4/global_costmap/scan`. - * * With this function, topic will be remapped to `/tb4/scan`. - * Use global topic `/scan` if you do not wish the node namespace to apply - * - * @param topic the topic to parse - */ - std::string joinWithParentNamespace(const std::string & topic); - private: double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index fe3d7134ce3..7c78424f02b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -161,6 +161,24 @@ class Layer /** @brief Convenience functions for declaring ROS parameters */ std::string getFullName(const std::string & param_name); + /** + * Joins the specified topic with the parent namespace of the layer node. + * If the topic has an absolute path, it is returned instead. + * + * This is necessary for user defined relative topics to work as expected since costmap layers + * add an additional `costmap_name` namespace to the topic. + * For example: + * * User chosen namespace is `tb4`. + * * User chosen topic is `scan`. + * * Costmap node namespace will be `/tb4/global_costmap`. + * * Without this function, the topic would be `/tb4/global_costmap/scan`. + * * With this function, topic will be remapped to `/tb4/scan`. + * Use global topic `/scan` if you do not wish the node namespace to apply + * + * @param topic the topic to parse + */ + std::string joinWithParentNamespace(const std::string & topic); + protected: LayeredCostmap * layered_costmap_; std::string name_; diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 74277fb58d3..ac01861ef67 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -63,7 +63,7 @@ void KeepoutFilter::initializeFilter( throw std::runtime_error{"Failed to lock node"}; } - filter_info_topic_ = filter_info_topic; + filter_info_topic_ = joinWithParentNamespace(filter_info_topic); // Setting new costmap filter info subscriber RCLCPP_INFO( logger_, @@ -108,7 +108,7 @@ void KeepoutFilter::filterInfoCallback( BASE_DEFAULT, MULTIPLIER_DEFAULT); } - mask_topic_ = msg->filter_mask_topic; + mask_topic_ = joinWithParentNamespace(msg->filter_mask_topic); // Setting new filter mask subscriber RCLCPP_INFO( diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 72a0120cd58..9b597962d10 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -273,20 +273,4 @@ CombinationMethod CostmapLayer::combination_method_from_int(const int value) return CombinationMethod::Max; } } - -std::string CostmapLayer::joinWithParentNamespace(const std::string & topic) -{ - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - - if (topic[0] != '/') { - std::string node_namespace = node->get_namespace(); - std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/")); - return parent_namespace + "/" + topic; - } - - return topic; -} } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index 852b4b29b5a..dba276d0364 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -116,4 +116,22 @@ Layer::getFullName(const std::string & param_name) return std::string(name_ + "." + param_name); } + +std::string +Layer::joinWithParentNamespace(const std::string & topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (topic[0] != '/') { + std::string node_namespace = node->get_namespace(); + std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/")); + return parent_namespace + "/" + topic; + } + + return topic; +} + } // end namespace nav2_costmap_2d diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 71d4189994a..b9ccee55d04 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -146,6 +146,7 @@ def generate_launch_description() -> LaunchDescription: launch_arguments={ 'namespace': '', 'map': map_yaml_file, + 'use_keepout_zones': 'False', 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml,