lattice planner在LGSVL中的仿真实践

本项目主要基于深蓝学院规划与控制课程的项目代码，在此基础上做出了一些改进，此帖用于对项目代码做一些梳理。

一 项目代码组成

一. main函数

主要对PathPlanningNode进行初始化

#include <iostream>
#include "path_planning_node.h"
int main(int argc, char** argv) {
  ros::init(argc, argv, "path_planning");
  PathPlanningNode planning_node;
  ROS_INFO("Start Demo!");
  if (!planning_node.init()) {
    std::cout << "fail to init planning node" << std::endl;
    return -1;
  }
  ros::spin();
  return 0;
}

二. pathplanningNode初始化函数

bool PathPlanningNode::init() {
  std::string vehicle_odom_topic;
  std::string vehicle_cmd_topic;
  std::string roadmap_path;
  std::string path_vis_topic;
  std::string frame_id;
  std::string imu_topic;
  std::string local_traj_topic = "local_traj";
  double speed_P, speed_I, speed_D, target_speed, vis_frequency;
  pnh_.getParam("vehicle_odom_topic",
                vehicle_odom_topic);  //读取车辆定位的topic名
  pnh_.getParam("vehicle_cmd_topic",
                vehicle_cmd_topic);             //读取车辆控制的topic名
  pnh_.getParam("roadmap_path", roadmap_path);  //读取路网文件名
  pnh_.getParam("map_path", map_path_);
  pnh_.getParam("path_vis_topic", path_vis_topic);  //读取可视化路网名
  pnh_.getParam("target_speed", target_speed);      //读取目标速度
  pnh_.getParam("goal_tolerance", goalTolerance_);  //读取到终点的容忍距离
  pnh_.getParam("speed_P", speed_P);                //读取PID参数
  pnh_.getParam("speed_I", speed_I);
  pnh_.getParam("speed_D", speed_D);
  pnh_.getParam("control_frequency", controlFrequency_);  //读取控制的频率
  pnh_.getParam("vis_frequency", vis_frequency);  //读取路网显示的频率
  pnh_.getParam("frame_id", frame_id);            //读取全局坐标系名
  pnh_.getParam("c_speed", c_speed_);  //读取Frenet规划器，初始目标速度
  pnh_.getParam("c_d", c_d_);  //读取Frenet规划器，初始横向偏差
  pnh_.getParam("c_d_d", c_d_d_);  //读取Frenet规划器，初始横向速度偏差
  pnh_.getParam("c_d_dd", c_d_dd_);  //读取Frenet规划器，初始横向加速度偏差
  pnh_.getParam("s0", s0_);  //读取Frenet规划器，初始纵向距离
  pnh_.getParam("imu_topic", imu_topic);

  frame_id_ = frame_id;

  //加载路网文件
  if (!loadRoadmap(roadmap_path, target_speed)) return false;
  GetWayPoints();  //在参考路径的基础上进行踩点
  speedPidControllerPtr_ = std::shared_ptr<PIDController>(
      new PIDController(speed_P, speed_I, speed_D));
  lqrController_ = std::shared_ptr<LqrController>(new LqrController());
  lqrController_->LoadControlConf();
  lqrController_->Init();
  // 可视化
  visualization_ = VisualizationPtr(new Visualization(nh_));

  // 加载参考线
  if (use_reference_line_) {
    LoadReferenceLine();
  }

  // 构建相对平滑的Frenet曲线坐标系，一个中间暂时方案
  csp_obj_ = new Spline2D(wx_, wy_);

  // 全局路径可视化
  PlotGlobalPath();

  //  Update Obstacle
  // 添加虚拟障碍物
  UpdateStaticObstacle();

  roadmapMarkerPtr_ =
      std::shared_ptr<RosVizTools>(new RosVizTools(nh_, path_vis_topic));

  localTrajectoryMarkerPtr_ =
      std::shared_ptr<RosVizTools>(new RosVizTools(nh_, local_traj_topic));

  VehiclePoseSub_ = nh_.subscribe(vehicle_odom_topic, 10,
                                  &PathPlanningNode::odomCallback, this);
  ImuSub_ = nh_.subscribe(imu_topic, 10, &PathPlanningNode::IMUCallback, this);
  controlPub_ =
      nh_.advertise<lgsvl_msgs::VehicleControlData>(vehicle_cmd_topic, 1000);

  visTimer_ = nh_.createTimer(ros::Duration(1 / vis_frequency),
                              &PathPlanningNode::visTimerLoop,
                              this);  //注册可视化线程

  plannerTimer_ = nh_.createTimer(ros::Duration(1 / plannerFrequency_),
                                  &PathPlanningNode::plannerTimerLoop,
                                  this);  //注册规划线程

  controlTimer_ = nh_.createTimer(ros::Duration(1 / controlFrequency_),
                                  &PathPlanningNode::controlTimerLoop,
                                  this);  //注册控制线程

  addRoadmapMarker(planningPublishedTrajectory_.trajectory_points, frame_id);

  goalPoint_ =
      planningPublishedTrajectory_.trajectory_points.back();  //确定目标点

  ROS_INFO("planner node and lqr_control_node init finish!");
  return true;
}

2.1 loadRoadmap函数

bool PathPlanningNode::loadRoadmap(const std::string &roadmap_path,const double target_speed)

主要的作用是将参考线数据读入，并根据参考线中的x,y坐标计算出每个路径点的航向角,相对于路径起点的累计历程s，路径中每个离散点的曲率，曲率一阶导。

2.1.1 ComputePathProfile函数

bool ReferenceLine::ComputePathProfile(std::vector<double>* headings,std::vector<double>* accumulated_s,std::vector<double>* kappas,std::vector<double>* dkappas)

这个函数对曲率的计算可以参考下面链接里的公式：链接: link

2.2 GetWayPoints()函数

GetWayPoints()

这个函数主要从之前的参考线离散点中每隔10米挑出一个路点，不知道这个有啥用。