Autoware(Pure pursuit代码学习)

Pure Pursuit学习（二）

1. 前言

pure pursuit只能用于一些简单的场景，如直线道路上的循迹；对于一些复杂的路径如U型/S型等曲线路径跟踪效果较差。

根据pure pursuit的原理可以知道，其跟踪效果很大程度上取决于前视距离$L_d$的选择，设置固定的前视距离和路径曲率肯定无法适应不同的路径，因此就需要对前视距离的计算方法进行研究改进。Pure Pursuit 纯追踪法理论推导、预瞄距离的确定见前篇文章：Pure Pursuit 纯追踪法

在Autoware中，绿色的球体即为计算的跟踪预瞄点；红色的点为规划好的路径点；白色的轨迹为轨迹跟踪算法计算出的车辆将要运行的轨迹。

2. Autoware算法框架

如图所示Autoware里Purepursuit的算法框架
见Cmake 4.1

总共有4个cpp文件：
pure_pursuit_core.cpp：Topic收发
pure_pursuit_node.cpp：main函数入口
pure_pursuit.cpp：规划路径点有效性的判定，曲率的计算
pure_pursuit_viz.cpp（暂且略过）：Rviz的显示

2.1 pure_pursuit_node.cpp

主函数，实例化对象并运行

// ROS Includes
#include <ros/ros.h>

// User defined includes
#include <pure_pursuit/pure_pursuit_core.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pure_pursuit");//节点初始化
  waypoint_follower::PurePursuitNode ppn;//实例化一个对象ppn
  ppn.run();//run()算法入口函数

  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

2.2 pure_pursuit_core.cpp

/*pure_pursuit_core.h*/
#ifndef PURE_PURSUIT_PURE_PURSUIT_CORE_H
#define PURE_PURSUIT_PURE_PURSUIT_CORE_H

// ROS includes
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <ros/ros.h>
#include <std_msgs/Float32.h>

// User defined includes
#include <autoware_config_msgs/ConfigWaypointFollower.h>
#include <autoware_msgs/ControlCommandStamped.h>
#include <autoware_msgs/Lane.h>
#include <autoware_health_checker/health_checker/health_checker.h>
#include <pure_pursuit/pure_pursuit.h>

// C++ includes
#include <vector>
#include <memory>
#include <string>

namespace waypoint_follower
{
enum class Mode : int32_t
{
  waypoint,//0
  dialog,//1
  unknown = -1,
};

template <class T> //这块模板相关的未看懂
typename std::underlying_type<T>::type enumToInteger(T t)
{
  return static_cast<typename std::underlying_type<T>::type>(t);
}
//框架定义，定义了一个PurePursuitNode类
class PurePursuitNode
{
public:
  PurePursuitNode(); //构造函数
  ~PurePursuitNode() = default; 

  void run();//算法的入口函数
  friend class PurePursuitNodeTestSuite; //友元类

private:
  // handle 句柄通过构造函数指定命名空间
  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;

  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;

  // class
  PurePursuit pp_;

  // publisher 发布者
  ros::Publisher pub1_, pub2_, pub11_, pub12_, pub13_, pub14_, pub15_, pub16_, pub17_, pub18_;

  // subscriber 订阅者
  ros::Subscriber sub1_, sub2_, sub3_, sub4_;

  // control loop update rate
  double update_rate_; //更新频率

  // variables
  bool is_linear_interpolation_, add_virtual_end_waypoints_;
  bool is_waypoint_set_, is_pose_set_, is_velocity_set_;
  double current_linear_velocity_, command_linear_velocity_;
  double wheel_base_;
  int expand_size_;
  LaneDirection direction_;
  int32_t velocity_source_;          // 0 = waypoint, 1 = Dialog
  double const_lookahead_distance_;  // meter 预瞄距离
  double const_velocity_;            // km/h 
  double lookahead_distance_ratio_;
  // the next waypoint must be outside of this threshold.
  double minimum_lookahead_distance_;
  std::string output_interface_;

