Autoware planning模塊學習筆記（二）：路徑規劃（2）- 節點waypoint_replanner（上）
前面一篇博客Autoware planning模塊學習筆記（二）：路徑規劃（1），介紹了“勾選waypoint_loader”這一操作時所啓動的waypoint_loader節點，還剩下waypoint_replanner和waypoint_marker_publisher兩個節點。這裏幫大夥兒逐行分析節點waypoint_replanner的代碼。
節點waypoint_replanner
節點waypoint_replanner的源文件是nodes/waypoint_replanner/waypoint_replanner.cpp和nodes/waypoint_replanner/waypoint_replanner_node.cpp
老規矩，先找到main函數。main函數在waypoint_replanner_node.cpp中，main函數中就做了一件事，新建waypoint_maker::WaypointReplannerNode對象wr，很容易就能猜測到waypoint_maker::WaypointReplannerNode的構造函數內執行了一些操作。
int main(int argc, char** argv)
{
ros::init(argc, argv, "waypoint_replanner");
waypoint_maker::WaypointReplannerNode wr;
ros::spin();
return 0;
}
接着我們查看waypoint_maker::WaypointReplannerNode的構造函數，可以發現replanning_mode_的初始值爲false，構造函數內實現了topic的訂閱和發佈。其中訂閱的“/based/lane_waypoints_raw”就是前面waypoint_loader節點所發佈的消息。
WaypointReplannerNode::WaypointReplannerNode() : replanning_mode_(false)
{
lane_pub_["with_decision"] = nh_.advertise<autoware_msgs::LaneArray>("/based/lane_waypoints_array", 10, true);
lane_pub_["without_decision"] = nh_.advertise<autoware_msgs::LaneArray>("/lane_waypoints_array", 10, true);
lane_sub_ = nh_.subscribe("/based/lane_waypoints_raw", 1, &WaypointReplannerNode::laneCallback, this);
config_sub_ = nh_.subscribe("/config/waypoint_replanner", 1, &WaypointReplannerNode::configCallback, this);
}
緊接着分析構造函數內的兩個回調函數laneCallback和configCallback，首先看回調函數configCallback，要注意的是subscribe的回調函數是接收到對應消息後纔會被調用，因此在代碼中的前後設定順序跟實際執行順序不是一樣的，實際執行順序是按照各自消息到達順序確定，因此可以認爲是隨機的。我們先分析configCallback函數是因爲有些參數需要通過它進行設置，如果參數未設置完畢，laneCallback函數被激發執行了呢？所以實際啓動系統過程中（如下圖），是在操作交互界面上設置相關參數完成後再勾選相關選項，再等一會全部初始化完成後切換至自動駕駛模式。
對應於topic "/config/waypoint_replanner"的回調函數configCallback，首先設置成員變量replanning_mode_和realtime_tuning_mode_，接着對replanner_進行初始化，replanner_的定義是“WaypointReplanner replanner_;”。接着（防盜標記：zhengkunxian）根據lane_array_.lanes（lane_array_定義爲autoware_msgs::LaneArray lane_array_;）是否爲空（lane_array_在另一個回調函數laneCallback中會被賦值）和realtime_tuning_mode_變量決定是否執行publishLaneArray函數。
void WaypointReplannerNode::configCallback(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf)
{
replanning_mode_ = conf->replanning_mode;
realtime_tuning_mode_ = conf->realtime_tuning_mode;
replanner_.initParameter(conf);
if (lane_array_.lanes.empty() || !realtime_tuning_mode_)
{
return;
}
publishLaneArray();
}
replanner_的參數初始化：
void WaypointReplanner::initParameter(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf)
{
velocity_max_ = kmph2mps(conf->velocity_max);
velocity_min_ = kmph2mps(conf->velocity_min);
accel_limit_ = conf->accel_limit;
decel_limit_ = conf->decel_limit;
r_th_ = conf->radius_thresh;
r_min_ = conf->radius_min;
lookup_crv_width_ = 5;
resample_mode_ = conf->resample_mode;
resample_interval_ = conf->resample_interval;
replan_curve_mode_ = conf->replan_curve_mode;
replan_endpoint_mode_ = conf->replan_endpoint_mode;
overwrite_vmax_mode_ = conf->overwrite_vmax_mode;
velocity_offset_ = conf->velocity_offset;
end_point_offset_ = conf->end_point_offset;
braking_distance_ = conf->braking_distance;
r_inf_ = 10 * r_th_;
vel_param_ = calcVelParam(velocity_max_);
}
publishLaneArray函數內如果replanning_mode_爲true，則調用replan函數重規劃。
void WaypointReplannerNode::publishLaneArray()
{
autoware_msgs::LaneArray array(lane_array_);
if (replanning_mode_)
{
replan(array);
}
lane_pub_["with_decision"].publish(array);
if (withoutDecisionMaker())
{
lane_pub_["without_decision"].publish(array);
}
}
replan函數內遍歷lane_array.lanes，這裏回顧一下lane_array.lanes內的元素都是autoware_msgs::Lane類型，其中儲存了從multi_lane_csv_文件內讀取的路徑點集合（std::vector<autoware_msgs::Waypoint>），每個路徑點記錄了UWB座標系下的座標x,y,z以及航向角等必要信息供規劃所用。對每一條lane，replanner_都調用replanLaneWaypointVel函數。
void WaypointReplannerNode::replan(autoware_msgs::LaneArray& lane_array)
{
for (auto &el : lane_array.lanes)
{
replanner_.replanLaneWaypointVel(el);
}
}
replanLaneWaypointVel函數：
void WaypointReplanner::replanLaneWaypointVel(autoware_msgs::Lane& lane)
{
if (lane.waypoints.empty())
{
return;
}
const int dir = getDirection(lane);
const unsigned long last = lane.waypoints.size() - 1;
changeVelSign(lane, true);
limitVelocityByRange(0, last, 0, velocity_max_, lane);
if (resample_mode_)
{
resampleLaneWaypoint(resample_interval_, lane, dir);
}
if (replan_curve_mode_)
{
std::vector<double> curve_radius;
KeyVal curve_list;
createRadiusList(lane, curve_radius);
createCurveList(curve_radius, curve_list);
if (overwrite_vmax_mode_)
{// set velocity_max for all_point
setVelocityByRange(0, last, 0, velocity_max_, lane);
}
// set velocity by curve（防盜標記：zhengkunxian）
for (const auto& el : curve_list)
{
const double& radius = el.second.second;
double vmin = velocity_max_ - vel_param_ * (r_th_ - radius);
vmin = (vmin < velocity_min_) ? velocity_min_ : vmin;
limitVelocityByRange(el.first, el.second.first, velocity_offset_, vmin, lane);
}
}
// set velocity on start & end of lane
if (replan_endpoint_mode_)
{
const unsigned long zerospeed_start = last - end_point_offset_;
const unsigned long lowspeed_start = zerospeed_start - braking_distance_;
raiseVelocityByRange(0, last, 0, velocity_min_, lane);
limitVelocityByRange(0, 0, 0, velocity_min_, lane);
limitVelocityByRange(lowspeed_start, last, 0, velocity_min_, lane);
setVelocityByRange(zerospeed_start, last, 0, 0.0, lane);
}
if (dir < 0)
{
changeVelSign(lane, false);
}
}
getDirection函數：
calcRelativeCoordinate函數（防盜標記：zhengkunxian）位於src/autoware/common/waypoint_follower/lib/libwaypoint_follower.cpp，libwaypoint_follower.cpp被src/autoware/core_planning/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.h所引用。
int WaypointReplanner::getDirection(const autoware_msgs::Lane& lane) const
{
if (lane.waypoints.size() < 2)
{
return 1;
}
const geometry_msgs::Pose& pose = lane.waypoints[0].pose.pose;
const geometry_msgs::Point& point = lane.waypoints[1].pose.pose.position;
const geometry_msgs::Point rlt_point(calcRelativeCoordinate(point, pose));
return rlt_point.x < 0 ? -1 : 1;
}
calcRelativeCoordinate函數：
作用：計算全局座標系中某點相對於機器人座標系中的位置,即傳入函數的point（全局座標系下的座標）相對於pose座標系的位置。具體含義可見下面示意圖，calcRelativeCoordinate函數返回的便是P點在車體座標系O’x’y’中的座標（x’, y’）。
實現：下面是Autoware的代碼實現，調用了TF的一些函數進行處理。但是具體的內部機理，涉及到TF庫的一些原理，座標系的轉換，四元數之類的，超出了博主的能力範圍，也許以後有精力會去研究一下TF的一些原理，這裏就直接用Autoware這個函數就好了。
// calculation relative coordinate of point from current_pose frame
geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
{
tf::Transform inverse;
tf::poseMsgToTF(current_pose, inverse);
tf::Transform transform = inverse.inverse();
tf::Point p;
pointMsgToTF(point_msg, p);
tf::Point tf_p = transform * p;
geometry_msgs::Point tf_point_msg;
pointTFToMsg(tf_p, tf_point_msg);
return tf_point_msg;
}
作用驗證：我們設計一個小實驗，驗證一下calcRelativeCoordinate函數的作用，其中current_pose所對應的座標系（也就是這個剛體對應的位置與方向，其中方向是用四元數表示的），其與世界座標系之間的x,y,z軸方向是一致的，（防盜標記：zhengkunxian）只是其原點位於世界座標系(4,7,0)處，很容易的我們知道point_msg(1,2,0)在current_pose所對應的座標系中的座標爲(-3,-5,0)，而代碼運行結果與我們的計算相符，因此得證代碼的作用。
再回到getDirection函數，其根據返回的P點在車體座標系O’x’y’中的座標的x座標，如果其小於0則返回-1，否則返回1。實際上getDirection函數內只分析了傳入的lane中記錄的lane.waypoints[0]所在座標系和lane.waypoints[1]座標點之間的關係。
再回到調用getDirection函數的replanLaneWaypointVel函數，接着分析changeVelSign函數的作用：顧名思義，改變路徑點的速度符號。
void WaypointReplanner::changeVelSign(autoware_msgs::Lane& lane, bool positive) const
{
const int sgn = positive ? 1 : -1;
for (auto& el : lane.waypoints)
{
el.twist.twist.linear.x = sgn * fabs(el.twist.twist.linear.x);
}
}
接着replanLaneWaypointVel函數繼續執行，調用limitVelocityByRange函數：
limitVelocityByRange(0, last, 0, velocity_max_, lane);
其中last：const unsigned long last = lane.waypoints.size() - 1;
velocity_max_：velocity_max_ = kmph2mps(conf->velocity_max);
首先判斷lane中儲存的路徑點是否爲空，若不爲空則接着判斷offset（偏移）是否大於0，如果大於0則進一步對start_idx和end_idx進行處理。這裏replanLaneWaypointVel函數調用時因爲傳入的offset=0，因此跳過。接着爲了防止下標越界對end_idx進行檢查。檢查完畢後對lane中所有路徑點的速度進行最大值的限制。
void WaypointReplanner::limitVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
double vmin, autoware_msgs::Lane& lane)
{
if (lane.waypoints.empty())
{
return;
}
if (offset > 0)
{
start_idx = (start_idx > offset) ? (start_idx - offset) : 0;
end_idx = (end_idx > offset) ? (end_idx - offset) : 0;
}
end_idx = (end_idx >= lane.waypoints.size()) ? lane.waypoints.size() - 1 : end_idx;
for (unsigned long idx = start_idx; idx <= end_idx; idx++)
{
if (lane.waypoints[idx].twist.twist.linear.x < vmin)
{
continue;
}
lane.waypoints[idx].twist.twist.linear.x = vmin;
}
limitAccelDecel(start_idx, lane);
limitAccelDecel(end_idx, lane);
}
limitAccelDecel函數：
根據下式（高一物理哦～其實博主高中時候物理超強的，但是現在基本忘光了）對lane中所存儲的下一路徑點的速度進行修正
其中 是最大加速度， 是距離（根據前後兩個路徑點的位置算得）。由此可以計算出在最大加速度的限制條件下，下一路徑點的速度最大值/最小值，據此進行修正。
void WaypointReplanner::limitAccelDecel(const unsigned long idx, autoware_msgs::Lane& lane)
{
const double acc[2] = { accel_limit_, decel_limit_ };
const unsigned long end_idx[2] = { lane.waypoints.size() - idx, idx + 1 };
const int sgn[2] = { 1, -1 };
for (int j = 0; j < 2; j++) // [j=0]: accel_limit_process, [j=1]: decel_limit_process
{
double v = lane.waypoints[idx].twist.twist.linear.x;//v保持不變
unsigned long next = idx + sgn[j];//[j=0]: next = idx + 1, [j=1]: next = idx - 1
for (unsigned long i = 1; i < end_idx[j]; i++, next += sgn[j])//[j=0]: end_idx[j] = lane.waypoints.size() - idx, [j=1]: end_idx[j] = idx + 1
{
const geometry_msgs::Point& p0 = lane.waypoints[next - sgn[j]].pose.pose.position;
const geometry_msgs::Point& p1 = lane.waypoints[next].pose.pose.position;
const double dist = std::hypot(p0.x - p1.x, p0.y - p1.y);//計算距離
v = sqrt(2 * acc[j] * dist + v * v);
if (v > velocity_max_ || v > lane.waypoints[next].twist.twist.linear.x)
{
break;
}
lane.waypoints[next].twist.twist.linear.x = v;
}
}
}
再次回到replanLaneWaypointVel函數（博主把下一步代碼貼出來，後面也這樣，爲大家節省往回翻的精力）：
根據設置的resample_mode_決定是否執行resampleLaneWaypoint函數。我們不管在Autoware交互界面是怎麼設置的，所有的函數都會被分析講解。
if (resample_mode_)
{
resampleLaneWaypoint(resample_interval_, lane, dir);
}
resampleLaneWaypoint函數：
顧名思義，這個函數的作用是重新採樣路徑點。
void WaypointReplanner::resampleLaneWaypoint(const double resample_interval, autoware_msgs::Lane& lane, int dir)
{
if (lane.waypoints.size() < 2)
{
return;
}
autoware_msgs::Lane original_lane(lane);
lane.waypoints.clear();
lane.waypoints.emplace_back(original_lane.waypoints[0]);
lane.waypoints.reserve(ceil(1.5 * calcPathLength(original_lane) / resample_interval_));
//reserve函數嘗試爲向量保留足夠的內存以容納指定數量的元素。
//ceil(x)函數返回的是大於x的最小整數，例如ceil(10.5) == 11，ceil(-10.5) ==-10
//calcPathLength函數計算original_lane中路徑點之間總的距離，也就是整條軌跡的長度
for (unsigned long i = 1; i < original_lane.waypoints.size(); i++)
{
CbufGPoint curve_point = getCrvPointsOnResample(lane, original_lane, i);//採三個軌跡點
const std::vector<double> curve_param = calcCurveParam(curve_point);//根據三個軌跡點求軌跡曲線的圓心和半徑
lane.waypoints.back().twist.twist = original_lane.waypoints[i - 1].twist.twist;
lane.waypoints.back().wpstate = original_lane.waypoints[i - 1].wpstate;
lane.waypoints.back().change_flag = original_lane.waypoints[i - 1].change_flag;
// if going straight
if (curve_param.empty())
{
resampleOnStraight(curve_point, lane);//如果三點確定的軌跡是直線
}
// else if turnning curve
else
{
resampleOnCurve(curve_point[1], curve_param, lane, dir);//如果三點確定的軌跡是曲線
}
}
lane.waypoints[0].pose.pose.orientation = lane.waypoints[1].pose.pose.orientation;
lane.waypoints.back().twist.twist = original_lane.waypoints.back().twist.twist;
lane.waypoints.back().wpstate = original_lane.waypoints.back().wpstate;
lane.waypoints.back().change_flag = original_lane.waypoints.back().change_flag;
}
對getCrvPointsOnResample採三個軌跡點，和進行分析：
const CbufGPoint WaypointReplanner::getCrvPointsOnResample(
const autoware_msgs::Lane& lane, const autoware_msgs::Lane& original_lane, unsigned long original_index) const
{
unsigned long id = original_index;
CbufGPoint curve_point(3);
const unsigned int n = (lookup_crv_width_ - 1) / 2;//lookup_crv_width_此處被定義爲5
const autoware_msgs::Waypoint cp[3] = {
(lane.waypoints.size() < n) ? lane.waypoints.front() : lane.waypoints[lane.waypoints.size() - n],
original_lane.waypoints[id],
(id < original_lane.waypoints.size() - n) ? original_lane.waypoints[id + n] : original_lane.waypoints.back()
};
for (int i = 0; i < 3; i++)
{
curve_point.push_back(cp[i].pose.pose.position);
}
return curve_point;
}
其中lookup_crv_width_此處被賦值爲5，因此n=2。我們不妨進行假設，original_lane.waypoints.size()=5。因爲lane.waypoints.clear()之後lane.waypoints.emplace_back(original_lane.waypoints[0])，可知lane在進入for循環被傳入getCrvPointsOnResample函數時初始lane.waypoints.size()=1。每次for循環一次，會調用resampleOnStraight或者resampleOnCurve函數，往lane裏面填入新的路徑點，因此每次for循環結束自然而然lane.waypoints.size()++。基於上面的假設以及已知條件，我們開始進行分析。
calcCurveParam函數根據採樣的三個點，和首先計算：
由上面各式進一步計算得到曲線中心點和曲率：
上面各式的原理是什麼呢？上面的公式都是合併整理過的，我們看函數名猜測是用來根據圓上三點求圓心和半徑的，而且最後一個求曲率的公式也確實是計算圓心到其中某一點的距離。“根據圓上三點求圓心和半徑”這一問題有多種解法，主流的有兩種：（1）分別通過其中兩點的中垂線交點求圓心；（2）通過三個點到圓心距離相等聯立方程求解。這兩種方法的具體編程大家可以看圓上三點求圓心和半徑，博主在這裏就對第一種方法所得的公式與Autoware中的WaypointReplanner::calcCurveParam函數所使用的公式的最終結果進行對比，驗證見下圖，驗證代碼我也附上了，注意編譯的時候要指明g++ -std=c++11，由此驗證了WaypointReplanner::calcCurveParam函數確實是用來根據圓上三點求圓心和半徑的，至於它的公式究竟是根據原理1整理而來，還是由原理2演化而來，都已經不重要了，這兩個原理的求解公式必定是能合併整理成跟WaypointReplanner::calcCurveParam函數所使用的公式一樣的（因爲結果是一致的）。
const std::vector<double> WaypointReplanner::calcCurveParam(CbufGPoint p) const
{
for (int i = 0; i < 3; i++, p.push_back(p.front())) // if exception occured, change points order
{
const double d = 2 * ((p[0].y - p[2].y) * (p[0].x - p[1].x) - (p[0].y - p[1].y) * (p[0].x - p[2].x));
if (fabs(d) < 1e-8)
{
continue;
}
const std::vector<double> x2 = { p[0].x * p[0].x, p[1].x * p[1].x, p[2].x * p[2].x };
const std::vector<double> y2 = { p[0].y * p[0].y, p[1].y * p[1].y, p[2].y * p[2].y };
const double a = y2[0] - y2[1] + x2[0] - x2[1];
const double b = y2[0] - y2[2] + x2[0] - x2[2];
std::vector<double> param(3);
const double cx = param[0] = ((p[0].y - p[2].y) * a - (p[0].y - p[1].y) * b) / d;
const double cy = param[1] = ((p[0].x - p[2].x) * a - (p[0].x - p[1].x) * b) / -d;
param[2] = sqrt((cx - p[0].x) * (cx - p[0].x) + (cy - p[0].y) * (cy - p[0].y));
return param;
}
return std::vector<double>(); // error
}
驗證代碼：
#include <string>
#include <vector>
#include <algorithm>
#include <math.h>
#include <iostream>
using namespace std;
class Point2f{
public:
double x,y,z;
public:
Point2f(double X=0,double Y=0,double Z=0){
this->x=X,this->y=Y,this->z=Z;
}
~Point2f(){}
};
struct CircleData
{
Point2f center;
double radius;
};
int main()
{
//隨便定義三個點
Point2f pt1, pt2, pt3;
pt1.x = 150;
pt1.y = 150;
pt2.x = 200;
pt2.y = 200;
pt3.x = 250;
pt3.y = 100;
//定義兩個點，分別表示兩個中點
Point2f midpt1, midpt2;
//求出點1和點2的中點
midpt1.x = (pt2.x + pt1.x) / 2;
midpt1.y = (pt2.y + pt1.y) / 2;
//求出點3和點1的中點
midpt2.x = (pt3.x + pt1.x) / 2;
midpt2.y = (pt3.y + pt1.y) / 2;
//求出分別與直線pt1pt2，pt1pt3垂直的直線的斜率
double k1 = -(pt2.x - pt1.x) / (pt2.y - pt1.y);
double k2 = -(pt3.x - pt1.x) / (pt3.y - pt1.y);
//然後求出過中點midpt1，斜率爲k1的直線方程（既pt1pt2的中垂線）：y - midPt1.y = k1( x - midPt1.x)
//以及過中點midpt2，斜率爲k2的直線方程（既pt1pt3的中垂線）：y - midPt2.y = k2( x - midPt2.x)
//定義一個圓的數據的結構體對象CD
CircleData CD;
//連立兩條中垂線方程求解交點得到：
CD.center.x = (midpt2.y - midpt1.y - k2* midpt2.x + k1*midpt1.x) / (k1 - k2);
CD.center.y = midpt1.y + k1*(midpt2.y - midpt1.y - k2*midpt2.x + k2*midpt1.x) / (k1 - k2);
//用圓心和其中一個點求距離得到半徑：
CD.radius = sqrt((CD.center.x - pt1.x)*(CD.center.x - pt1.x) + (CD.center.y - pt1.y)*(CD.center.y - pt1.y));
cout << "CD.center.x = " << CD.center.x << endl;
cout << "CD.center.y = " << CD.center.y << endl;
cout << "CD.radius = " << CD.radius << endl;
//隨便定義三個點
Point2f p0, p1, p2;
p0.x = 150;
p0.y = 150;
p1.x = 200;
p1.y = 200;
p2.x = 250;
p2.y = 100;
const double d = 2 * ((p0.y - p2.y) * (p0.x - p1.x) - (p0.y - p1.y) * (p0.x - p2.x));
const std::vector<double> x2 = { p0.x * p0.x, p1.x * p1.x, p2.x * p2.x };
const std::vector<double> y2 = { p0.y * p0.y, p1.y * p1.y, p2.y * p2.y };
const double a = y2[0] - y2[1] + x2[0] - x2[1];
const double b = y2[0] - y2[2] + x2[0] - x2[2];
std::vector<double> param(3);
const double cx = ((p0.y - p2.y) * a - (p0.y - p1.y) * b) / d;
const double cy = ((p0.x - p2.x) * a - (p0.x - p1.x) * b) / -d;
const double r = sqrt((cx - p0.x) * (cx - p0.x) + (cy - p0.y) * (cy - p0.y));
cout << "cx = " << cx << endl;
cout << "cy = " << cy << endl;
cout << "r = " << r << endl;
return 0;
}
驗證結果：
resampleOnStraight函數：
void WaypointReplanner::resampleOnStraight(const CbufGPoint& curve_point, autoware_msgs::Lane& lane)
{
if (curve_point.size() != 3)
{
return;
}
autoware_msgs::Waypoint wp(lane.waypoints.back());
const geometry_msgs::Point& pt = wp.pose.pose.position;
const double yaw = atan2(curve_point[2].y - curve_point[0].y, curve_point[2].x - curve_point[0].x);
wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
//向量nvec由目標點和新lane的最後一個軌跡點確定
const std::vector<double> nvec = { curve_point[1].x - pt.x, curve_point[1].y - pt.y, curve_point[1].z - pt.z };
double dist = sqrt(nvec[0] * nvec[0] + nvec[1] * nvec[1]);//計算目標軌跡點和lane中最後一個軌跡點之間的距離
std::vector<double> resample_vec = nvec;
const double coeff = resample_interval_ / dist;
for (auto& el : resample_vec)//根據重採樣間隔resample_interval_矯正向量resample_vec的模
{
el *= coeff;
}
for (; dist > resample_interval_; dist -= resample_interval_)
{//在lane的最後一個軌跡點的基礎上根據向量resample_vec確定下一軌跡點，直到當前軌跡點離目標點的距離不足採樣間隔
wp.pose.pose.position.x += resample_vec[0];
wp.pose.pose.position.y += resample_vec[1];
wp.pose.pose.position.z += resample_vec[2];
lane.waypoints.emplace_back(wp);
}
}
resampleOnCurve函數：
void WaypointReplanner::resampleOnCurve(const geometry_msgs::Point& target_point,
const std::vector<double>& curve_param, autoware_msgs::Lane& lane, int dir)
{
if (curve_param.size() != 3)
{
return;
}
autoware_msgs::Waypoint wp(lane.waypoints.back());
const double& cx = curve_param[0];
const double& cy = curve_param[1];
const double& radius = curve_param[2];
const double reverse_angle = (dir < 0) ? M_PI : 0.0;
const geometry_msgs::Point& p0 = wp.pose.pose.position;
const geometry_msgs::Point& p1 = target_point;
double theta = fmod(atan2(p1.y - cy, p1.x - cx) - atan2(p0.y - cy, p0.x - cx), 2 * M_PI);//fmod取餘，
//保證atan2(p1.y - cy, p1.x - cx) - atan2(p0.y - cy, p0.x - cx)的值在2 * M_PI範圍內
//atan2(p1.y - cy, p1.x - cx) - atan2(p0.y - cy, p0.x - cx)代表本次重採樣的起始點和目標點與軌跡曲線圓心之間的夾角
int sgn = (theta > 0.0) ? (1) : (-1);
if (fabs(theta) > M_PI)
{
theta -= 2 * sgn * M_PI;//將theta的取值限定在-M_PI~M_PI
}
sgn = (theta > 0.0) ? (1) : (-1);
// interport
double t = atan2(p0.y - cy, p0.x - cx);
double dist = radius * fabs(theta);//本次重採樣的起始點和目標點之間的軌跡的弧長
const double resample_dz = resample_interval_ * (p1.z - p0.z) / dist;//z座標增長率，
//在WaypointReplanner::resampleOnStraight函數中忘了z座標的變化
for (; dist > resample_interval_; dist -= resample_interval_)
{
if (lane.waypoints.size() == lane.waypoints.capacity())//這裏檢查了lane是否已經儲存滿，然而在
//WaypointReplanner::resampleOnStraight函數中同樣重採樣軌跡點並存入lane，但是卻忘了檢查lane是否存滿的問題
{
break;
}
t += sgn * resample_interval_ / radius;//resample_interval_ / radius->採樣弧長/半徑 得到 採樣弧度
const double yaw = fmod(t + sgn * M_PI / 2.0, 2 * M_PI) + reverse_angle;
wp.pose.pose.position.x = cx + radius * cos(t);
wp.pose.pose.position.y = cy + radius * sin(t);
wp.pose.pose.position.z += resample_dz;
wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
lane.waypoints.emplace_back(wp);
}
}
篇幅已經挺長的了，本篇就到此爲止了。這一篇將節點waypoint_replanner中函數WaypointReplanner::replanLaneWaypointVel（位於src/autoware/core_planning/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.cpp）所調用的函數WaypointReplanner::resampleLaneWaypoint分析完成。後面一篇博客回到函數WaypointReplanner::replanLaneWaypointVel繼續分析。