Auto Ware 代码解析系列-pure_pursuit节点

节点pure_pursuit主要对人车进行轨迹追踪，该节点订阅话题如下：

 sub1_ = nh_.subscribe("final_waypoints", 10, &PurePursuitNode::callbackFromWayPoints, this);//订阅路径规划出来的路径点
 sub2_ = nh_.subscribe("current_pose", 10, &PurePursuitNode::callbackFromCurrentPose, this);//订阅机器人发布的姿态
 sub3_ = nh_.subscribe("config/waypoint_follower", 10, &PurePursuitNode::callbackFromConfig, this);//订阅机器人参数数据
 sub4_ = nh_.subscribe("current_velocity", 10, &PurePursuitNode::callbackFromCurrentVelocity, this);//订阅机器人当前速度

发布的话题如下：

 publishTwistStamped(can_get_curvature, kappa);//发布机器人运动信息
 publishControlCommandStamped(can_get_curvature, kappa);//发布机器人控制量信息

////for visualization with Rviz
 pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint()));
 pub13_.publish(displaySearchRadius(pp_.getCurrentPose().position, pp_.getLookaheadDistance()));
 pub12_.publish(displayNextTarget(pp_.getPoseOfNextTarget()));
 pub15_.publish(displayTrajectoryCircle(
 waypoint_follower::generateTrajectoryCircle(pp_.getPoseOfNextTarget(), pp_.getCurrentPose())));

节点执行顺序如下：

一、实例化对象

waypoint_follower::PurePursuitNode ppn;//实例化对象

进入构造函数，初始化对象的成员变量：

PurePursuitNode::PurePursuitNode()
  : private_nh_("~")
  , pp_()
  , LOOP_RATE_(30)
  , is_waypoint_set_(false)
  , is_pose_set_(false)
  , is_velocity_set_(false)
  , is_config_set_(false)
  , current_linear_velocity_(0)
  , command_linear_velocity_(0)
  , param_flag_(-1)
  , const_lookahead_distance_(4.0)
  , const_velocity_(5.0)
  , lookahead_distance_ratio_(2.0)
  , minimum_lookahead_distance_(2.6)
{
  initForROS();

  // initialize for PurePursuit，是否进行线性插值
  pp_.setLinearInterpolationParameter(is_linear_interpolation_);
}

initForROS()初始化ROS中一些参数，包括获取参数，设置订阅和发布的函数

二、进入成员函数，循环执行pure pursuit

 ppn.run();

ppn代码如下：

void PurePursuitNode::run()
{
  ROS_INFO_STREAM("pure pursuit start");
  ros::Rate loop_rate(LOOP_RATE_);
  while (ros::ok())
  {
    ros::spinOnce();
   // if (!is_pose_set_ || !is_waypoint_set_ || !is_velocity_set_ || !is_config_set_)
   ///当机器人的速度和位置路径点订阅函数都接收到数据后，开始pure_pursuit算法
    if (!is_pose_set_ || !is_waypoint_set_ || !is_velocity_set_ )
    {
    //  ROS_WARN("Necessary topics are not subscribed yet ... ");
      loop_rate.sleep();
      continue;
    }
   /// ROS_WARN("Begin Begin BeginBegin Begin Begin Begin... ");
    pp_.setLookaheadDistance(computeLookaheadDistance());//计算无人车的预瞄距离，该参数主要和无人车当前的车速，转向角度相关

    double kappa = 0;
    bool can_get_curvature = pp_.canGetCurvature(&kappa);//计算无人车的曲率，计算曲率的
    publishTwistStamped(can_get_curvature, kappa);
    publishControlCommandStamped(can_get_curvature, kappa);

    // for visualization with Rviz
    pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint()));
    pub13_.publish(displaySearchRadius(pp_.getCurrentPose().position, pp_.getLookaheadDistance()));
    pub12_.publish(displayNextTarget(pp_.getPoseOfNextTarget()));
    pub15_.publish(displayTrajectoryCircle(
    waypoint_follower::generateTrajectoryCircle(pp_.getPoseOfNextTarget(), pp_.getCurrentPose())));

    is_pose_set_ = false;
    is_velocity_set_ = false;
    is_waypoint_set_ = false;
    loop_rate.sleep();
  }