  // callbacks
  //回调函数：订阅相关车辆配置参数，包含预瞄距离Ld，车辆速度v和最小预瞄距Ld_min
  void callbackFromConfig(const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config);
  //回调函数：获取当前车辆的位姿信息，包含坐标和四元数
  void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
  //回调函数：获取当前汽车的车速信息
  void callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg);
  //回调函数：订阅规划的路径点
  void callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg);

  // initializer
  void initForROS();

  // functions
  //发布控制命令
  void publishControlCommands(const bool& can_get_curvature, const double& kappa) const;
  //发布x轴线速度和z轴角速度
  void publishTwistStamped(const bool& can_get_curvature, const double& kappa) const;
  //发布车辆速度、加速度、转角信息
  void publishCtrlCmdStamped(const bool& can_get_curvature, const double& kappa) const;
  //发布偏离当前位置的信息
  void publishDeviationCurrentPosition(const geometry_msgs::Point& point,
                                       const std::vector<autoware_msgs::Waypoint>& waypoints) const;
  void connectVirtualLastWaypoints(autoware_msgs::Lane* expand_lane, LaneDirection direction);

  int getSgn() const;
  //计算预瞄距离
  double computeLookaheadDistance() const;
  double computeCommandVelocity() const;
  double computeCommandAccel() const;
  double computeAngularGravity(double velocity, double kappa) const;//计算角速度
};
//转换曲率为转角
double convertCurvatureToSteeringAngle(const double& wheel_base, const double& kappa);
//函数定义时，在返回类型前加上关键字 inline 即把函数指定为内联，函数申明时可加也可不加。但是建议函数申明的时候，也加上 inline，这样能够达到"代码即注释"的作用
inline double kmph2mps(double velocity_kmph) // km/h转换为m/s
{
  return (velocity_kmph * 1000) / (60 * 60);
}

}  // namespace waypoint_follower

#endif  // PURE_PURSUIT_PURE_PURSUIT_CORE_H
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123

/*pure_pursuit_core.cpp*/
#include <vector>
#include <string>

#include <pure_pursuit/pure_pursuit_core.h>
#include <pure_pursuit/pure_pursuit_viz.h>

namespace waypoint_follower
{
// Constructor 
PurePursuitNode::PurePursuitNode() //初始化列表
  : private_nh_("~")//句柄通过构造函数指定命名空间
  , pp_()
  , update_rate_(30.0)
  , is_waypoint_set_(false)
  , is_pose_set_(false)
  , is_velocity_set_(false)
  , current_linear_velocity_(0)
  , command_linear_velocity_(0)
  , direction_(LaneDirection::Forward)
  , velocity_source_(-1)
  , const_lookahead_distance_(4.0)
  , const_velocity_(5.0)
  , lookahead_distance_ratio_(2.0)
  , minimum_lookahead_distance_(6.0)
{
  initForROS();
  //在动态内存中分配一个对象并初始化
  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh_, private_nh_);
  health_checker_ptr_->ENABLE();
  // initialize for PurePursuit
  pp_.setLinearInterpolationParameter(is_linear_interpolation_);
}

