templates/lms-nav2-gazebo-localization
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(nav2): align localization with tested platform runtime

Browse Source
This commit is contained in:
chyoo
2026-08-10 16:57:41 +09:00
parent 8291e681ad
commit 1a0f1d18ba
5 changed files with 77 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/localization_pkg/config/localization.yaml
+1 -1
View File
@@ -42,4 +42,4 @@ amcl:
z_rand: 0.2
z_short: 0.05
scan_topic: /scan
set_initial_pose: False # RViz 2D Pose Estimate 으로 사용자가 직접 init
set_initial_pose: False # RViz 2D Pose Estimate 으로 사용자가 직접 init
src/localization_pkg/config/rtabmap_mkmini.yaml
+2 -2
View File
@@ -68,7 +68,7 @@ rtabmap:
Grid/MinClusterSize: "10"
Grid/FlatObstacleDetected: "true"
RGBD/StartAtOrigin: "true"
RGBD/StartAtOrigin: "false"
RGBD/LocalizationPriorError: "5.0"
RGBD/LocalizationSmoothing: "false"
Icp/GuessFromTf: "false"
@@ -93,4 +93,4 @@ rtabmap:
Icp/Force4DoF: "true"
Icp/CorrespondenceRatio: "0.02"
Odom/GuessMotion: "false"
Odom/GuessMotion: "false"
src/localization_pkg/launch/localization.launch.py
+10 -2
View File
@@ -10,14 +10,22 @@ def generate_launch_description():
default_value='amcl',
choices=['amcl', 'slam'],
)
database_path = DeclareLaunchArgument(
'database_path',
default_value='',
description='Saved RTAB-Map database used only by 3D localization.',
)
node = Node(
package='localization_pkg',
executable='localization_manager',
name='localization_manager',
output='screen',
parameters=[{'localization_mode': LaunchConfiguration('localization_mode')}],
parameters=[{
'localization_mode': LaunchConfiguration('localization_mode'),
'database_path': LaunchConfiguration('database_path'),
}],
)
ld = LaunchDescription([localization_mode])
ld = LaunchDescription([localization_mode, database_path])
ld.add_action(node)
return ld
src/localization_pkg/launch/localization3d.launch.py
+44 -17
View File
@@ -2,28 +2,40 @@
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import GroupAction
from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterFile
from launch.substitutions import PythonExpression
from launch.conditions import IfCondition, UnlessCondition
from nav2_common.launch import RewrittenYaml
import os
ARGUMENTS = [
DeclareLaunchArgument('sim_mode', default_value='false'),
DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09']),
DeclareLaunchArgument('database_path', default_value=''),
]
def generate_launch_description():
def launch_setup(context, *args, **kwargs):
robot = LaunchConfiguration('robot')
sim_mode = LaunchConfiguration('sim_mode')
database_path = LaunchConfiguration('database_path').perform(context).strip()
if not database_path:
raise RuntimeError('3D localization requires database_path:=/modules/maps/<saved-map>.db')
if not os.path.isfile(database_path):
raise RuntimeError(f'3D localization database was not found: {database_path}')
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static'),
('odom', 'icp_odom'),
('scan_cloud', '/lidar_filtered')]
remappings = [
('/tf', 'tf'),
('/tf_static', 'tf_static'),
('odom', 'icp_odom'),
('scan_cloud', '/lidar_filtered'),
]
# Nav2 map_server owns /map during navigation and publishes the complete
# saved PGM map. Keep RTAB-Map's local occupancy update separate so it
# cannot race map_server and replace the navigation map intermittently.
rtabmap_remappings = [*remappings, ('map', '/rtabmap/map')]
# Directories
pkg_localization = get_package_share_directory(
@@ -33,16 +45,14 @@ def generate_launch_description():
pkg_localization,
'config',
PythonExpression(["'rtabmap_' + '", robot, "' + '.yaml'"])])
param_substitutions = {
'use_sim_time': 'true'
}
param_substitutions = {'use_sim_time': sim_mode}
configured_params = ParameterFile(
RewrittenYaml(
source_file=rtabmap_params_file,
param_rewrites=param_substitutions,
convert_types=True),
allow_substs=True)
load_nodes = GroupAction(
actions=[
Node(
@@ -50,15 +60,32 @@ def generate_launch_description():
executable='icp_odometry',
name='icp_odometry',
output='screen',
parameters=[configured_params, {'use_sim_time': True}],
parameters=[configured_params, {'use_sim_time': sim_mode}],
remappings=remappings,
)
),
Node(
package='rtabmap_slam',
executable='rtabmap',
name='rtabmap',
output='screen',
parameters=[configured_params, {
'use_sim_time': sim_mode,
'database_path': database_path,
'Mem/IncrementalMemory': 'false',
'Mem/InitWMWithAllNodes': 'true',
'map_frame_id': 'map',
'odom_frame_id': 'icp_odom',
'publish_tf': True,
}],
remappings=rtabmap_remappings,
),
],
)
return [load_nodes]
def generate_launch_description():
# Create launch description and add actions
ld = LaunchDescription(ARGUMENTS)
ld.add_action(load_nodes)
ld.add_action(OpaqueFunction(function=launch_setup))
return ld
src/localization_pkg/localization_pkg/localization_manager.py
+20 -6
View File
@@ -6,6 +6,7 @@ from std_msgs.msg import String
import subprocess
import json
import uuid
class LocalizationManager(Node):
def __init__(self):
@@ -19,8 +20,14 @@ class LocalizationManager(Node):
)
self.localization_process = None
self.config_received = False
self.localization_session_id = uuid.uuid4().hex
self.declare_parameter('localization_mode', 'amcl')
self.declare_parameter('database_path', '')
self.localization_mode = self.get_parameter('localization_mode').value
self.database_path = self.get_parameter('database_path').value
self.localization_session_pub = self.create_publisher(String, '/localization_session_id', qos)
self.localization_session_pub.publish(String(data=self.localization_session_id))
self.get_logger().info(f'Localization session: {self.localization_session_id}')
self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos)
@@ -31,6 +38,19 @@ class LocalizationManager(Node):
self.get_logger().info(f"Localization Received config: {msg.data}")
config_data = json.loads(msg.data)
if config_data.get("level") == "3d":
if not self.database_path:
self.get_logger().error('3D localization requires a saved RTAB-Map database_path.')
return
self.get_logger().info('Starting RTAB-Map 3D localization mode.')
self.localization_process = subprocess.Popen(['ros2', 'launch',
'localization_pkg',
'localization3d.launch.py',
f'sim_mode:={config_data.get("sim_mode")}',
f'robot:={config_data.get("robot")}',
f'database_path:={self.database_path}'])
return
if self.localization_mode == 'slam':
self.get_logger().info('Starting SLAM Toolbox localization mode.')
self.localization_process = subprocess.Popen(['ros2', 'launch',
@@ -46,12 +66,6 @@ class LocalizationManager(Node):
'localization2d.launch.py',
f'sim_mode:={config_data.get("sim_mode")}',
f'robot:={config_data.get("robot")}'])
elif config_data.get("level") == "3d":
self.localization_process = subprocess.Popen(['ros2', 'launch',
'localization_pkg',
'localization3d.launch.py',
f'sim_mode:={config_data.get("sim_mode")}',
f'robot:={config_data.get("robot")}'])
pass
def destroy_node(self):