FallenSigh
a546aff1a6
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>
129 lines
5.2 KiB
Python
129 lines
5.2 KiB
Python
#!/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',
|
||
),
|
||
])
|