void PurePursuitNode::initForROS()
{
  // ros parameter settings
  std::string out_twist, out_ctrl_cmd;
  //ros::NodeHandle::param() 如果get不到指定的param，它可以给param指定一个默认值
  private_nh_.param("velocity_source", velocity_source_, 0);
  private_nh_.param("is_linear_interpolation", is_linear_interpolation_, true);
  private_nh_.param("add_virtual_end_waypoints", add_virtual_end_waypoints_, false);
  private_nh_.param("const_lookahead_distance", const_lookahead_distance_, 4.0);
  private_nh_.param("const_velocity", const_velocity_, 5.0);
  private_nh_.param("lookahead_ratio", lookahead_distance_ratio_, 2.0);
  private_nh_.param("minimum_lookahead_distance", minimum_lookahead_distance_, 6.0);
  private_nh_.param("update_rate", update_rate_, 30.0);
  private_nh_.param("out_twist_name", out_twist, std::string("twist_raw"));
  private_nh_.param("out_ctrl_cmd_name", out_ctrl_cmd, std::string("ctrl_raw"));
  private_nh_.param("output_interface", output_interface_, std::string("ctrl_cmd"));
  nh_.param("vehicle_info/wheel_base", wheel_base_, 2.7);

  // Output type, use old parameter name only if it is set
  //hasParam(键) 是否包含某个键，存在返回 true，否则返回 false
  if (private_nh_.hasParam("publishes_for_steering_robot"))
  {
    bool publishes_for_steering_robot;
    private_nh_.param(
      "publishes_for_steering_robot", publishes_for_steering_robot, false);
    if (publishes_for_steering_robot)
    {
      output_interface_ = "ctrl_cmd";
    }
    else
    {
      output_interface_ = "twist";
    }
  }
  else
  {
    private_nh_.param(
      "output_interface", output_interface_, std::string("all"));
  }

  if (output_interface_ != "twist" && output_interface_ != "ctrl_cmd" &&
      output_interface_ != "all")
  {
    ROS_ERROR("Control command interface type is not valid");//控制命令接口不合理
    ros::shutdown();
  }

  // setup subscriber
  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);

  // setup publishers
  pub1_ = nh_.advertise<geometry_msgs::TwistStamped>(out_twist, 10);
  pub2_ = nh_.advertise<autoware_msgs::ControlCommandStamped>(out_ctrl_cmd, 10);
  pub11_ = nh_.advertise<visualization_msgs::Marker>("next_waypoint_mark", 0);
  pub12_ = nh_.advertise<visualization_msgs::Marker>("next_target_mark", 0);
  pub13_ = nh_.advertise<visualization_msgs::Marker>("search_circle_mark", 0);
  // debug tool
  pub14_ = nh_.advertise<visualization_msgs::Marker>("line_point_mark", 0);
  pub15_ = nh_.advertise<visualization_msgs::Marker>("trajectory_circle_mark", 0);
  pub16_ = nh_.advertise<std_msgs::Float32>("angular_gravity", 0);
  pub17_ = nh_.advertise<std_msgs::Float32>("deviation_of_current_position", 0);
  pub18_ = nh_.advertise<visualization_msgs::Marker>("expanded_waypoints_mark", 0);
}

void PurePursuitNode::run()
{
  //ros::Rate loop_rate( );    写在循环外部
  //loop_rate.sleep();   写在循环内部
  ros::Rate loop_rate(update_rate_);
  while (ros::ok())
  {
    ros::spinOnce();    //调用回调函数
    if (!is_pose_set_ || !is_waypoint_set_ || !is_velocity_set_)
    {
      if (!is_pose_set_)
      {
        ROS_WARN_THROTTLE(5, "Waiting for current_pose topic ...");
      }
      if (!is_waypoint_set_)
      {
        ROS_WARN_THROTTLE(5, "Waiting for final_waypoints topic ...");
      }
      if (!is_velocity_set_)
      {
        ROS_WARN_THROTTLE(5, "Waiting for current_velocity topic ...");
      }

      loop_rate.sleep();
      continue;
    }

    pp_.setLookaheadDistance(computeLookaheadDistance());
    pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_);

    double kappa = 0;
    bool can_get_curvature = pp_.canGetCurvature(&kappa);

    publishControlCommands(can_get_curvature, kappa);
    health_checker_ptr_->NODE_ACTIVATE();
    health_checker_ptr_->CHECK_RATE("topic_rate_vehicle_cmd_slow", 8, 5, 1, "topic vehicle_cmd publish rate slow.");
    // 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())));
    if (add_virtual_end_waypoints_)
    {
      pub18_.publish(displayExpandWaypoints(pp_.getCurrentWaypoints(), expand_size_));
    }
    std_msgs::Float32 angular_gravity_msg;
    angular_gravity_msg.data = computeAngularGravity(computeCommandVelocity(), kappa);
    pub16_.publish(angular_gravity_msg);

    publishDeviationCurrentPosition(pp_.getCurrentPose().position, pp_.getCurrentWaypoints());

    is_pose_set_ = false;
    is_velocity_set_ = false;

    loop_rate.sleep();
  }
}

