（九）学习笔记autoware源码core_planning（lane_select)

1.lane_select_node

就一个主函数。

#include <ros/ros.h>

#include "lane_select_core.h"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "lane_select");
  lane_planner::LaneSelectNode lsn;
  lsn.run();

  return 0;
}

2.hermite_curve

#include "hermite_curve.h"

namespace lane_planner
{

  /*
  crcateVectorFromPosc函数实际上得到的是世界坐标系下位置为(1,0,0)的点经过跟
  当前车体坐标系(位姿p 所对应的车体坐标系)一样的x,y,z轴的旋转变换后在世界坐标系中新的位置。
  current_pose所对应的坐标系是沿着z轴旋转90度得到的，因此向量(1,0,0)同样绕z轴旋转
  90度，其终点在世界坐标系下的新坐标为(0,1,0)，符合createVectorFromPose函数输出的结果。
  */
void createVectorFromPose(const geometry_msgs::Pose &p, tf::Vector3 *v)
{
  tf::Transform pose;
  tf::poseMsgToTF(p, pose);
  tf::Vector3 x_axis(1, 0, 0);

  //getBasis 函数返回旋转的基础矩阵乘以x_axis后得到矩阵的第一列
  *v = pose.getBasis() * x_axis;
}


/*
getPointAndVectorFromPose函数赋值传入函数的point和 vector。其中，point为位姿pose在
世界坐标系下的位置坐标，vector为位姿pose对应的车体坐标系的x轴正方向在世界坐标系下的方位(用二维单位向量表示)。
*/
void getPointAndVectorFromPose(const geometry_msgs::Pose &pose, Element2D *point, Element2D *vector)
{
  point->set(pose.position.x, pose.position.y);

  tf::Vector3 tmp_tf_vevtor;
  createVectorFromPose(pose, &tmp_tf_vevtor);
  vector->set(tmp_tf_vevtor.getX(), tmp_tf_vevtor.getY());
}


//generateHermiteCurveForROS函数确定从start 到end的具体行车轨迹。
std::vector<autoware_msgs::Waypoint> generateHermiteCurveForROS(const geometry_msgs::Pose &start,
                                                                const geometry_msgs::Pose &end,
                                                                const double velocity_mps, const double vlength)
{
  std::vector<autoware_msgs::Waypoint> wps;
  Element2D p0(0, 0), v0(0, 0), p1(0, 0), v1(0, 0);
  getPointAndVectorFromPose(start, &p0, &v0);
  getPointAndVectorFromPose(end, &p1, &v1);


//已知曲线的两个端点坐标p0、p1和端点处的切线v0、v1,确定Hermite(埃尔米特)曲线，获得采样点序列形式的曲线表达结果
  std::vector<Element2D> result = generateHermiteCurve(p0, v0, p1, v1, vlength);

  double height_d = fabs(start.position.z - end.position.z);
  for (uint32_t i = 0; i < result.size(); i++)
  {
    autoware_msgs::Waypoint wp;
    wp.pose.pose.position.x = result.at(i).x;
    wp.pose.pose.position.y = result.at(i).y;
    wp.twist.twist.linear.x = velocity_mps;

    // height
    wp.pose.pose.position.z =
        (i == 0) ? start.position.z : (i == result.size() - 1) ? end.position.z : start.position.z < end.position.z ?
                                                                 start.position.z + height_d * i / result.size() :
                                                                 start.position.z - height_d * i / result.size();

    // orientation
    if (i != result.size() - 1)
    {
      double radian = atan2(result.at(i + 1).y - result.at(i).y, result.at(i + 1).x - result.at(i).x);
      wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(radian);
    }
    else
    {
      wp.pose.pose.orientation = wps.at(wps.size() - 1).pose.pose.orientation;
    }

    wps.push_back(wp);
  }
  return wps;
}

//generateHermiteCurve函数根据曲线端点坐标和切线返回对应Hermite曲线上的点序列。
std::vector<Element2D> generateHermiteCurve(const Element2D &p0, const Element2D &v0, const Element2D &p1,
                                            const Element2D &v1, const double vlength)
{
  std::vector<Element2D> result;
  const double interval = 1.0;
  int32_t divide = 2;
  const int32_t loop = 100;
  while (divide < loop)
  {
    result.reserve(divide);
    for (int32_t i = 0; i < divide; i++)
    {
      double u = i * 1.0 / (divide - 1);
      double u_square = pow(u, 2);
      double u_cube = pow(u, 3);
      double coeff_p0 = 2 * u_cube - 3 * u_square + 1;
      double coeff_v0 = u_cube - 2 * u_square + u;
      double coeff_p1 = (-1) * 2 * u_cube + 3 * u_square;
      double coeff_v1 = u_cube - u_square;
      // printf("u: %lf, u^2: %lf, u^3: %lf, coeff_p0: %lf, coeff_v0: %lf, coeff_p1: %lf, coeff_v1: %lf\n", u, u_square,
      // u_cube, coeff_p0, coeff_p1, coeff_v0, coeff_v1);
      result.push_back(
          Element2D((p0.x * coeff_p0 + vlength * v0.x * coeff_v0 + p1.x * coeff_p1 + vlength * v1.x * coeff_v1),
                    (p0.y * coeff_p0 + vlength * v0.y * coeff_v0 + p1.y * coeff_p1 + vlength * v1.y * coeff_v1)));
    }

    double dt = sqrt(pow((result.at(divide / 2 - 1).x - result.at(divide / 2).x), 2) +
                     pow((result.at(divide / 2 - 1).y - result.at(divide / 2).y), 2));
    std::cout << "interval : " << dt << std::endl;
    if (interval > dt || divide == loop - 1)
      return result;
    else
    {
      result.clear();
      result.shrink_to_fit();
      divide++;
    }
  }
  return result;
}
}  // namespace