1、setLookaheadDistance函数中computeLookaheadDistance，lookahead_distance_ 为预瞄距离：

double PurePursuitNode::computeLookaheadDistance() const
{
  if (param_flag_ == enumToInteger(Mode::dialog))///对话框模式下恒定输入lookahead_distance_
    return const_lookahead_distance_;

  double maximum_lookahead_distance = current_linear_velocity_ * 10;//最大预瞄距离为当前速度的10倍
  double ld = current_linear_velocity_ * lookahead_distance_ratio_;//设置预瞄距离，线性比率为2
///根据预瞄距离的范围，选择合适的值，程序中最小预瞄距离，我设置为无人的最小转弯半径，最大预瞄距离设置为当前速度的10倍
  return ld < minimum_lookahead_distance_ ? minimum_lookahead_distance_
        : ld > maximum_lookahead_distance ? maximum_lookahead_distance
        : ld;
}

2、bool can_get_curvature = pp_.canGetCurvature(&kappa)函数,计算曲率

bool PurePursuit::canGetCurvature(double *output_kappa)
{
  // search next waypoint
  getNextWaypoint();//获取将要走的目标点，只有当目标点与机器人当前位置之间的距离大于预瞄距离才有效（最后一个waypoint除外），返回next_waypoint_number_
  if (next_waypoint_number_ == -1)
  {
    ROS_INFO("lost next waypoint");
    return false;
  }

  /// if is_linear_interpolation_ is false or next waypoint is first or last
  if (!is_linear_interpolation_ || next_waypoint_number_ == 0 ||
      next_waypoint_number_ == (static_cast(current_waypoints_.size() - 1)))
  {
    next_target_position_ = current_waypoints_.at(next_waypoint_number_).pose.pose.position;
    *output_kappa = calcCurvature(next_target_position_);
    return true;
  }

  // /linear interpolation and calculate angular velocity
  bool interpolation = interpolateNextTarget(next_waypoint_number_, &next_target_position_);

  if (!interpolation)
  {
    ROS_INFO_STREAM("lost target! ");
    return false;
  }

  /// ROS_INFO("next_target : ( %lf , %lf , %lf)", next_target.x, next_target.y,next_target.z);

  *output_kappa = calcCurvature(next_target_position_);
  return true;
}

函数中getNextWaypoint为得到purepursuit的路径点：

void PurePursuit::getNextWaypoint()
{
  int path_size = static_cast(current_waypoints_.size());///得到vector的大小

  /// if waypoints are not given, do nothing.如果没有点，不做任何事
  if (path_size == 0)
  {
    next_waypoint_number_ = -1;
    return;
  }

  /// look for the next waypoint.
  for (int i = 0; i < path_size; i++)
  {
    // if search waypoint is the last，如果是最后一个点，不再查询下一个点，认为该点有效
    if (i == (path_size - 1))
    {
      ROS_INFO("search waypoint is the last");
      next_waypoint_number_ = i;
      return;
    }

    /// if there exists an effective waypoint，只有当前路径点大于预瞄距离时该点才有效
    if (getPlaneDistance(current_waypoints_.at(i).pose.pose.position, current_pose_.position) > lookahead_distance_)
    {
      next_waypoint_number_ = i;
      return;
    }
  }
  /// if this program reaches here , it means we lost the waypoint!
  next_waypoint_number_ = -1;
  return;
}

计算机器人的曲率半径，计算下一个路径点和机器人当前位置规划圆弧的曲率，参考公式R=L^2/(2*x)， 曲率为1/R

