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..fd6d1dba242 --- /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 diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index f85b9161e34..4ad3d73cfa7 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -261,12 +261,25 @@ 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);