feat: add lms-nav2-gazebo-map module

This commit is contained in:
2026-07-28 10:35:36 +09:00
commit 7d49d33cfc
33 changed files with 663 additions and 0 deletions
+5
View File
@@ -0,0 +1,5 @@
build/
install/
log/
.colcon/
*.log
Executable
+6
View File
@@ -0,0 +1,6 @@
#!/usr/bin/env bash
set -euo pipefail
MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}"
exec bash "$RUNTIME_ROOT/docker/docker_build_nav2.sh" "$(dirname "$MODULE_PATH")" "$(basename "$MODULE_PATH")" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}"
Executable
+8
View File
@@ -0,0 +1,8 @@
#!/usr/bin/env bash
set -euo pipefail
MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}"
FOLDER_NAME="$(basename "$MODULE_PATH")"
CONTAINER_NAME="${FOLDER_NAME#\[}"; CONTAINER_NAME="${CONTAINER_NAME/\]/-}"
exec bash "$RUNTIME_ROOT/docker/docker_run_nav2.sh" "$CONTAINER_NAME" "$(dirname "$MODULE_PATH")" "$FOLDER_NAME" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}" "$@"
+15
View File
@@ -0,0 +1,15 @@
# File: map.yaml
# Description: Map server parameters
map_server:
ros__parameters:
use_sim_time: True
yaml_filename: "" # overridden at launch time via map_yaml arg
map_saver:
ros__parameters:
use_sim_time: True
map_subscribe_transient_local: True
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
+97
View File
@@ -0,0 +1,97 @@
icp_odometry:
ros__parameters:
odom_frame_id: icp_odom
guess_frame_id: odom
OdomF2M/ScanSubtractRadius: '0.0'
OdomF2M/ScanMaxSize: '10000'
frame_id: base_link
wait_for_transform: 5.0
approx_sync: true
topic_queue_size: 30
sync_queue_size: 30
Reg/Strategy: "1"
Reg/Force3DoF: "true"
Mem/NotLinkedNodesKept: "false"
Icp/VoxelSize: "0.03"
Icp/MaxCorrespondenceDistance: "0.2"
Icp/PointToPlaneGroundNormalsUp: "0.9"
Icp/RangeMin: "0.0"
Icp/MaxTranslation: "1.0"
Icp/MaxRotation: "0.5" # 한 번의 odometry 업데이트 최대 회전 제한. 크면 오정합 발생. 작으면 계산량 증가. default: 0.78rad
Icp/PointToPlaneMinComplexity: "0.03"
Icp/PointToPlaneLowComplexityStrategy: "1"
Icp/OutlierRatio: "0.6"
Icp/Force4DoF: "true"
Icp/CorrespondenceRatio: "0.02"
Icp/GuessFromTf: "true"
Odom/GuessMotion: "false"
rtabmap:
ros__parameters:
subscribe_rgb: false
subscribe_depth: false
subscribe_scan_cloud: true
use_action_for_goal: true
odom_sensor_sync: true
approx_sync: true
topic_queue_size: 30
sync_queue_size: 30
Mem/NotLinkedNodesKept: "false"
Grid/RangeMin: "0.0"
Grid/NormalsSegmentation: "false"
Grid/MaxGroundHeight: "-0.8"
Grid/MinGroundHeight: "-1.3"
Grid/MaxObstacleHeight: "1.0"
Grid/RayTracing: "false"
Grid/3D: "false"
Grid/Sensor: "0"
RGBD/OptimizeMaxError: "0.5"
RGBD/LinearUpdate: "0.05"
RGBD/AngularUpdate: "0.05"
RGBD/ProximityPathMaxNeighbors: "1"
Grid/RangeMax: "25.0"
Mem/LaserScanVoxelSize: "0.01"
Odom/FilteringStrategy: "1"
Odom/ScanKeyFrameThr: "0.9"
Odom/GuessMotion: "false"
Grid/CellSize: "0.2"
Grid/MinClusterSize: "10"
Grid/FlatObstacleDetected: "false"
RGBD/StartAtOrigin: "true"
RGBD/LocalizationPriorError: "2.0"
RGBD/LocalizationSmoothing: "false"
Icp/GuessFromTf: "true"
frame_id: base_link
wait_for_transform: 5.0
Reg/Strategy: "1"
Reg/Force3DoF: "true"
Icp/VoxelSize: "0.03"
Icp/MaxCorrespondenceDistance: "0.2"
Icp/PointToPlaneGroundNormalsUp: "0.9"
Icp/RangeMin: "0.0"
Icp/MaxTranslation: "1.0"
Icp/MaxRotation: "0.78"
Icp/PointToPlaneMinComplexity: "0.03"
Icp/PointToPlaneLowComplexityStrategy: "1"
Icp/OutlierRatio: "0.6"
Icp/Force4DoF: "true"
Icp/CorrespondenceRatio: "0.2"
Odom/GuessMotion: "false"
+96
View File
@@ -0,0 +1,96 @@
icp_odometry:
ros__parameters:
odom_frame_id: icp_odom
guess_frame_id: odom
OdomF2M/ScanSubtractRadius: '0.0'
OdomF2M/ScanMaxSize: '10000'
frame_id: base_link
wait_for_transform: 2.0
approx_sync: true
topic_queue_size: 30
sync_queue_size: 30
Reg/Strategy: "1"
Reg/Force3DoF: "true"
Mem/NotLinkedNodesKept: "false"
Icp/VoxelSize: "0.03"
Icp/MaxCorrespondenceDistance: "0.2"
Icp/PointToPlaneGroundNormalsUp: "0.9"
Icp/RangeMin: "0.0"
Icp/MaxTranslation: "1.0"
Icp/MaxRotation: "0.78"
Icp/PointToPlaneMinComplexity: "0.03"
Icp/PointToPlaneLowComplexityStrategy: "1"
Icp/OutlierRatio: "0.6"
Icp/Force4DoF: "true"
Icp/CorrespondenceRatio: "0.02"
Icp/GuessFromTf: "false"
Odom/GuessMotion: "false"
rtabmap:
ros__parameters:
subscribe_rgb: false
subscribe_depth: false
subscribe_scan_cloud: true
use_action_for_goal: true
odom_sensor_sync: true
approx_sync: true
topic_queue_size: 30
sync_queue_size: 30
Mem/NotLinkedNodesKept: "false"
Grid/RangeMin: "0.0"
Grid/NormalsSegmentation: "false"
Grid/MaxGroundHeight: "0.1"
Grid/MinGroundHeight: "-0.2"
Grid/MaxObstacleHeight: "1.0"
Grid/RayTracing: "false"
Grid/3D: "false"
RGBD/OptimizeMaxError: "0.5"
RGBD/LinearUpdate: "0.05"
RGBD/AngularUpdate: "0.05"
RGBD/ProximityPathMaxNeighbors: "1"
Grid/RangeMax: "10.0"
Mem/LaserScanVoxelSize: "0.01"
Odom/FilteringStrategy: "1"
Odom/ScanKeyFrameThr: "0.9"
Odom/GuessMotion: "false"
Grid/CellSize: "0.2"
Grid/MinClusterSize: "10"
Grid/FlatObstacleDetected: "true"
RGBD/StartAtOrigin: "true"
RGBD/LocalizationPriorError: "5.0"
RGBD/LocalizationSmoothing: "false"
Icp/GuessFromTf: "false"
frame_id: base_link
wait_for_transform: 2.0
Reg/Strategy: "1"
Reg/Force3DoF: "true"
Icp/VoxelSize: "0.03"
Icp/MaxCorrespondenceDistance: "0.2"
Icp/PointToPlaneGroundNormalsUp: "0.9"
Icp/RangeMin: "0.0"
Icp/MaxTranslation: "1.0"
Icp/MaxRotation: "0.78"
Icp/PointToPlaneMinComplexity: "0.03"
Icp/PointToPlaneLowComplexityStrategy: "1"
Icp/OutlierRatio: "0.6"
Icp/Force4DoF: "true"
Icp/CorrespondenceRatio: "0.02"
Odom/GuessMotion: "false"
Binary file not shown.
+45
View File
@@ -0,0 +1,45 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
default_map = [FindPackageShare('map_pkg'), '/map/gazebo/turtlebot3_world.yaml']
selected_map = LaunchConfiguration('map')
return LaunchDescription([
DeclareLaunchArgument('map', default_value=default_map),
DeclareLaunchArgument('use_sim_time', default_value='true'),
Node(
package='map_pkg',
executable='pub_map_config',
name='pub_map_config',
output='screen',
parameters=[{'level': '2d'}],
),
Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[{
'use_sim_time': LaunchConfiguration('use_sim_time'),
'yaml_filename': selected_map,
}],
),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='map_server_lifecycle_manager',
output='screen',
condition=IfCondition(PythonExpression(["'", selected_map, "' != ''"])),
parameters=[{
'use_sim_time': LaunchConfiguration('use_sim_time'),
'autostart': True,
'node_names': ['map_server'],
}],
),
])
+153
View File
@@ -0,0 +1,153 @@
# File: map.launch.py
# Description:
# 맵 서버 — map_server (2D) 또는 rtabmap (3D)
# /nav2_ws/selected_map/ 폴더 자동 스캔 → *.yaml 면 2D, *.db 면 3D 분기
# 사용자 UI 가 선택한 map 을 그 폴더에 cp 해두면 launch 가 자동 처리.
# Publishes : /map
# Note: localization(AMCL)보다 먼저 실행되어야 함
import glob
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.substitutions import PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch_ros.descriptions import ParameterFile
from nav2_common.launch import RewrittenYaml
SELECTED_MAP_DIR = '/nav2_ws/selected_map'
ARGUMENTS = [
DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09']),
]
def _detect_level_and_file():
"""selectedMap 폴더 스캔: *.yaml → 2D, *.db → 3D. 우선순위 yaml > db (둘 다 있을 때)."""
yaml_files = sorted(glob.glob(os.path.join(SELECTED_MAP_DIR, '*.yaml')))
db_files = sorted(glob.glob(os.path.join(SELECTED_MAP_DIR, '*.db')))
if yaml_files:
return '2d', yaml_files[0]
if db_files:
return '3d', db_files[0]
raise RuntimeError(
f"No map file found in {SELECTED_MAP_DIR}. "
f"Expected *.yaml (2D) or *.db (3D). "
f"User must select a map in the UI which copies the file to selectedMap/."
)
def launch_setup(context, *args, **kwargs):
robot = LaunchConfiguration('robot').perform(context)
level, map_file = _detect_level_and_file()
print(f"[map.launch.py] auto-detected level={level} map={map_file}")
if level == '2d':
lifecycle_nodes = ['map_server']
configured_params = ParameterFile(
RewrittenYaml(
source_file=PathJoinSubstitution([
FindPackageShare('map_pkg'), 'config', 'map_2d.yaml'
]),
param_rewrites={
'use_sim_time': 'true',
'yaml_filename': map_file,
},
convert_types=True,
),
allow_substs=True,
)
load_nodes = GroupAction(
actions=[
Node(
package='map_pkg',
executable='pub_map_config',
name='pub_map_config',
output='screen',
parameters=[{'level': level}]
),
Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[configured_params],
),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_map',
output='screen',
parameters=[{
'use_sim_time': True,
'autostart': True,
'node_names': lifecycle_nodes,
}],
),
],
)
else: # 3d
configured_params = ParameterFile(
RewrittenYaml(
source_file=PathJoinSubstitution([
FindPackageShare('map_pkg'),
'config',
PythonExpression(["'map_", level, "_", robot, ".yaml'"])
]),
param_rewrites={
'use_sim_time': 'true',
'Mem/IncrementalMemory': 'False',
'Mem/InitWMWithAllNodes': 'True',
},
convert_types=True,
),
allow_substs=True,
)
remappings = [
('/tf', 'tf'),
('/tf_static', 'tf_static'),
('odom', 'icp_odom'),
('scan_cloud', '/lidar_filtered'),
('rgb/image', '/carla/hero/rgb_front/image'),
('rgb/camera_info', '/carla/hero/rgb_front/camera_info'),
]
load_nodes = GroupAction(
actions=[
Node(
package='map_pkg',
executable='pub_map_config',
name='pub_map_config',
output='screen',
parameters=[{'level': level}]
),
Node(
package='rtabmap_slam',
executable='rtabmap',
name='rtabmap',
output='screen',
parameters=[configured_params,
{'Mem/IncrementalMemory': 'False',
'Mem/InitWMWithAllNodes': 'True',
'use_sim_time': True,
'database_path': map_file}],
remappings=remappings,
)
],
)
return [load_nodes]
def generate_launch_description():
ld = LaunchDescription(ARGUMENTS)
ld.add_action(OpaqueFunction(function=launch_setup))
return ld
BIN
View File
Binary file not shown.
+7
View File
@@ -0,0 +1,7 @@
image: hallway.pgm
mode: trinary
resolution: 0.05
origin: [-6.78, -5.39, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
BIN
View File
Binary file not shown.
+7
View File
@@ -0,0 +1,7 @@
image: map.pgm
mode: trinary
resolution: 0.05
origin: [-7.36, -29.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
BIN
View File
Binary file not shown.
+7
View File
@@ -0,0 +1,7 @@
image: office.pgm
mode: trinary
resolution: 0.05
origin: [-3.44, -11.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
BIN
View File
Binary file not shown.
+7
View File
@@ -0,0 +1,7 @@
image: parkinglot.pgm
mode: trinary
resolution: 0.05
origin: [-7.36, -29.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
+4
View File
File diff suppressed because one or more lines are too long
+7
View File
@@ -0,0 +1,7 @@
image: map.pgm
mode: trinary
resolution: 0.05
origin: [-3.78, -4.29, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
File diff suppressed because one or more lines are too long
@@ -0,0 +1,6 @@
image: turtlebot3_world.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
View File
Binary file not shown.
Binary file not shown.
+47
View File
@@ -0,0 +1,47 @@
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy
from std_msgs.msg import String
import json
class ConfigPublisher(Node):
def __init__(self):
super().__init__('pub_map_config')
self.declare_parameter('level', '2d')
level = self.get_parameter('level').value
qos = QoSProfile(
depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
reliability=ReliabilityPolicy.RELIABLE
)
self.pub = self.create_publisher(String, '/map_config', qos)
msg = {
"level": level
}
ros_msg = String()
ros_msg.data = json.dumps(msg)
# 약간 대기 후 1회 발행
self.create_timer(1.0, self.publish_once)
self.sent = False
self.cached_msg = ros_msg
def publish_once(self):
if not self.sent:
self.pub.publish(self.cached_msg)
self.get_logger().info("Published config once")
self.sent = True
def main():
rclpy.init()
node = ConfigPublisher()
rclpy.spin(node)
rclpy.shutdown()
+6
View File
@@ -0,0 +1,6 @@
def main():
print('Hi from map_pkg.')
if __name__ == '__main__':
main()
+18
View File
@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>map_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="jang1future@gmail.com">dgist</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
View File
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/map_pkg
[install]
install_scripts=$base/lib/map_pkg
+40
View File
@@ -0,0 +1,40 @@
import os
from glob import glob
from setuptools import find_packages, setup
package_name = 'map_pkg'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
(os.path.join('share', package_name, 'map/2d'), glob('map/2d/*.yaml')),
(os.path.join('share', package_name, 'map/2d'), glob('map/2d/*.pgm')),
(os.path.join('share', package_name, 'map/gazebo'), glob('map/gazebo/*.yaml')),
(os.path.join('share', package_name, 'map/gazebo'), glob('map/gazebo/*.pgm')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='dgist',
maintainer_email='jang1future@gmail.com',
description='TODO: Package description',
license='TODO: License declaration',
extras_require={
'test': [
'pytest',
],
},
entry_points={
'console_scripts': [
'test = map_pkg.test:main',
'pub_map_config = map_pkg.pub_map_config:main'
],
},
)
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
+23
View File
@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# 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.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'