feat(craic_localization): add package foundation, configs, and launch files
New ROS2 package for wheel odometry + LiDAR localization. Includes: AMCL config (omni model, likelihood_field), gmapping config (5cm grid, 30 particles), YDLiDAR TminiPro config, launch files for lidar bringup, SLAM mapping, AMCL localization, and sensor calibration, plus RViz config with Map/LaserScan/Odometry displays. Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent) Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
This commit is contained in:
128
ros2/src/craic_localization/launch/localization.launch.py
Normal file
128
ros2/src/craic_localization/launch/localization.launch.py
Normal file
@@ -0,0 +1,128 @@
|
||||
#!/usr/bin/env python3
|
||||
"""P5 精准定位：激光 + rf2o 激光里程计 + 已知地图 + AMCL。
|
||||
|
||||
完整 TF 树：map ─(amcl)→ odom ─(rf2o)→ base_footprint ─(静态)→ laser_frame
|
||||
odom→base_footprint 由 rf2o 从 /scan 连续提供(不依赖底盘里程计板)，AMCL 用它做运动模型、
|
||||
激光匹配已知地图做校正，输出绝对、无漂移位姿。需要 P4 建好并存盘的地图。
|
||||
|
||||
说明：底盘轮式里程计板上电后仅短时上报，暂不参与 odom→base；待其连续上报问题解决后，
|
||||
可用 robot_localization EKF 融合轮速 + rf2o 再喂 AMCL 提升鲁棒性。
|
||||
|
||||
用法：
|
||||
ros2 launch craic_localization localization.launch.py map:=/abs/path/maps/craic.yaml
|
||||
# 起点不在地图原点时，rviz 用 "2D Pose Estimate" 给初值，或：
|
||||
ros2 launch craic_localization localization.launch.py map:=... init_x:=0.3 init_y:=0.2 init_yaw:=1.57
|
||||
"""
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.descriptions import ParameterValue
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_share = get_package_share_directory('craic_localization')
|
||||
lidar_launch = os.path.join(pkg_share, 'launch', 'lidar.launch.py')
|
||||
amcl_params = os.path.join(pkg_share, 'config', 'amcl.yaml')
|
||||
default_rviz = os.path.join(pkg_share, 'rviz', 'localization.rviz')
|
||||
default_map = os.path.join(pkg_share, 'maps', 'craic.yaml')
|
||||
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
map_yaml = LaunchConfiguration('map')
|
||||
init_x = LaunchConfiguration('init_x')
|
||||
init_y = LaunchConfiguration('init_y')
|
||||
init_yaw = LaunchConfiguration('init_yaw')
|
||||
show_points = LaunchConfiguration('show_points')
|
||||
points_file = LaunchConfiguration('points_file')
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('use_rviz', default_value='true'),
|
||||
DeclareLaunchArgument('map', default_value=default_map,
|
||||
description='地图 yaml 路径(P4 由 map_saver 存盘得到)'),
|
||||
DeclareLaunchArgument('init_x', default_value='0.0'),
|
||||
DeclareLaunchArgument('init_y', default_value='0.0'),
|
||||
DeclareLaunchArgument('init_yaw', default_value='0.0'),
|
||||
DeclareLaunchArgument('show_points', default_value='true'),
|
||||
DeclareLaunchArgument('points_file',
|
||||
default_value=os.path.join(pkg_share, 'config', 'taught_points.yaml'),
|
||||
description='示教点位文件(rviz 显示)；想实时看示教就指向你的 taught_points.yaml'),
|
||||
|
||||
# ===== 传感器 + 激光里程计(odom -> base_footprint) =====
|
||||
IncludeLaunchDescription(PythonLaunchDescriptionSource(lidar_launch)),
|
||||
Node(
|
||||
package='rf2o_laser_odometry',
|
||||
executable='rf2o_laser_odometry_node',
|
||||
name='rf2o_laser_odometry',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'laser_scan_topic': '/scan',
|
||||
'odom_topic': '/odom',
|
||||
'publish_tf': True,
|
||||
'base_frame_id': 'base_footprint',
|
||||
'odom_frame_id': 'odom',
|
||||
'init_pose_from_topic': '',
|
||||
'freq': 10.0,
|
||||
}],
|
||||
),
|
||||
|
||||
# ===== 地图服务 =====
|
||||
Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='map_server',
|
||||
output='screen',
|
||||
parameters=[amcl_params, {'yaml_filename': map_yaml}],
|
||||
),
|
||||
|
||||
# ===== AMCL 定位 =====
|
||||
Node(
|
||||
package='nav2_amcl',
|
||||
executable='amcl',
|
||||
name='amcl',
|
||||
output='screen',
|
||||
parameters=[amcl_params, {
|
||||
'set_initial_pose': True,
|
||||
'initial_pose.x': ParameterValue(init_x, value_type=float),
|
||||
'initial_pose.y': ParameterValue(init_y, value_type=float),
|
||||
'initial_pose.yaw': ParameterValue(init_yaw, value_type=float),
|
||||
}],
|
||||
),
|
||||
|
||||
# ===== 生命周期管理：自动 configure+activate map_server 与 amcl =====
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_localization',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': False,
|
||||
'autostart': True,
|
||||
'node_names': ['map_server', 'amcl'],
|
||||
}],
|
||||
),
|
||||
|
||||
# ===== 示教点位可视化 =====
|
||||
Node(
|
||||
package='craic_localization',
|
||||
executable='show_points',
|
||||
name='show_points',
|
||||
output='screen',
|
||||
parameters=[{'points_file': points_file}],
|
||||
condition=IfCondition(show_points),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
arguments=['-d', default_rviz],
|
||||
condition=IfCondition(use_rviz),
|
||||
output='screen',
|
||||
),
|
||||
])
|
||||
Reference in New Issue
Block a user