Auto Ware 代码解析系列-pure_pursuit节点_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);//订阅机器人当前速度
1
2
3
4

发布的话题如下：

 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())));
1
2
3
4
5
6
7
8
9

节点执行顺序如下：

一、实例化对象

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

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

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_);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

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

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

 ppn.run();
1

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
1
2
3
4
5
6
7
8
9
10