1. 基于地图的全局路径规划

任务目标

在Autoware环境中，基于lanelet2高精地图，实现从指定起点到终点的全局路径规划，输出可行驶的路径点序列（含经纬度坐标与航向角），用于后续局部规划模块的参考线输入。

环境与依赖

• Autoware Universe（>= 2024.03）
• lanelet2地图文件（OpenDRIVE格式，本案例使用示例地图 sample_map.osm）
• 依赖包：autoware_planning_msgs、lanelet2_extension、autoware_route_handler

核心实现

本案例通过ROS 2节点加载lanelet2地图，调用Autoware的RouteHandler模块完成全局路径搜索。核心逻辑包括：地图加载、起点/终点绑定到最近车道、使用A*算法搜索车道序列、将车道中心线采样为路径点并发布。

#!/usr/bin/env python3
# global_planner.py
import math
from typing import List, Tuple

import lanelet2
from lanelet2.core import BasicPoint2d
from lanelet2.projection import UtmProjector

import rclpy
from rclpy.node import Node
from autoware_planning_msgs.msg import PathPoint, Path
from geometry_msgs.msg import PoseStamped

class GlobalPlanner(Node):
    def __init__(self):
        super().__init__('global_planner')
        
        # 1. 加载lanelet2地图
        self.projector = UtmProjector(lanelet2.io.Origin(0.0, 0.0))
        self.lanelet_map = lanelet2.io.load('sample_map.osm', self.projector)
        
        # 2. 初始化RouteHandler（Autoware全局规划核心）
        from autoware_route_handler.route_handler import RouteHandler
        self.route_handler = RouteHandler(self.lanelet_map)
        
        # 3. 路径发布器
        self.path_pub = self.create_publisher(Path, '/planning/global_path', 10)
        
        # 4. 规划请求订阅（示例中直接调用）
        self.start = self._find_nearest_lanelet(139.75, 35.65)   # 起点经纬度
        self.goal = self._find_nearest_lanelet(139.78, 35.68)    # 终点经纬度
        
    def _find_nearest_lanelet(self, lat: float, lon: float) -> lanelet2.core.Lanelet:
        """将经纬度坐标转换为地图中的车道let"""
        point = self.projector.forward(BasicPoint2d(lon, lat))
        nearest = None
        min_dist = float('inf')
        for lanelet in self.lanelet_map.laneletLayer:
            center = lanelet.centerline
            for pt in center:
                d = (pt.x - point.x)**2 + (pt.y - point.y)**2
                if d < min_dist:
                    min_dist = d
                    nearest = lanelet
        return nearest
    
    def plan_global_path(self) -> Path:
        """执行全局路径规划并返回路径点序列"""
        # 使用RouteHandler搜索车道级路径
        route = self.route_handler.plan_route(self.start, self.goal)
        if not route:
            self.get_logger().error('路径规划失败：起点与终点不可达')
            return Path()
        
        # 将车道序列转换为路径点（每0.5m采样）
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        path_msg.header.stamp = self.get_clock().now().to_msg()
        
        for lanelet_id in route:
            lanelet = self.lanelet_map.laneletLayer[lanelet_id]
            centerline = lanelet.centerline
            
            # 按弧长采样
            accumulated = 0.0
            prev_pt = None
            for pt in centerline:
                if prev_pt is not None:
                    dx = pt.x - prev_pt.x
                    dy = pt.y - prev_pt.y
                    accumulated += math.hypot(dx, dy)
                
                if accumulated >= 0.5 or prev_pt is None:
                    path_point = PathPoint()
                    # 投影回经纬度（仅用于可视化，实际规划使用平面坐标）
                    lon, lat = self.projector.reverse(pt.x, pt.y)
                    path_point.pose.position.x = pt.x
                    path_point.pose.position.y = pt.y
                    path_point.pose.position.z = 0.0
                    
                    # 计算航向角（基于相邻点）
                    if prev_pt is not None:
                        yaw = math.atan2(dy, dx)
                        path_point.pose.orientation.z = math.sin(yaw/2)
                        path_point.pose.orientation.w = math.cos(yaw/2)
                    
                    path_msg.points.append(path_point)
                    accumulated = 0.0
                prev_pt = pt
        
        self.get_logger().info(f'生成路径点 {len(path_msg.points)} 个')
        return path_msg
    
    def run(self):
        """执行一次规划并发布"""
        path = self.plan_global_path()
        if path.points:
            self.path_pub.publish(path)
            self.get_logger().info('全局路径已发布到 /planning/global_path')

def main(args=None):
    rclpy.init(args=args)
    planner = GlobalPlanner()
    planner.run()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

运行与验证

1. 准备地图：将sample_map.osm放置于工作目录（可从Autoware示例地图包获取）
2. 编译与运行：

   # 在ROS 2工作空间中
   colcon build --packages-select global_planner
   source install/setup.bash
   ros2 run global_planner global_planner

3. 验证方法：
   • 终端输出“生成路径点 N 个”，N应大于10（表示路径非空）
   • 另开终端执行 ros2 topic echo /planning/global_path，可看到连续的PathPoint序列
   • 在RViz中添加Path显示，选择/planning/global_path话题，应看到从起点到终点的连续曲线

通过判定标准

• 路径点序列的起点与终点经纬度分别接近设定的(139.75, 35.65)和(139.78, 35.68)，误差小于5m
• 相邻路径点间距约为0.5m（允许±0.2m偏差）
• 路径点均落在车道中心线附近（横向偏差小于车道宽度的一半）

常见问题

• 地图加载失败：确认sample_map.osm路径正确，且文件为OpenDRIVE格式
• 规划返回空路径：检查起点/终点是否在可行驶车道上，或地图拓扑是否连通
• 航向角跳变：若路径出现急转弯，可增加采样密度（将0.5m改为0.3m）

上图展示了全局路径规划的基本流程：起点与终点分别绑定到最近车道，RouteHandler搜索车道级连通序列，最终沿车道中心线按固定间距采样生成路径点。该路径点序列可直接作为后续速度规划与局部避障的参考输入。