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<int>(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<int>(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<int>(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"<<get_linear_flag);//  ROS_INFO_STREAM("get_linear_fla"<<get_linear_fla);  if (!get_linear_flag)    return false;  // 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);  if (d > 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)**