double PurePursuit::calcCurvature(geometry_msgs::Point target) const
{
  double kappa;
  double denominator = pow(getPlaneDistance(targeZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZon), 2);
  double numerator = 2 * calcRelativeCoordinate(target, current_pose_).y;

  if (denominator != 0)
    kappa = numerator / denominator;
  else
  {
    if (numerator > 0)
      kappa = KAPPA_MIN_;///RADIUS_MAX_(9e10) , KAPPA_MIN_=1/RADIUS_MAX_
    else
      kappa = -KAPPA_MIN_;
  }
  ROS_INFO("kappa : %lf", kappa);
  return kappa;
}

根据is_linear_interpolation_判断是否插值：

bool PurePursuit::interpolateNextTarget(int next_waypoint, geometry_msgs::Point *next_target) const
{
  constexpr double ERROR = pow(10, -5);  // 0.00001



  int path_size = static_cast(current_waypoints_.size());
  if (next_waypoint == path_size - 1)
  {
    *next_target = current_waypoints_.at(next_waypoint).pose.pose.position;
    return true;
  }
  double search_radius = lookahead_distance_;
  geometry_msgs::Point zero_p;
  geometry_msgs::Point end = current_waypoints_.at(next_waypoint).pose.pose.position;
  geometry_msgs::Point start = current_waypoints_.at(next_waypoint - 1).pose.pose.position;

  // let the linear equation be "ax + by + c = 0"
  // if there are two points (x1,y1) , (x2,y2), a = "y2-y1, b = "(-1) * x2 - x1" ,c = "(-1) * (y2-y1)x1 + (x2-x1)y1"
  double a = 0;
  double b = 0;
  double c = 0;
  double get_linear_flag = getLinearEquation(start, end, &a, &b, &c);
   ROS_INFO_STREAM("get_linear_flag"</  ROS_INFO_STREAM("get_linear_fla"</ let the center of circle be "(x0,y0)", in my code , the center of circle is vehicle position
  // the distance  "d" between the foot of a perpendicular line and the center of circle is ...
  //    | a * x0 + b * y0 + c |
  // d = -------------------------------
  //          √( a~2 + b~2)
  double d = getDistanceBetweenLineAndPoint(current_pose_.position, a, b, c);

  // ROS_INFO("a : %lf ", a);
  // ROS_INFO("b : %lf ", b);
  // ROS_INFO("c : %lf ", c);
  ROS_INFO("distance : %lf ", d);

   ROS_INFO_STREAM("search_radius"< search_radius)//该处主要判断机器人规划的路径是否在机器人的预瞄距离内，如果不在，则报错

    return false;

  // unit vector of point 'start' to point 'end'
  tf::Vector3 v((end.x - start.x), (end.y - start.y), 0);
  tf::Vector3 unit_v = v.normalize();

  // normal unit vectors of v
  tf::Vector3 unit_w1 = rotateUnitVector(unit_v, 90);   // rotate to counter clockwise 90 degree
  tf::Vector3 unit_w2 = rotateUnitVector(unit_v, -90);  // rotate to counter clockwise 90 degree

  // the foot of a perpendicular line
  geometry_msgs::Point h1;
  h1.x = current_pose_.position.x + d * unit_w1.getX();
  h1.y = current_pose_.position.y + d * unit_w1.getY();
  h1.z = current_pose_.position.z;

  geometry_msgs::Point h2;
  h2.x = current_pose_.position.x + d * unit_w2.getX();
  h2.y = current_pose_.position.y + d * unit_w2.getY();
  h2.z = current_pose_.position.z;

  // ROS_INFO("error : %lf", error);
  // ROS_INFO("whether h1 on line : %lf", h1.y - (slope * h1.x + intercept));
  // ROS_INFO("whether h2 on line : %lf", h2.y - (slope * h2.x + intercept));

  // check which of two foot of a perpendicular line is on the line equation
  geometry_msgs::Point h;
  if (fabs(a * h1.x + b * h1.y + c) < ERROR)
  {
    h = h1;
    //   ROS_INFO("use h1");
  }
  else if (fabs(a * h2.x + b * h2.y + c) < ERROR)
  {
    //   ROS_INFO("use h2");
    h = h2;
  }
  else
  {
    return false;
  }

  // get intersection[s]
  // if there is a intersection
  if (d == search_radius)
  {
    *next_target = h;
    return true;
  }
  else
  {
    // if there are two intersection
    // get intersection in front of vehicle
    double s = sqrt(pow(search_radius, 2) - pow(d, 2));
    geometry_msgs::Point target1;
    target1.x = h.x + s * unit_v.getX();
    target1.y = h.y + s * unit_v.getY();
    target1.z = current_pose_.position.z;

    geometry_msgs::Point target2;
    target2.x = h.x - s * unit_v.getX();
    target2.y = h.y - s * unit_v.getY();
    target2.z = current_pose_.position.z;

    // ROS_INFO("target1 : ( %lf , %lf , %lf)", target1.x, target1.y, target1.z);
    // ROS_INFO("target2 : ( %lf , %lf , %lf)", target2.x, target2.y, target2.z);
    // displayLinePoint(a, b, c, target1, target2, h);  // debug tool

    // check intersection is between end and start
    double interval = getPlaneDistance(end, start);
    if (getPlaneDistance(target1, end) < interval)
    {
      // ROS_INFO("result : target1");
      *next_target = target1;
      return true;
    }
    else if (getPlaneDistance(target2, end) < interval)
    {
      // ROS_INFO("result : target2");
      *next_target = target2;
      return true;
    }
    else
    {
      // ROS_INFO("result : false ");
      return false;
    }
  }
}

3、 发布机器人的运动速度方程publishTwistStamped(can_get_curvature, kappa);

    void PurePursuitNode::publishTwistStamped(const bool &can_get_curvature, const double &kappa) const
{
  geometry_msgs::TwistStamped ts;
  ts.header.stamp = ros::Time::now();
  ts.twist.linear.x = can_get_curvature ? computeCommandVelocity() : 0;//计算线速度
  ts.twist.angular.z = can_get_curvature ? kappa * ts.twist.linear.x : 0;//计算角度度参考公式w=V/R
  pub1_.publish(ts);
}

函数computeCommandVelocity如下：

double PurePursuitNode::computeCommandVelocity() const
{
  if (param_flag_ == enumToInteger(Mode::dialog))
    return kmph2mps(const_velocity_);

  return command_linear_velocity_;
}

其中 command_linear_velocity_为waypoint路径点中指定的速度，通过回调函数获得

void PurePursuitNode::callbackFromWayPoints(const waypoint_follower::laneConstPtr &msg)
{
  if (!msg->waypoints.empty())
    command_linear_velocity_ = msg->waypoints.at(0).twist.twist.linear.x;
  else
    command_linear_velocity_ = 0;

  pp_.setCurrentWaypoints(msg->waypoints);

//  ROS_INFO_STREAM(" is_waypoint_set_ is_waypoint_set_");
  is_waypoint_set_ = true;
}

4、 发布机器人的控制命令，速度，和转向角度publishControlCommandStamped(can_get_curvature, kappa);

void PurePursuitNode::publishControlCommandStamped(const bool &can_get_curvature, const double &kappa) const
{
  if (!publishes_for_steering_robot_)
    return;

  waypoint_follower::ControlCommandStamped ccs;
  ccs.header.stamp = ros::Time::now();
  ccs.cmd.linear_velocity = can_get_curvature ? computeCommandVelocity() : 0;
  ccs.cmd.steering_angle = can_get_curvature ? convertCurvatureToSteeringAngle(wheel_base_, kappa) : 0;//计算机器人的转角，wheel_base为车的轴距

  pub2_.publish(ccs);
}

**其中函数convertCurvatureToSteeringAngle为

return atan(wheel_base * kappa);//参考公式theta=arctan(H/R)**