void PurePursuitNode::publishControlCommands(const bool& can_get_curvature, const double& kappa) const
{
  if (output_interface_ == "twist")
  {
    publishTwistStamped(can_get_curvature, kappa);
  }
  else if (output_interface_ == "ctrl_cmd")
  {
    publishCtrlCmdStamped(can_get_curvature, kappa);
  }
  else if (output_interface_ == "all")
  {
    publishTwistStamped(can_get_curvature, kappa);
    publishCtrlCmdStamped(can_get_curvature, kappa);
  }
  else
  {
    ROS_WARN("[pure_pursuit] control command interface is not appropriate");
  }
}
//发布x轴线速度和z轴角速度
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;
  pub1_.publish(ts);
}
//发布车辆速度、加速度以及转角
void PurePursuitNode::publishCtrlCmdStamped(const bool& can_get_curvature, const double& kappa) const
{
  autoware_msgs::ControlCommandStamped ccs;
  ccs.header.stamp = ros::Time::now();
  ccs.cmd.linear_velocity = can_get_curvature ? computeCommandVelocity() : 0;
  ccs.cmd.linear_acceleration = can_get_curvature ? computeCommandAccel() : 0;
  ccs.cmd.steering_angle = can_get_curvature ? convertCurvatureToSteeringAngle(wheel_base_, kappa) : 0;
  pub2_.publish(ccs);
}

double PurePursuitNode::computeLookaheadDistance() const
{
  if (velocity_source_ == enumToInteger(Mode::dialog))
  {
    return const_lookahead_distance_;
  }

  const double maximum_lookahead_distance = current_linear_velocity_ * 10;//最大预瞄距离 = 当前速度×10
  const double ld = current_linear_velocity_ * lookahead_distance_ratio_;//预瞄距离 = 当前速度×K值(可配置，可初始化)
//如果ld小于最小预瞄距离，ld=最小预瞄距离；ld大于最小预瞄距离，ld=最大预瞄距离=>设置预瞄距离约束
  return ld < minimum_lookahead_distance_ ? minimum_lookahead_distance_ :
                                            ld > maximum_lookahead_distance ? maximum_lookahead_distance : ld;
}

int PurePursuitNode::getSgn() const
{
  int sgn = 0;
  if (direction_ == LaneDirection::Forward)
  {
    sgn = 1;
  }
  else if (direction_ == LaneDirection::Backward)
  {
    sgn = -1;
  }
  return sgn;
}
//计算速度控制的大小
double PurePursuitNode::computeCommandVelocity() const
{
  if (velocity_source_ == enumToInteger(Mode::dialog))
  {
    return getSgn() * kmph2mps(const_velocity_);
  }

  return command_linear_velocity_;
}

// Assume constant acceleration motion, v_f^2 - v_i^2 = 2 * a * delta_d
//计算加速度
double PurePursuitNode::computeCommandAccel() const
{
  const geometry_msgs::Pose current_pose = pp_.getCurrentPose();
  const geometry_msgs::Pose target_pose = pp_.getCurrentWaypoints().at(1).pose.pose;

  const double delta_d =
      std::hypot(target_pose.position.x - current_pose.position.x, target_pose.position.y - current_pose.position.y);
  const double v_i = current_linear_velocity_;
  const double v_f = computeCommandVelocity();
  return (v_f * v_f - v_i * v_i) / (2 * delta_d);
}
//计算角加速度
double PurePursuitNode::computeAngularGravity(double velocity, double kappa) const
{
  const double gravity = 9.80665;
  return (velocity * velocity) / (1.0 / kappa * gravity);
}
//回调函数：订阅相关车辆配置参数，包括预瞄距离/车辆速度/最小预瞄距离
void PurePursuitNode::callbackFromConfig(const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config)
{
  velocity_source_ = config->param_flag;
  const_lookahead_distance_ = config->lookahead_distance;//预瞄距离
  const_velocity_ = config->velocity;//车辆速度
  lookahead_distance_ratio_ = config->lookahead_ratio;//预瞄距离比
  minimum_lookahead_distance_ = config->minimum_lookahead_distance;//最小预瞄距离
}