3.lane_select_core

#include "lane_select_core.h"

#include <algorithm>

namespace lane_planner
{
// Constructor

/*
节点lane_select启动时新建LaneSelectNode对象，查看LaneSelectNode 唯一的构造函数LaneSelectNode()。
LaneSelectNode函数中首先对一些成员变量进行赋值,接着调用initForROS函数。
*/
LaneSelectNode::LaneSelectNode()
  : private_nh_("~")
  , lane_array_id_(-1)
  , current_lane_idx_(-1)
  , right_lane_idx_(-1)
  , left_lane_idx_(-1)
  , is_lane_array_subscribed_(false)
  , is_current_pose_subscribed_(false)
  , is_current_velocity_subscribed_(false)
  , is_current_state_subscribed_(false)
  , is_config_subscribed_(false)
  , distance_threshold_(3.0)
  , lane_change_interval_(10.0)
  , lane_change_target_ratio_(2.0)
  , lane_change_target_minimum_(5.0)
  , vlength_hermite_curve_(10)
  , search_closest_waypoint_minimum_dt_(5)
  , current_state_("UNKNOWN")
{
  initForROS();
}

// Destructor
LaneSelectNode::~LaneSelectNode()
{
}


//initForROS函数的内容跟前面分析的一些ROS节点的main 函数中的内容类似:设置订阅者/发布者，设置参数。
void LaneSelectNode::initForROS()
{
  // setup subscriber  设置订阅
  sub1_ = nh_.subscribe("traffic_waypoints_array", 1, &LaneSelectNode::callbackFromLaneArray, this);
  sub2_ = nh_.subscribe("current_pose", 1, &LaneSelectNode::callbackFromPoseStamped, this);
  sub3_ = nh_.subscribe("current_velocity", 1, &LaneSelectNode::callbackFromTwistStamped, this);
  sub4_ = nh_.subscribe("state", 1, &LaneSelectNode::callbackFromState, this);
  sub5_ = nh_.subscribe("/config/lane_select", 1, &LaneSelectNode::callbackFromConfig, this);
  sub6_ = nh_.subscribe("/decision_maker/state", 1, &LaneSelectNode::callbackFromDecisionMakerState, this);

  // setup publisher  设置发布
  pub1_ = nh_.advertise<autoware_msgs::Lane>("base_waypoints", 1);
  pub2_ = nh_.advertise<std_msgs::Int32>("closest_waypoint", 1);
  pub3_ = nh_.advertise<std_msgs::Int32>("change_flag", 1);
  pub4_ = nh_.advertise<std_msgs::Int32>("/current_lane_id", 1);
  pub5_ = nh_.advertise<autoware_msgs::VehicleLocation>("vehicle_location", 1);

  vis_pub1_ = nh_.advertise<visualization_msgs::MarkerArray>("lane_select_marker", 1);

  // get from rosparam  设置参数
  private_nh_.param<double>("lane_change_interval", lane_change_interval_, double(2));
  private_nh_.param<double>("distance_threshold", distance_threshold_, double(3.0));
  private_nh_.param<int>("search_closest_waypoint_minimum_dt", search_closest_waypoint_minimum_dt_, int(5));
  private_nh_.param<double>("lane_change_target_ratio", lane_change_target_ratio_, double(2.0));
  private_nh_.param<double>("lane_change_target_minimum", lane_change_target_minimum_, double(5.0));
  private_nh_.param<double>("vector_length_hermite_curve", vlength_hermite_curve_, double(10.0));
}

//isAl1TopicsSubscribed 函数判断节点lane_select是否订阅了所有所需的话题。
bool LaneSelectNode::isAllTopicsSubscribed()
{
  //查看各个标志确定是否此节点所需的所有话题都被订阅
  if (!is_current_pose_subscribed_ || !is_lane_array_subscribed_ || !is_current_velocity_subscribed_)
  {
    RO