前言

在【一看就会】Autoware.universe的车道规划逻辑文章中，我们介绍了车道规划逻辑。
车道规划是指在两个路径点之间规划一条连续可行的车道。

也就是说，在我们给了起点和终点之后，会先生成路径点，将起点和终点分割成几段，再将这几段进行车道规划。

那路径点又是如何生成的呢？
本文便详细介绍了路径点的生成逻辑。

mission_planner通过两个回调函数开始整个流程，分别是on_set_lanelet_route和on_set_waypoint_route，我们先来讲解这两个函数。

一、on_set_lanelet_route：根据道路生成路线

处理基于 Lanelet 的路线设置请求。

使用场景：当你已经知道具体的车道(lanelet)序列时使用
输入参数包含 segments，这是一个预定义好的车道序列
直接使用给定的车道序列创建路线，不需要额外的路径规划
适用于当你需要车辆严格按照特定车道序列行驶的场景

一般不会调用这个函数，像是我们在仿真里面给了起点和终点，一般不会进这里面。

void MissionPlanner::on_set_lanelet_route(
  const SetLaneletRoute::Request::SharedPtr req, const SetLaneletRoute::Response::SharedPtr res)
{
  // 定义一个别名，方便引用响应消息中的错误代码
  using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response;

  // 判断当前是否处于重新规划路线的状态
  const auto is_reroute = state_.state == RouteState::SET;

  // 检查当前状态是否允许设置路线
  if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state.");
  }

  // 检查路径规划器是否准备好
  if (!planner_->ready()) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready.");
  }

  // 检查是否接收到车辆的里程信息
  if (!odometry_) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
  }

  // 如果是重新规划路线，检查是否接收到操作模式状态
  if (is_reroute && !operation_mode_state_) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received.");
  }

  // 判断车辆是否处于自动驾驶模式
  const bool is_autonomous_driving =
    operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS &&
                              operation_mode_state_->is_autoware_control_enabled
                          : false;

  // 如果是重新规划路线且车辆处于自动驾驶模式，检查重新规划路线的可用性
  if (is_reroute && is_autonomous_driving) {
    const auto reroute_availability = sub_reroute_availability_.takeData();
    if (!reroute_availability || !reroute_availability->availability) {
      throw service_utils::ServiceException(
        ResponseCode::ERROR_INVALID_STATE,
        "Cannot reroute as the planner is not in lane following.");
    }
  }

  // 根据当前是否是重新规划路线，切换状态机状态
  change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING);

  // 根据请求消息创建路线
  const auto route = create_route(*req);

  // 检查创建的路线是否为空
  if (route.segments.empty()) {
    cancel_route(); // 取消当前路线
    change_state(is_reroute ? RouteState::SET : RouteState::UNSET); // 切换状态
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
  }

  // 如果是重新规划路线且车辆处于自动驾驶模式，检查新路线是否安全
  if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) {
    cancel_route(); // 取消当前路线
    change_state(RouteState::SET); // 切换状态
    throw service_utils::ServiceException(
      ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed.");
  }

  // 设置新路线
  change_route(route);

  // 切换状态机状态为已设置路线
  change_state(RouteState::SET);

  // 设置响应消息的成功标志
  res->status.success = true;

  // 发布初始姿态和目标姿态的日志信息
  publish_pose_log(odometry_->pose.pose, "initial");
  publish_pose_log(req->goal_pose, "goal");
}

二、on_set_waypoint_route：根据路径点生成路线

用于处理设置航点路线的请求。

使用场景：当你只知道途经点(waypoints)时使用
输入参数包含 waypoints，这是一系列需要经过的位置点
会调用planner来规划一条通过所有途经点的完整路线
系统会自动规划如何在这些途经点之间选择合适的车道
更灵活，适用于只知道大致路线但具体车道选择可以由系统决定的场景

一般使用这个函数，我们点了起点和终点就进这个函数里面了。

这个里面会调用create_route，这个就和我们另外一篇文章Autoware.universe的车道规划逻辑衔接起来了。

调用create_route时，传入的参数是*req，这个是on_set_waypoint_route传入的参数，也就是路径点。

而void MissionPlanner::on_set_waypoint_route是作为一个服务的回调函数进行使用的。