void PurePursuitNode::publishDeviationCurrentPosition(const geometry_msgs::Point& point,
                                                      const std::vector<autoware_msgs::Waypoint>& waypoints) const
{
  // Calculate the deviation of current position from the waypoint approximate line
  if (waypoints.size() < 3)
  {
    return;
  }

  const geometry_msgs::Point end = waypoints.at(2).pose.pose.position;
  const geometry_msgs::Point start = waypoints.at(1).pose.pose.position;

  const tf::Vector3 p_A(start.x, start.y, 0.0);
  const tf::Vector3 p_B(end.x, end.y, 0.0);
  const tf::Vector3 p_C(point.x, point.y, 0.0);

  // The distance form a point C to a line passing through A and B is given by
  // length(AB.crossProduct(AC))/length(AC)
  const tf::Vector3 AB = p_B - p_A;
  const tf::Vector3 AC = p_C - p_A;
  const float distance = (AB.cross(AC)).length() / AC.length();

  std_msgs::Float32 msg;
  msg.data = distance;
  pub17_.publish(msg);
}
//获取当前汽车的位资信息，包含坐标和四元数
void PurePursuitNode::callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
{
  pp_.setCurrentPose(msg);
  is_pose_set_ = true;
}
//回调函数：获取当前汽车的车速信息
void PurePursuitNode::callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg)
{
  current_linear_velocity_ = msg->twist.linear.x;
  pp_.setCurrentVelocity(current_linear_velocity_);
  is_velocity_set_ = true;
}
//订阅规划的路径点
void PurePursuitNode::callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg)
{
  command_linear_velocity_ = (!msg->waypoints.empty()) ? msg->waypoints.at(0).twist.twist.linear.x : 0;
  if (add_virtual_end_waypoints_)
  {
    const LaneDirection solved_dir = getLaneDirection(*msg);
    direction_ = (solved_dir != LaneDirection::Error) ? solved_dir : direction_;
    autoware_msgs::Lane expanded_lane(*msg);
    expand_size_ = -expanded_lane.waypoints.size();
    connectVirtualLastWaypoints(&expanded_lane, direction_);
    expand_size_ += expanded_lane.waypoints.size();

    pp_.setCurrentWaypoints(expanded_lane.waypoints);
  }
  else
  {
    pp_.setCurrentWaypoints(msg->waypoints);
  }
  is_waypoint_set_ = true;
}

void PurePursuitNode::connectVirtualLastWaypoints(autoware_msgs::Lane* lane, LaneDirection direction)
{
  if (lane->waypoints.empty())
  {
    return;
  }
  static double interval = 1.0;
  const geometry_msgs::Pose& pn = lane->waypoints.back().pose.pose;
  autoware_msgs::Waypoint virtual_last_waypoint;
  virtual_last_waypoint.pose.pose.orientation = pn.orientation;
  virtual_last_waypoint.twist.twist.linear.x = 0.0;
  geometry_msgs::Point virtual_last_point_rlt;
  const int sgn = getSgn();
  for (double dist = minimum_lookahead_distance_; dist > 0.0; dist -= interval)
  {
    virtual_last_point_rlt.x += interval * sgn;
    virtual_last_waypoint.pose.pose.position = calcAbsoluteCoordinate(virtual_last_point_rlt, pn);
    lane->waypoints.emplace_back(virtual_last_waypoint);
  }
}
//转换曲率为转角delta = actan(kappa*L)
double convertCurvatureToSteeringAngle(const double& wheel_base, const double& kappa)
{
  return atan(wheel_base * kappa);
}

}  // namespace waypoint_follower

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356

2.3 pure_pursuit.cpp

/*pure_pursuit.h*/
#ifndef PURE_PURSUIT_PURE_PURSUIT_H
#define PURE_PURSUIT_PURE_PURSUIT_H

// ROS includes
#include <ros/ros.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>

// C++ includes
#include <vector>

// User defined includes
#include <autoware_msgs/Lane.h>
#include <libwaypoint_follower/libwaypoint_follower.h>

namespace waypoint_follower
{
class PurePursuit
{
public:
  PurePursuit() = default;
  // 关键字 =default 标记编译器隐式生成该类的默认构造函数，
  // 代码更简洁，且隐式生成的版本执行效率更高
  ~PurePursuit() = default;

  // for setting data
  void setLookaheadDistance(const double& ld)
  {
    lookahead_distance_ = ld; //预瞄距离
  }
  void setMinimumLookaheadDistance(const double& minld)
  {
    minimum_lookahead_distance_ = minld; //最小预瞄距离
  }
  void setCurrentVelocity(const double& cur_vel)
  {
    current_linear_velocity_ = cur_vel; //当前速度
  }
  void setCurrentWaypoints(const std::vector<autoware_msgs::Waypoint>& wps)
  {
    current_waypoints_ = wps;//当前规划点 
  }
  void setCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
  {
    current_pose_ = msg->pose;//朝向
  }
  void setLinearInterpolationParameter(const bool& param)
  {
    is_linear_interpolation_ = param;//是否线性插值
  }

