Autoware的MPC源码解析（一）mpc_waypoints_converter解析

版权声明：本文为博主原创文章，未经博主允许不得转载。
源码地址：https://gitlab.com/autowarefoundation/autoware.ai/autoware/wikis/Source-Build
目前在最新的Autoware源码中，包含两种路径跟踪(横向)控制方法：纯跟踪（pure pursuit）和模型预测控制（Model Predict Control，MPC）。本文主要对其中的MPC源码进行解析。
如有错误或建议，欢迎提出。

关于mpc方法的原理可参考此博文：https://blog.csdn.net/u013914471/article/details/83824490

在mpc源码中包含了两个节点，一个是/mpc_waypoints_converter，另一个是/mpc_follower。关系如下图所示。
此图来源于源码

在此篇中先解析mpc_waypoints_converter节点的源码。此节点主要是接收路径信息后，截取最近点前后的若干点生成局部路径发出去。将此节点订阅三个topic，分别是：

    sub_closest_waypoint_ = nh_.subscribe("/closest_waypoint", 1, &MPCWaypointsConverter::callbackClosestWaypoints, this);
    //接受最近路径点的序号
    sub_base_waypoints_ = nh_.subscribe("/base_waypoints", 1, &MPCWaypointsConverter::callbackBaseWaypoints, this);
    //接受原始全局路径信息
    sub_final_waypoints_ = nh_.subscribe("/final_waypoints", 1, &MPCWaypointsConverter::callbackFinalWaypoints, this);
    //接受最近路径点之后的路径点信息

发布一个topic：

pub_waypoints_ = nh_.advertise<autoware_msgs::Lane>("/mpc_waypoints", 1);
//发布局部路径信息（只包含最近路径点前后的若干个点）

接受路径信息后，先找出在/base_waypoints路径上的最近点：

// find closest point index in base_waypoints (topic /closest_waypoints has no consistency with /final_waypoints due to delay)
    int closest_idx = -1;
    for (int i = 0; i < (int)base_waypoints_.waypoints.size(); ++i) {
      const double d = sq_dist(final_waypoints.waypoints[1].pose.pose.position, base_waypoints_.waypoints[i].pose.pose.position);
      if (d < 0.01) {
        closest_idx = i;
        break;
      }
    }

将最近点之前的back_waypoints_num_个路径点（已经跑过的路径点）注入mpc_waypoints中：

int base_start = std::max(closest_idx - back_waypoints_num_, 0);
    for (int i = base_start; i < closest_idx; ++i)
    {
      mpc_waypoints.waypoints.push_back(base_waypoints_.waypoints.at(i));
      mpc_waypoints.waypoints.back().twist = final_waypoints.waypoints[1].twist;
    }

将最近点之后的front_waypoints_num_个路径点（未跑过的路径）注入mpc_waypoints中：

int final_end = std::min(front_waypoints_num_ + 1, (int)final_waypoints.waypoints.size());
    for (int i = 1; i < final_end; ++i)
    {
      mpc_waypoints.waypoints.push_back(final_waypoints.waypoints.at(i));
    }

发布局部路径信息：

pub_waypoints_.publish(mpc_waypoints);