void MissionPlanner::on_set_waypoint_route(
  const SetWaypointRoute::Request::SharedPtr req, const SetWaypointRoute::Response::SharedPtr res)
{
  // 定义一个别名，方便引用响应消息中的错误代码
  using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response;

  // 判断当前是否处于重新规划路线的状态
  const auto is_reroute = state_.state == RouteState::SET;

  // 检查当前状态是否允许设置路线
  if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state.");
  }

  // 检查路径规划器是否准备好
  if (!planner_->ready()) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready.");
  }

  // 检查是否接收到车辆的里程信息
  if (!odometry_) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
  }

  // 如果是重新规划路线，检查是否接收到操作模式状态
  if (is_reroute && !operation_mode_state_) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received.");
  }

  // 判断车辆是否处于自动驾驶模式
  const bool is_autonomous_driving =
    operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS &&
                              operation_mode_state_->is_autoware_control_enabled
                          : false;

  // 如果是重新规划路线且车辆处于自动驾驶模式，检查重新规划路线的可用性
  if (is_reroute && is_autonomous_driving) {
    const auto reroute_availability = sub_reroute_availability_.takeData();
    if (!reroute_availability || !reroute_availability->availability) {
      throw service_utils::ServiceException(
        ResponseCode::ERROR_INVALID_STATE,
        "Cannot reroute as the planner is not in lane following.");
    }
  }

  // 根据当前是否是重新规划路线，切换状态机状态
  change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING);

  // 根据请求消息创建路线
  const auto route = create_route(*req);

  // 检查创建的路线是否为空
  if (route.segments.empty()) {
    cancel_route(); // 取消当前路线
    change_state(is_reroute ? RouteState::SET : RouteState::UNSET); // 切换状态
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
  }

  // 如果是重新规划路线且车辆处于自动驾驶模式，检查新路线是否安全
  if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) {
    cancel_route(); // 取消当前路线
    change_state(RouteState::SET); // 切换状态
    throw service_utils::ServiceException(
      ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed.");
  }

  // 设置新路线
  change_route(route);

  // 切换状态机状态为已设置路线
  change_state(RouteState::SET);

  // 设置响应消息的成功标志
  res->status.success = true;

  // 发布初始姿态和目标姿态的日志信息
  publish_pose_log(odometry_->pose.pose, "initial");
  publish_pose_log(req->goal_pose, "goal");
}

三、调用on_set_waypoint_route的流程

为了搞清楚create_route里面的*req的生成过程，我们需要搞清楚on_set_waypoint_route的调用流程。

首先需要说明，on_set_waypoint_route是作为一个服务的回调函数进行使用的。
在mission_planner中： srv_set_waypoint_route = create_service( “~/set_waypoint_route”,service_utils::handle_exception(&MissionPlanner::on_set_waypoint_route, this));

它还有上层，具体的结构如下：
1.首先，有一个RouteSelector节点，它创建了两组服务：
// 主路由服务
main_.srv_set_waypoint_route = create_service(
“~/main/set_waypoint_route”,
service_utils::handle_exception(&RouteSelector::on_set_waypoint_route_main, this));

// MRM路由服务
mrm_.srv_set_waypoint_route = create_service(
“~/mrm/set_waypoint_route”,
service_utils::handle_exception(&RouteSelector::on_set_waypoint_route_mrm, this));

2.RouteSelector同时也是一个客户端，它会连接到实际的规划器服务：
cli_set_waypoint_route_ = create_client(“~/planner/set_waypoint_route”, service_qos, group_);

3.当收到路由请求时，处理流程如下：
void RouteSelector::on_set_waypoint_route_main(
SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res)
{
// 生成UUID（如果为空）
req->uuid = uuid::generate_if_empty(req->uuid);
// 保存请求
main_request_ = req;
main_.change_route();

// 如果在MRM模式，只改变状态
if (mrm_operating_) {
main_.change_state(RouteState::INTERRUPTED);
res->status.success = true;
return;
}

// 如果不在MRM模式，转发请求到实际的规划器
res->status = service_utils::sync_call(cli_set_waypoint_route_, req);
}

大家需要先理解ros2中的service机制。
总而言之，在运行过程中。
首先由其他节点设置路径点，然后再调用这个服务并等待相应，比如：
// 在其他节点中
auto client = node->create_client(“~/main/set_waypoint_route”);
auto request = std::make_sharedSetWaypointRoute::Request();
// 设置路径点
request->waypoints = …;
request->goal_pose = …;
// 调用服务并等待响应
auto future = client->async_send_request(request);

这个方式不是通过pub发布话题，再由sub接受话题并调用回调函数。
而是其他节点主动调用这个服务，传入路径点，从而进行后续操作，使用起来更像是一个远程函数的调用。

四、设置路径点

上面说了需要别的节点设置完路径点之后主动调用set_waypoint_route从而启动on_set_waypoint_route这个回调函数。

在进行on_set_waypoint_route处理时，会传入SetWaypointRoute::Request::SharedPtr req参数，而SetWaypointRoute::Request::SharedPtr的结构在SetWaypointRoute.srv中定义：
header (std_msgs/Header类型)
frame_id：坐标系的ID
stamp：时间戳
goal_pose (geometry_msgs/Pose类型)
最终目标位置和姿态
包含位置(position)和方向(orientation)
waypoints (geometry_msgs/Pose[]类型)
路径点数组
每个路径点都包含位置和姿态信息
这些是车辆需要经过的中间点
uuid (unique_identifier_msgs/UUID类型)
路径的唯一标识符
用于追踪和管理不同的路径请求
allow_modification (bool类型)
是否允许系统修改路径
如果为true，系统可以根据需要优化或调整路径

我使用过程中打印了一下，无论如何选择起点和终点，路径点都是0.

总结

整理了半天，发现实际上并没有用到这部分，之后再看看。