  // for debug on ROS
  geometry_msgs::Point getPoseOfNextWaypoint() const
  {
    return current_waypoints_.at(next_waypoint_number_).pose.pose.position;
  }
  geometry_msgs::Point getPoseOfNextTarget() const
  {
    return next_target_position_;
  }
  geometry_msgs::Pose getCurrentPose() const
  {
    return current_pose_;
  }
  std::vector<autoware_msgs::Waypoint> getCurrentWaypoints() const
  {
    return current_waypoints_;
  }
  double getLookaheadDistance() const
  {
    return lookahead_distance_;
  }
  double getMinimumLookaheadDistance() const
  {
    return minimum_lookahead_distance_;
  }
  // processing
  bool canGetCurvature(double* output_kappa);//获取曲率的

private:
  // constant
  static constexpr double RADIUS_MAX_ = 9e10; //转弯半径
  static constexpr double KAPPA_MIN_ = 1.0 / 9e10; //最小曲率

  // variables
  bool is_linear_interpolation_{ false };
  int next_waypoint_number_{ -1 };
  double lookahead_distance_{ 0.0 };
  double minimum_lookahead_distance_{ 6.0 };
  double current_linear_velocity_{ 0.0 };
  geometry_msgs::Pose current_pose_{};
  geometry_msgs::Point next_target_position_{};
  std::vector<autoware_msgs::Waypoint> current_waypoints_{};

  // functions
  //用于计算下一路径点与汽车当前位置之间的圆弧曲率
  double calcCurvature(const geometry_msgs::Point& target) const;
  bool interpolateNextTarget(int next_waypoint, geometry_msgs::Point* next_target) const;
  void getNextWaypoint();//主要是用于获取有效的目标规划路径点
};
}  // namespace waypoint_follower

#endif  // PURE_PURSUIT_PURE_PURSUIT_H
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105

/*pure_pursuit.cpp*/
#include <pure_pursuit/pure_pursuit.h>
#include <cmath>

