在 lane driving 场景里，路径规划大多是沿着车道中心线、参考路线、行为决策结果往下做，问题虽然复杂，但“路”本身是明确的。
停车场景不是这样。你可能需要：
这时问题就变成了：给定当前位姿、目标位姿、占据栅格地图和车辆转向约束，怎样找到一条无碰撞、满足车辆运动学、并且允许换向的可执行机动路径。
当然，Free space planner不止适合于停车领域，而是任何在需要精确地改变车辆姿态的情况下都可以使用。
从代码层面看，这个任务被拆成两层：
autoware_freespace_planner 负责节点调度、状态管理和轨迹分段发布。autoware_freespace_planning_algorithms 负责真正的搜索、碰撞检测和路径恢复。这个模块不是一次性生成一条从起点开到终点的完整停车轨迹，而是先搜索一条允许前进、倒车、再前进的机动路径，再把它拆成若干段单方向轨迹，要求车辆每到一个换向点先停稳，再继续下一段。
使用的主要算法有和
autoware_freespace_planner 的核心是
src/autoware_freespace_planner/freespace_planner_node.cpp。
这个节点订阅四类输入：
~/input/route~/input/occupancy_grid~/input/odometry~/input/scenario然后发布：
~/output/trajectoryis_completed虽然输入里有 route，但当前实现并没有把 route 当作一条要跟踪的参考线去使用。onRoute() 只是把消息里的 goal_pose 取出来，更新内部目标，然后把停车周期重新开始。
再往下看 onTimer()，整个节点的工作方式就清楚了。它的主循环大意如下：
if (!parking_scenario_active) {
reset();
return;
}
updateData();
if (!isDataReady()) return;
if (is_completed_) {
publish_stop_trajectory();
return;
}
current_pose_ = odom_->pose.pose;
if (!reset_in_progress_ && isPlanRequired()) {
publish_stop_trajectory_if_needed();
reset();
reset_in_progress_ = true;
}
if (reset_in_progress_ && ego_is_stopped()) {
planTrajectory();
reset_in_progress_ = false;
}
updateTargetIndex();
publish_current_partial_trajectory();
它先判断场景是否激活，再判断数据是否齐全，再决定要不要重规划。如果要重规划，也不是立刻算，而是先让车停住，等速度真的降下来之后再重新生成轨迹。
isPlanRequired() 的条件并不多
第一种情况最简单：当前根本没有轨迹，那当然要规划。
第二种情况是当前轨迹上出现了障碍物，而且这个障碍物持续存在了一段时间，超过 th_obstacle_time_sec。这里的实现需要多说一句，因为它检查的不是整条完整机动路径，而是当前正在执行的那一段 partial_trajectory_。
代码会先找到车辆在当前段上的最近点，然后只截取“从当前位置到当前分段终点”的这一截，交给算法层的 hasObstacleOnTrajectory() 去做碰撞判断。也就是说，节点只关心眼下这段是否还能走，不会提前担心后面那一段倒车路径会不会遇到新障碍。
第三种情况是车辆明显偏离了当前轨迹，超过 th_course_out_distance_m。这个判断也比较朴素：当前车位姿到轨迹最近点的二维距离。
onTimer() 在发现需要重规划时，首先做的是发布 stop trajectory，然后把 reset_in_progress_ 置为 true。此后节点会持续检查 odom_buffer_，只有当这段时间里的速度都低于 th_stopped_velocity_mps，才真正调用 planTrajectory()。
停车轨迹往往会涉及前进和倒车切换。如果车辆还处在运动中，规划器突然给出一条方向、曲率、甚至分段边界都不同的新轨迹，控制器未必能优雅处理。停车场景又是低速场景，时间预算通常没那么紧，于是系统更偏向保守做法：先把状态收敛到“车已停住”，再开始下一次规划。
读这部分源码之前，我对 freespace planning 最大的误解，是以为算法层吐出什么，节点层就原样发什么。实际上完全不是这样。
算法层返回的是 PlannerWaypoints。每个 waypoint 带一个 is_back 标志，表示这一小段是前进还是倒车。节点层在 utils::create_trajectory() 里把它们转成 Trajectory 时，会统一赋一个常速 waypoints_velocity / 3.6，然后根据 is_back 决定速度是正还是负。
于是完整轨迹里自然会出现这样的情况：
这条轨迹在“搜索意义”上是完整的，但在“控制意义”上并不适合直接下发。于是 utils::get_reversing_indices() 会扫描整条轨迹，只要相邻两个点的速度符号发生变化，就把这个位置记下来，作为换向边界。
后面节点层真正发布时，只会取 prev_target_index_ 到 target_index_ 之间的那一段，也就是当前方向一致的这一截 partial_trajectory_。当车辆停住且靠近当前分段终点之后，updateTargetIndex() 才会把目标切到下一个换向点。
utils::get_partial_trajectory() 里还有两个小处理，很能说明设计意图：
首点这样处理，是为了避免切段边界上遗留错误的速度符号；尾点强制为 0，则是为了明确要求车辆在分段结束时停稳，给下一次切段或者下一次规划创造稳定的起点。
读到这里，再回头看节点里的 is_completed_、target_index_、reversing_indices_ 这些成员变量，整个结构就顺了。
autoware_freespace_planning_algorithms 里最值得先读的是 abstract_algorithm.cpp。
因为无论选 还是 ，真正把这套东西变成“能在车上跑”的，是这层公共底座。
setMap() 会把 occupancy grid 转成内部的障碍表。逻辑很简单：
cost < 0 算障碍cost >= obstacle_threshold 算障碍这意味着 unknown cell 在这里是不可走的。它不是“暂时未知，先试试”，而是“保守处理，直接封掉”。
对停车场景来说，这个选择是合理的。
节点层传进来的起点和终点，最终都会先通过 global2local() 转到 costmap 的局部坐标系。后面的离散化、碰撞检测、路径搜索，基本都发生在这个局部平面里。
这样做的好处很实际：你可以把地图、索引、车辆 footprint 这些逻辑都统一在一个平面世界里处理，不必在搜索过程中反复考虑全局坐标变换。
computeCollisionIndexes() 会针对每个离散朝向，把车辆矩形 footprint 采样成一组栅格索引，提前缓存下来。运行时如果要判断某个 (x, y, theta) 是否碰撞，只需要把这一组 footprint 索引平移到当前位置去查表。
更妙的是，它前面还加了一层 EDT 剪枝。
computeEDTMap() 会为整张地图算 Euclidean Distance Transform，得到每个栅格到最近障碍物的距离。真正进入 detectCollision() 时，先看这个位置离最近障碍有多远：
falsetrue这一段实现看起来不花哨，但非常有效。它把最贵的逐栅格检测留给了真正有必要的情况。
如果把它压成伪代码，大概就是这样：
if (detectBoundaryExit(base_index)) return true;
const auto obstacle_edt = getObstacleEDT(base_index).distance;
if (obstacle_edt > vehicle.max_dimension) return false;
if (obstacle_edt < vehicle.min_dimension) return true;
for (auto cell : precomputed_footprint(theta)) {
if (is_obstacle(cell + base_index.xy)) return true;
}
return false;
很多人看 freespace planning 时，注意力都放在 A* 的启发式或者 RRT* 的采样策略上。但如果没有这层足够快、足够稳定的碰撞底座，上层算法很快就会变成纸上谈兵。
节点初始化算法对象之前，会先按 vehicle_shape_margin_m 把车长、车宽和后悬做一轮扩张。若你选的是 rrtstar，构造函数里还会再叠一层 rrtstar.margin。
也就是说，$RRT* $实际使用的碰撞车模，往往会比 $ A* $更保守一点。
这也是调参时一个很容易忽略的点。有时候你以为是采样参数的问题，结果只是碰撞模型已经被放大得太保守了。
默认参数文件 config/freespace_planner.param.yaml 里，planning_algorithm 设的是 astar。所以大多数情况下，真正跑起来的是 AstarSearch。
Autoware 这里的 $A* $不是平面上的普通 ，而是典型的 ：搜索状态包含 x、y 和离散后的 theta。换句话说，它搜索的是三维状态格，而不是二维占据栅格。
它的扩展不是“朝八邻域走一步”，而是通过 kinematic_bicycle_model::getPose() 用运动学自行车模型推下一状态。给出当前位姿、轴距、转角和前进或倒车的距离，算法会按车模推演出下一个位姿。
这意味着，搜索图上的边从一开始就带着车辆非完整约束，而不是最后再拿一条栅格路径去做平滑。
expandNodes() 的结构如下
for (steering_index in [-N, ..., +N]) {
next_pose = bicycle_model(current_pose, steering, distance);
next_index = pose2index(next_pose);
if (out_of_range || occupied || detectCollision(next_index)) continue;
if (long_arc_has_intermediate_collision) continue;
move_cost = current.gc
+ length_cost
+ steering_change_cost
+ obstacle_cost
+ near_goal_lateral_cost
+ direction_switch_cost;
total_cost = move_cost + estimateCost(next_pose, next_index);
update_openlist_if_better(...);
}
很多文章会把 讲成“多了个朝向维度的 ”。从源码中我们可以知道，它远不止如此。这里的节点扩展、碰撞检查、代价设计和启发式都已经非常工程化。
AstarSearch::estimateCost() 用到两种启发式信息。
第一种是 col_free_distance_map_。setCollisionFreeDistanceMap() 会从目标位置出发，在二维自由栅格上做一轮类似 Dijkstra 的传播。它忽略朝向，只关心“在不穿障碍物的前提下，从这个位置到目标位置至少还要走多远”。这个量比单纯的欧氏距离更贴近障碍物结构。
第二种是 Reeds-Shepp distance。它考虑了车辆最小转弯半径、位置和朝向，也允许前进和倒车，因此是一个很自然的 car-like 终端距离下界。
代码没有在这两个启发式之间二选一，而是取它们的较大值，再乘 distance_heuristic_weight。也就是：
double total_cost = col_free_distance_map_[indexToId(index)];
total_cost = std::max(total_cost, calcReedsSheppDistance(pose, goal_pose_, avg_turning_radius_));
return distance_heuristic_weight * total_cost;
这个写法很聪明。障碍物形状复杂时，二维传播距离更有用；终端姿态要求更严格时，Reeds-Shepp 下界更有用。取两者较大值，相当于把启发式收紧了。
里的 g 代价不只是长度。你还能在代码里看到这些偏好：
这会让控制不会只追求几何最短，而会更偏向于一条控制起来舒服、离障碍物别太近、终点姿态也更顺的停车机动。
如果打开 adapt_expansion_distance，扩展距离不是固定值，而会综合考虑离目标还有多远、离障碍物有多近，在 min_expansion_dist_ 和 max_expansion_dist_ 之间自适应调整。
这带来的直观效果是：空旷地方步子可以大一点，搜索更快；接近障碍物或目标时步子收小一点，结果更细。
同时代码还会对较长弧段插中间检查点，防止一步跨过去直接漏掉碰撞。
isGoal() 用的是一个带容差的判定方式。只要当前位置落在目标的纵向、横向和朝向容差范围内，就可以认为已经到达。这样做比“必须完全重合”实用得多，也更符合停车问题的实际需求。
rrtstar.cpp 和 rrtstar_core.cpp 也很值得看，但它们在整个模块里的位置，我更愿意理解成“另一条设计路径”。
这套 不是教科书里纯二维几何版的$ RRT*$。它的状态空间同样包含 x、y 和 yaw，距离度量、插值和连边有效性检查都依赖 ReedsSheppStateSpace。换句话说，它在做的是 car-like configuration space 上的采样规划。
rrtstar_core::RRTStar::extend() 的主干也很干净：
x_rand = informed ? sample_in_ellipse() : sample_uniform();
x_nearest = findNearestNode(x_rand);
x_new = interpolate_child2parent(x_nearest->pose, x_rand, mu_);
if (!isValidPath(x_new, x_nearest)) return;
neighbors = findNeighborNodes(x_new);
best_parent = getBestParentNode(x_new, x_nearest, neighbors);
node_new = addNewNode(x_new, best_parent);
if (node_reconnect = getReconnectTargetNode(node_new, neighbors)) {
reconnect(node_new, node_reconnect);
}
if (isValidPath(goal, node_new)) {
reached_nodes_.push_back(node_new);
update_best_goal_parent();
}
整套流程没有什么隐藏机关，就是采样、延申、找更优父节点、重连、检查能否连到 goal。
有意思的是它做了几处很工程化的取舍。
第一，邻域半径没有按论文里那样随样本数收缩，而是固定使用一个尺度。源码注释里也写得很直白：因为这里用的是 Reeds-Shepp 距离，不再是简单欧氏空间，照搬论文里的收缩半径并不好设计。
第二，neighbor_radius 这个参数在实现里其实一人分饰两角：既影响从最近节点向随机样本推进时迈多大一步，也影响查邻居时的范围。它不只是“邻域大小”，也是树的增长节奏。
第三，如果已经找到一条可行解，并且开了 use_informed_sampling，采样区域会缩到一个椭圆内，也就是 informed RRT* 的经典做法。不过这里的椭圆是基于二维欧氏下界构造的，而不是直接对 Reeds-Shepp 代价做解析采样。这个取舍在 rrtstar.md 里也有解释：欧氏下界虽然没那么紧，但好采样，而且仍然是 admissible 的。
从结果上说，我能理解为什么默认配置还是选择 。停车场景里， 的结果更稳定、更规整，也更容易通过代价项把“贴障要少一点”“换向别太频繁”这些偏好写进去。 当然有它的价值，尤其是在更大范围、更开放的空间里，但在这份实现里，它更像一条备用路线。
第一，freespace_planner 不是单纯的 path solver，而是一个停车机动执行器。它知道什么时候该规划，什么时候该停，什么时候该切下一段。
第二，算法层的输出不是最终给控制器的输出。搜索层先给出一条完整机动路径，节点层再把它切成若干段单方向轨迹逐段发布。
第三，replanning 的默认策略是保守的。不是边开边丝滑换轨，而是先停住，再算下一条。
第四，真正让这套系统能跑起来的，不只是 $A* $或 的大框架，而是 AbstractPlanningAlgorithm 那层很扎实的地图处理、坐标转换、EDT 和碰撞检测。
第五，默认主力算法仍然是 ，而且这份实现已经不是“教科书级别的简化版”，而是一套把车模、启发式、惩罚项和执行需求揉在一起的工程版本。
我自己的阅读顺序是这样的，体验还不错：
先看 planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp，搞清楚节点层到底在调度什么；再看 utils.cpp，把 reversing index、partial trajectory、stop trajectory 这些细节吃透。接着去看 planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp，理解地图、碰撞和 EDT；最后再进入 astar_search.cpp 和 rrtstar_core.cpp。