From 637eb13a3b7f8e8c1315f0e8824f29c88d30f917 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 9 May 2025 12:58:15 +0000 Subject: [PATCH 1/3] [nav2_map_server] add launcher for map_server Signed-off-by: Adi Vardi --- nav2_map_server/launch/map_server.launch.py | 66 +++++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 nav2_map_server/launch/map_server.launch.py diff --git a/nav2_map_server/launch/map_server.launch.py b/nav2_map_server/launch/map_server.launch.py new file mode 100644 index 00000000000..90329c004ff --- /dev/null +++ b/nav2_map_server/launch/map_server.launch.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2020 Samsung Research Russia +# +# 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 +import launch_ros.actions + + +def generate_launch_description() -> LaunchDescription: + # Parameters + bringup_dir = get_package_share_directory("nav2_bringup") + map_yaml_file = os.path.join(bringup_dir, "maps", "depot.yaml") + + lifecycle_nodes = ["map_server"] + use_sim_time = True + autostart = True + topic_name = "map" + frame_id = "map" + service_introspection_mode = "disabled" + + start_map_server_cmd = launch_ros.actions.Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + output="screen", + parameters=[ + {"yaml_filename": map_yaml_file}, + {"topic_name": topic_name}, + {"frame_id": frame_id}, + {"service_introspection_mode": service_introspection_mode}, + ], + ) + + start_lifecycle_manager_cmd = launch_ros.actions.Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager", + output="screen", + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + ) + + ld = LaunchDescription() + + ld.add_action(start_map_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + + return ld From b31b2541a823ffe33fa55465d9ce65669f69580b Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 9 May 2025 13:03:38 +0000 Subject: [PATCH 2/3] [nav2_map_server] fix loading trinary maps Signed-off-by: Adi Vardi --- nav2_map_server/src/map_io.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index f85b9161e34..79678541796 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -261,12 +261,26 @@ void loadMapFromFile( normalized = (1.0f - normalized.array()).matrix(); } + double occupied_thresh = load_parameters.occupied_thresh; + double free_thresh = load_parameters.free_thresh; + + // For Trinary mode, the thresholds depend on the constants used when saving the map, + // not on the thresholds from the yaml file + // occupied is always 1.0, free is always 0.00392157 ( = 1.0 - 254 / 255.0), + // and unknown is always 0.196078431 ( = 1.0 - 205 / 255.0) + // For Scale mode, the thresholds are set by the user in the yaml file + if (load_parameters.mode == MapMode::Trinary) + { + occupied_thresh = 254 / 255.0; + free_thresh = 2 / 255.0; + } + // Compute binary occupancy masks Eigen::Matrix occupied = - (normalized.array() >= load_parameters.occupied_thresh).cast(); + (normalized.array() >= occupied_thresh).cast(); Eigen::Matrix free = - (normalized.array() <= load_parameters.free_thresh).cast(); + (normalized.array() <= free_thresh).cast(); // Initialize occupancy grid with UNKNOWN values (-1) result.setConstant(nav2_util::OCC_GRID_UNKNOWN); From fdcf5876ac0afdcee18abde8d8d733e37f78d886 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 9 May 2025 13:21:00 +0000 Subject: [PATCH 3/3] fix linter issues Signed-off-by: Adi Vardi --- nav2_map_server/launch/map_server.launch.py | 42 ++++++++++----------- nav2_map_server/src/map_io.cpp | 3 +- 2 files changed, 22 insertions(+), 23 deletions(-) diff --git a/nav2_map_server/launch/map_server.launch.py b/nav2_map_server/launch/map_server.launch.py index 90329c004ff..fd6d1dba242 100644 --- a/nav2_map_server/launch/map_server.launch.py +++ b/nav2_map_server/launch/map_server.launch.py @@ -23,38 +23,38 @@ def generate_launch_description() -> LaunchDescription: # Parameters - bringup_dir = get_package_share_directory("nav2_bringup") - map_yaml_file = os.path.join(bringup_dir, "maps", "depot.yaml") + bringup_dir = get_package_share_directory('nav2_bringup') + map_yaml_file = os.path.join(bringup_dir, 'maps', 'depot.yaml') - lifecycle_nodes = ["map_server"] + lifecycle_nodes = ['map_server'] use_sim_time = True autostart = True - topic_name = "map" - frame_id = "map" - service_introspection_mode = "disabled" + topic_name = 'map' + frame_id = 'map' + service_introspection_mode = 'disabled' start_map_server_cmd = launch_ros.actions.Node( - package="nav2_map_server", - executable="map_server", - name="map_server", - output="screen", + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', parameters=[ - {"yaml_filename": map_yaml_file}, - {"topic_name": topic_name}, - {"frame_id": frame_id}, - {"service_introspection_mode": service_introspection_mode}, + {'yaml_filename': map_yaml_file}, + {'topic_name': topic_name}, + {'frame_id': frame_id}, + {'service_introspection_mode': service_introspection_mode}, ], ) start_lifecycle_manager_cmd = launch_ros.actions.Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="lifecycle_manager", - output="screen", + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', parameters=[ - {"use_sim_time": use_sim_time}, - {"autostart": autostart}, - {"node_names": lifecycle_nodes}, + {'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, ], ) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 79678541796..4ad3d73cfa7 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -269,8 +269,7 @@ void loadMapFromFile( // occupied is always 1.0, free is always 0.00392157 ( = 1.0 - 254 / 255.0), // and unknown is always 0.196078431 ( = 1.0 - 205 / 255.0) // For Scale mode, the thresholds are set by the user in the yaml file - if (load_parameters.mode == MapMode::Trinary) - { + if (load_parameters.mode == MapMode::Trinary) { occupied_thresh = 254 / 255.0; free_thresh = 2 / 255.0; }