namespace waypoint_follower
{
// Simple estimation of curvature given two points.
// 1. Convert the target point from map frame into the current pose frame,
//    so it has a local coorinates of (pt.x, pt.y, pt.z).
// 2. If we think it is a cirle with a curvature kappa passing the two points,
//    kappa = 2 * pt.y / (pt.x * pt.x + pt.y * pt.y). For detailed derivation, please
//    refer to "Integrated Mobile Robot Control" by Omead Amidi
//    (CMU-RI-TR-90-17, Equation 3.10 on Page 21)
//计算下一路径点与汽车当前位置之间的圆弧曲率 这里利用的相对坐标和圆里面的直角三角形的相似来求得圆的半径，最后得到圆弧曲率
double PurePursuit::calcCurvature(const geometry_msgs::Point& target) const
{
  double kappa;
  const geometry_msgs::Point pt = calcRelativeCoordinate(target, current_pose_);
  const double denominator = pt.x * pt.x + pt.y * pt.y;
  const double numerator = 2.0 * pt.y;

  if (denominator != 0.0)
  {
    kappa = numerator / denominator;
  }
  else
  {
    kappa = numerator > 0.0 ? KAPPA_MIN_ : -KAPPA_MIN_;
  }

  return kappa;
}

// linear interpolation of next target
bool PurePursuit::interpolateNextTarget(int next_waypoint, geometry_msgs::Point* next_target) const
{
  const int path_size = static_cast<int>(current_waypoints_.size());
  if (next_waypoint == path_size - 1)
  {
    *next_target = current_waypoints_.back().pose.pose.position;
    return true;
  }
  const double search_radius = lookahead_distance_;
  const geometry_msgs::Point end = current_waypoints_.at(next_waypoint).pose.pose.position;
  const geometry_msgs::Point start = current_waypoints_.at(next_waypoint - 1).pose.pose.position;

  // project ego vehicle's current position at C onto the line at D in between two waypoints A and B.
  const tf::Vector3 p_A(start.x, start.y, 0.0);
  const tf::Vector3 p_B(end.x, end.y, 0.0);
  const tf::Vector3 p_C(current_pose_.position.x, current_pose_.position.y, 0.0);
  const tf::Vector3 AB = p_B - p_A;
  const tf::Vector3 AC = p_C - p_A;
  const tf::Vector3 p_D = p_A + AC.dot(AB) / AB.dot(AB) * AB;
  const double dist_CD = (p_D - p_C).length();

  bool found = false;
  tf::Vector3 final_goal;
  // Draw a circle centered at p_C with a radius of search_radius
  if (dist_CD > search_radius)
  {
    // no intersection in between the circle and AB
    found = false;
  }
  else if (dist_CD == search_radius)
  {
    // one intersection
    final_goal = p_D;
    found = true;
  }
  else
  {
    // two intersections
    // get intersection in front of vehicle
    double s = sqrt(pow(search_radius, 2) - pow(dist_CD, 2));
    tf::Vector3 p_E = p_D + s * AB.normalized();
    tf::Vector3 p_F = p_D - s * AB.normalized();

    // verify whether these two points lie on line segment AB
    if ((p_B - p_E).length2() < AB.length2())
    {
      final_goal = p_E;
      found = true;
    }
    else if ((p_B - p_F).length2() < AB.length2())
    {
      final_goal = p_F;
      found = true;
    }
    else
    {
      found = false;
    }
  }

  if (found)
  {
    next_target->x = final_goal.x();
    next_target->y = final_goal.y();
    next_target->z = current_pose_.position.z;
  }

  return found;
}

void PurePursuit::getNextWaypoint()//主要是用于获取有效的目标规划路径点
{
  //获取规划路径点的个数
  const int path_size = static_cast<int>(current_waypoints_.size());

  // 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;
}

bool PurePursuit::canGetCurvature(double* output_kappa)//用来获取曲率的
{
  // search next waypoint
  //判断规划路径点的有效性，赋值->next_waypoint_number_
  getNextWaypoint();
  if (next_waypoint_number_ == -1)
  {
    ROS_INFO("lost next waypoint");
    return false;
  }
  // check whether curvature is valid or not
  //判断曲率是否有效
  bool is_valid_curve = false;
  for (const auto& el : current_waypoints_)
  {
  	//只有汽车的当前位置点与当前规划路径点之间的距离大于最小预瞄距离点时，曲率才算有效
    if (getPlaneDistance(el.pose.pose.position, current_pose_.position) > minimum_lookahead_distance_)
    {
      is_valid_curve = true;
      break;
    }
  }
  if (!is_valid_curve)
  {
    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
  const bool interpolation = interpolateNextTarget(next_waypoint_number_, &next_target_position_);

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

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

}  // namespace waypoint_follower
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192

2.4 详解求曲率kappa

//计算下一路径点与汽车当前位置之间的圆弧曲率 这里利用的相对坐标和圆里面的直角三角形的相似来求得圆的半径，最后得到圆弧曲率
double PurePursuit::calcCurvature(const geometry_msgs::Point& target) const
{
  double kappa;
  const geometry_msgs::Point pt = calcRelativeCoordinate(target, current_pose_);
  const double denominator = pt.x * pt.x + pt.y * pt.y;
  const double numerator = 2.0 * pt.y;

  if (denominator != 0.0)
  {
    kappa = numerator / denominator;  //kappa = 2y/L^2
  }
  else
  {
    kappa = numerator > 0.0 ? KAPPA_MIN_ : -KAPPA_MIN_;
  }

  return kappa;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

将全局坐标下的目标点转换成车辆坐标下

$XOY$: 世界坐标系
$X_1{O}_1{Y}_1$ :车辆坐标系/ROS坐标系
$X_1$轴是车辆的运动方向,$Y_1$轴在车辆左方
$AB=L,AC=y,BC=x,B{O}_2=R$

$x^2+y^2=L^2$
$x^2+(R-y{)}^2=R^2$ => $x^2+y^2=2Ry$

$R=\frac{L^2}{2y}$ => $kappa=\frac{1}{R}=\frac{2y}{L^2}$

$X_1{O}_1{Y}_1$以当前车辆为原点，通过函数计算出目标位置点相对于车辆当前位置的坐标P点。

本文作为自己Autoware中pure pursuit代码学习记录。