[nav2_map_server] add launcher for map_server

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

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<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> occupied =
-      (normalized.array() >= load_parameters.occupied_thresh).cast<uint8_t>();
+      (normalized.array() >= occupied_thresh).cast<uint8_t>();
 
     Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> free =
-      (normalized.array() <= load_parameters.free_thresh).cast<uint8_t>();
+      (normalized.array() <= free_thresh).cast<uint8_t>();
 
     // Initialize occupancy grid with UNKNOWN values (-1)
     result.setConstant(nav2_util::OCC_GRID_UNKNOWN);