1 fast-planner框架
2 fast_planner_node.cpp
ros启动的节点
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <plan_manage/kino_replan_fsm.h>
#include <plan_manage/topo_replan_fsm.h>
#include <plan_manage/backward.hpp>
namespace backward {
backward::SignalHandling sh;
}
using namespace fast_planner;
int main(int argc, char** argv) {
ros::init(argc, argv, "fast_planner_node");
ros::NodeHandle nh("~");
int planner;
// 根据launch文件中planner_node/planner选择控制器。
nh.param("planner_node/planner", planner, -1);
TopoReplanFSM topo_replan;
//实例化 状态函数
/*******
接收参数：飞机参数（位置，速度，加速度，，odom参数）
订阅无人机位置和目标点信息， 并通过定时回调函数execFsmCallback判断当前状态，
利用exec_state判断是否等待目标点，是否进行重规划，是否执行轨迹。
利用定时回调函数。
checkCollisionCallback判断是否需要重新寻找目标点，是否需要进行重规划。
规划的接口都是CallKinodynamicReplan函数，
其中利用的是planner_manager的kinodynamicReplan函数
*********/
KinoReplanFSM kino_replan;
if (planner == 1) {
kino_replan.init(nh);
} else if (planner == 2) {
topo_replan.init(nh);
}
ros::Duration(1.0).sleep();
ros::spin();
return 0;
}
3 初始化状态KinoReplanFSM::init()函数
#include <plan_manage/kino_replan_fsm.h>
namespace fast_planner {
void KinoReplanFSM::init(ros::NodeHandle& nh) {
/* */
current_wp_ = 0;
exec_state_ = FSM_EXEC_STATE::INIT;
have_target_ = false;
have_odom_ = false;
/* fsm param */
// 通过target_type_模式选择输入初始点或者路径点
nh.param("fsm/flight_type", target_type_, -1);
nh.param("fsm/thresh_replan", replan_thresh_, -1.0);
nh.param("fsm/thresh_no_replan", no_replan_thresh_, -1.0);
nh.param("fsm/waypoint_num", waypoint_num_, -1);
for (int i = 0; i < waypoint_num_; i++) {
nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0);
nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0);
}
/* initialize main modules */
planner_manager_.reset(new FastPlannerManager);
planner_manager_->initPlanModules(nh);
visualization_.reset(new PlanningVisualization(nh));
/* callback */
// 定时器
exec_timer_ = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this);
safety_timer_ = nh.createTimer(ros::Duration(0.05), &KinoReplanFSM::checkCollisionCallback, this);
// 订阅路径点
waypoint_sub_ =
nh.subscribe("/waypoint_generator/waypoints", 1, &KinoReplanFSM::waypointCallback, this);
//订阅里程计
odom_sub_ = nh.subscribe("/odom_world", 1, &KinoReplanFSM::odometryCallback, this);
// 发布器 样条曲线 路径点
replan_pub_ = nh.advertise<std_msgs::Empty>("/planning/replan", 10);
new_pub_ = nh.advertise<std_msgs::Empty>("/planning/new", 10);
bspline_pub_ = nh.advertise<plan_manage::Bspline>("/planning/bspline", 10);
}
/* 订阅目标点回调函数 */
void KinoReplanFSM::waypointCallback(const nav_msgs::PathConstPtr& msg) {
if (msg->poses[0].pose.position.z < -0.1) return;
cout << "Triggered!" << endl;
trigger_ = true;
if (target_type_ == TARGET_TYPE::MANUAL_TARGET) {
end_pt_ << msg->poses[0].pose.position.x, msg->poses[0].pose.position.y, 1.0;
} else if (target_type_ == TARGET_TYPE::PRESET_TARGET) {
end_pt_(0) = waypoints_[current_wp_][0];
end_pt_(1) = waypoints_[current_wp_][1];
end_pt_(2) = waypoints_[current_wp_][2];
current_wp_ = (current_wp_ + 1) % waypoint_num_;
}
visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0));
end_vel_.setZero();
have_target_ = true;
if (exec_state_ == WAIT_TARGET)
changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
else if (exec_state_ == EXEC_TRAJ)
changeFSMExecState(REPLAN_TRAJ, "TRIG");
}
/* 里程计订阅回调函数 */
void KinoReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr& msg) {
odom_pos_(0) = msg->pose.pose.position.x;
odom_pos_(1) = msg->pose.pose.position.y;
odom_pos_(2) = msg->pose.pose.position.z;
odom_vel_(0) = msg->twist.twist.linear.x;
odom_vel_(1) = msg->twist.twist.linear.y;
odom_vel_(2) = msg->twist.twist.linear.z;
odom_orient_.w() = msg->pose.pose.orientation.w;
odom_orient_.x() = msg->pose.pose.orientation.x;
odom_orient_.y() = msg->pose.pose.orientation.y;
odom_orient_.z() = msg->pose.pose.orientation.z;
have_odom_ = true;
}
void KinoReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call) {
string state_str[5] = { "INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ" };
int pre_s = int(exec_state_);
exec_state_ = new_state;
cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl;
}
void KinoReplanFSM::printFSMExecState() {
string state_str[5] = { "INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ" };
cout << "[FSM]: state: " + state_str[int(exec_state_)] << endl;
}
/** 状态机FSM改变函数：这个函数的触发时间是每0.01秒。
首先，每1秒打印一次当前执行状态。
然后，根据执行状态变量 exec_state_进入switch循环。
订阅：odom与trigger在：KinoReplanFSM::odometryCallback()和KinoReplanFSM::waypointCallback()
*/
void KinoReplanFSM::execFSMCallback(const ros::TimerEvent& e) {
static int fsm_num = 0;
fsm_num++;
//这个函数的触发时间是每0.01秒。
//首先，每1秒打印一次当前执行状态。
//然后，根据执行状态变量 exec_state_进入switch循环。
if (fsm_num == 100) {
printFSMExecState();
if (!have_odom_) cout << "no odom." << endl;
if (!trigger_) cout << "wait for goal." << endl;
fsm_num = 0;
}
switch (exec_state_) {
/*
如果状态是INIT（初始化），则判断有没有收到odometry 和 plan的 ttrigger是否被触发。
如果都通过，改变当前状态为WAIT_TARGET,并跳出当前循环。
*/
case INIT: {
if (!have_odom_) {
return;
}
if (!trigger_) {
return;
}
changeFSMExecState(WAIT_TARGET, "FSM");
break;
}
/*
如果状态是WAIT_TARGET（等待目标点）, 则判断是否有target，
如果有，则改变状态为GEN_NEW_TRAJ，并跳出当前循环。
*/
case WAIT_TARGET: {
if (!have_target_)
return;
else {
// 生成路径的轨迹
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
break;
}
/*
如果状态是GEN_NEW_TRAJ(生成轨迹)，则调用callKinodynamicReplan()函数进行规划。
成功则改变执行状态为EXEC_TRAJ,失败则改变执行状态为GEN_NEW_TRAJ。
这里我们来看callKinodynamicReplan()函数。如果规划成功的话，
就通过Bspline_pub把相应的B样条轨迹发布出去，否则返回FALSE
*/
case GEN_NEW_TRAJ: {
start_pt_ = odom_pos_;
start_vel_ = odom_vel_;
start_acc_.setZero();
Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
start_yaw_(0) = atan2(rot_x(1), rot_x(0));
start_yaw_(1) = start_yaw_(2) = 0.0;
bool success = callKinodynamicReplan();
if (success) {
//执行路径规划的轨迹
changeFSMExecState(EXEC_TRAJ, "FSM");
} else {
// have_target_ = false;
// changeFSMExecState(WAIT_TARGET, "FSM");
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
break;
}
/*
如果状态为EXEC_TRAJ(执行轨迹)，则判断是否需要进行重规划。
如果当前时间距离起始时间已经超过当前执行轨迹的时长，
则将have_target置于false，将执行状态变为WAIT_TARGET。
如果当前距离与终点距离小于不需要重规划的阈值，
或者当前距离与起点距离小于horizon，那么直接返回。
反之，则进入重规划阶段。
*/
case EXEC_TRAJ: {
/* determine if need to replan */
LocalTrajData* info = &planner_manager_->local_data_;
ros::Time time_now = ros::Time::now();
double t_cur = (time_now - info->start_time_).toSec();
t_cur = min(info->duration_, t_cur);
Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);
/* && (end_pt_ - pos).norm() < 0.5 */
if (t_cur > info->duration_ - 1e-2) {
have_target_ = false;
changeFSMExecState(WAIT_TARGET, "FSM");
return;
} else if ((end_pt_ - pos).norm() < no_replan_thresh_) {
// cout << "near end" << endl;
return;
} else if ((info->start_pos_ - pos).norm() < replan_thresh_) {
// cout << "near start" << endl;
return;
} else {
changeFSMExecState(REPLAN_TRAJ, "FSM");
}
break;
}
/*
如果状态为REPLAN_TRAJ(重新规划)，则利用当前的位置，速度，状态，
进行callKinodynamicReplan重规划，
如果成功，则将执行状态更改为EXEC_TRAJ,否则将执行状态更改为GEN_NEW_TRAJ.
*/
case REPLAN_TRAJ: {
LocalTrajData* info = &planner_manager_->local_data_;
ros::Time time_now = ros::Time::now();
double t_cur = (time_now - info->start_time_).toSec();
start_pt_ = info->position_traj_.evaluateDeBoorT(t_cur);
start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur);
start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur);
start_yaw_(0) = info->yaw_traj_.evaluateDeBoorT(t_cur)[0];
start_yaw_(1) = info->yawdot_traj_.evaluateDeBoorT(t_cur)[0];
start_yaw_(2) = info->yawdotdot_traj_.evaluateDeBoorT(t_cur)[0];
std_msgs::Empty replan_msg;
replan_pub_.publish(replan_msg);
bool success = callKinodynamicReplan();
if (success) {
changeFSMExecState(EXEC_TRAJ, "FSM");
} else {
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
break;
}
}
}
/*
这一执行器回调函数是用来判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的。
如果目标点有障碍物，就在目标点周围通过离散的半径及角度循环来寻找新的安全的目标点。
如果找到了，就直接改变状态进入REPLAN_TRAJ。
如果目标点周围没有障碍物且目前状态是EXEX_TRAJ,
则利用planner_manager的checkTrajCollision函数进行轨迹检查，
如果轨迹不发生碰撞，则无事发生，如果轨迹碰撞，则状态变为REPLAN_TRAJ,进行重规划
*/
// 圆形范围判断轨迹是否碰撞
void KinoReplanFSM::checkCollisionCallback(const ros::TimerEvent& e) {
LocalTrajData* info = &planner_manager_->local_data_;
if (have_target_) {
auto edt_env = planner_manager_->edt_environment_;
double dist = planner_manager_->pp_.dynamic_ ?
edt_env->evaluateCoarseEDT(end_pt_, /* time to program start + */ info->duration_) :
edt_env->evaluateCoarseEDT(end_pt_, -1.0);
if (dist <= 0.3) {
/* try to find a max distance goal around */
bool new_goal = false;
const double dr = 0.5, dtheta = 30, dz = 0.3;
double new_x, new_y, new_z, max_dist = -1.0;
Eigen::Vector3d goal;
for (double r = dr; r <= 5 * dr + 1e-3; r += dr) {
for (double theta = -90; theta <= 270; theta += dtheta) {
for (double nz = 1 * dz; nz >= -1 * dz; nz -= dz) {
new_x = end_pt_(0) + r * cos(theta / 57.3);
new_y = end_pt_(1) + r * sin(theta / 57.3);
new_z = end_pt_(2) + nz;
Eigen::Vector3d new_pt(new_x, new_y, new_z);
dist = planner_manager_->pp_.dynamic_ ?
edt_env->evaluateCoarseEDT(new_pt, /* time to program start+ */ info->duration_) :
edt_env->evaluateCoarseEDT(new_pt, -1.0);
if (dist > max_dist) {
/* reset end_pt_ */
goal(0) = new_x;
goal(1) = new_y;
goal(2) = new_z;
max_dist = dist;
}
}
}
}
if (max_dist > 0.3) {
cout << "change goal, replan." << endl;
end_pt_ = goal;
have_target_ = true;
end_vel_.setZero();
if (exec_state_ == EXEC_TRAJ) {
changeFSMExecState(REPLAN_TRAJ, "SAFETY");
}
visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0));
} else {
// have_target_ = false;
// cout << "Goal near collision, stop." << endl;
// changeFSMExecState(WAIT_TARGET, "SAFETY");
cout << "goal near collision, keep retry" << endl;
changeFSMExecState(REPLAN_TRAJ, "FSM");
std_msgs::Empty emt;
replan_pub_.publish(emt);
}
}
}
/* ---------- check trajectory ---------- */
if (exec_state_ == FSM_EXEC_STATE::EXEC_TRAJ) {
double dist;
bool safe = planner_manager_->checkTrajCollision(dist);
if (!safe) {
// cout << "current traj in collision." << endl;
ROS_WARN("current traj in collision.");
changeFSMExecState(REPLAN_TRAJ, "SAFETY");
}
}
}
// 路径规划并检查规划是否成功（planner_manager的入口）
bool KinoReplanFSM::callKinodynamicReplan() {
bool plan_success =
planner_manager_->kinodynamicReplan(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_);
if (plan_success) {
planner_manager_->planYaw(start_yaw_);
auto info = &planner_manager_->local_data_;
/* publish traj */
plan_manage::Bspline bspline;
bspline.order = 3;
bspline.start_time = info->start_time_;
bspline.traj_id = info->traj_id_;
Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
for (int i = 0; i < pos_pts.rows(); ++i) {
geometry_msgs::Point pt;
pt.x = pos_pts(i, 0);
pt.y = pos_pts(i, 1);
pt.z = pos_pts(i, 2);
bspline.pos_pts.push_back(pt);
}
Eigen::VectorXd knots = info->position_traj_.getKnot();
for (int i = 0; i < knots.rows(); ++i) {
bspline.knots.push_back(knots(i));
}
Eigen::MatrixXd yaw_pts = info->yaw_traj_.getControlPoint();
for (int i = 0; i < yaw_pts.rows(); ++i) {
double yaw = yaw_pts(i, 0);
bspline.yaw_pts.push_back(yaw);
}
bspline.yaw_dt = info->yaw_traj_.getInterval();
bspline_pub_.publish(bspline);
/* visulization */
//可视化
auto plan_data = &planner_manager_->plan_data_;
visualization_->drawGeometricPath(plan_data->kino_path_, 0.075, Eigen::Vector4d(1, 1, 0, 0.4));
visualization_->drawBspline(info->position_traj_, 0.1, Eigen::Vector4d(1.0, 0, 0.0, 1), true, 0.2,
Eigen::Vector4d(1, 0, 0, 1));
return true;
} else {
cout << "generate new traj fail." << endl;
return false;
}
}
// KinoReplanFSM::
} // namespace fast_planner
4 Planner_manager.cpp
响应fsm，调用底层算法包含了一系列plan的接口函数；一系列规划算法包中的数据结构和类；其他模块的支持API服务响应。
#include <plan_manage/planner_manager.h>
#include <thread>
namespace fast_planner {
// SECTION interfaces for setup and query
FastPlannerManager::FastPlannerManager() {}
FastPlannerManager::~FastPlannerManager() { std::cout << "des manager" << std::endl; }
// 初始化参数模块
void FastPlannerManager::initPlanModules(ros::NodeHandle& nh) {
/* read algorithm parameters */
nh.param("manager/max_vel", pp_.max_vel_, -1.0);
nh.param("manager/max_acc", pp_.max_acc_, -1.0);
nh.param("manager/max_jerk", pp_.max_jerk_, -1.0);
nh.param("manager/dynamic_environment", pp_.dynamic_, -1);
nh.param("manager/clearance_threshold", pp_.clearance_, -1.0);
nh.param("manager/local_segment_length", pp_.local_traj_len_, -1.0);
nh.param("manager/control_points_distance", pp_.ctrl_pt_dist, -1.0);
bool use_geometric_path, use_kinodynamic_path, use_topo_path, use_optimization, use_active_perception;
nh.param("manager/use_geometric_path", use_geometric_path, false);
nh.param("manager/use_kinodynamic_path", use_kinodynamic_path, false);
nh.param("manager/use_topo_path", use_topo_path, false);
nh.param("manager/use_optimization", use_optimization, false);
local_data_.traj_id_ = 0;
sdf_map_.reset(new SDFMap);
sdf_map_->initMap(nh);
edt_environment_.reset(new EDTEnvironment);
edt_environment_->setMap(sdf_map_);
if (use_geometric_path) {
geo_path_finder_.reset(new Astar);
geo_path_finder_->setParam(nh);
geo_path_finder_->setEnvironment(edt_environment_);
geo_path_finder_->init();
}
if (use_kinodynamic_path) {
kino_path_finder_.reset(new KinodynamicAstar);
kino_path_finder_->setParam(nh);
kino_path_finder_->setEnvironment(edt_environment_);
kino_path_finder_->init();
}
if (use_optimization) {
bspline_optimizers_.resize(10);
for (int i = 0; i < 10; ++i) {
bspline_optimizers_[i].reset(new BsplineOptimizer);
bspline_optimizers_[i]->setParam(nh);
bspline_optimizers_[i]->setEnvironment(edt_environment_);
}
}
if (use_topo_path) {
topo_prm_.reset(new TopologyPRM);
topo_prm_->setEnvironment(edt_environment_);
topo_prm_->init(nh);
}
}
void FastPlannerManager::setGlobalWaypoints(vector<Eigen::Vector3d>& waypoints) {
plan_data_.global_waypoints_ = waypoints;
}
bool FastPlannerManager::checkTrajCollision(double& distance) {
double t_now = (ros::Time::now() - local_data_.start_time_).toSec();
double tm, tmp;
local_data_.position_traj_.getTimeSpan(tm, tmp);
Eigen::Vector3d cur_pt = local_data_.position_traj_.evaluateDeBoor(tm + t_now);
double radius = 0.0;
Eigen::Vector3d fut_pt;
double fut_t = 0.02;
while (radius < 6.0 && t_now + fut_t < local_data_.duration_) {
fut_pt = local_data_.position_traj_.evaluateDeBoor(tm + t_now + fut_t);
double dist = edt_environment_->evaluateCoarseEDT(fut_pt, -1.0);
if (dist < 0.1) {
distance = radius;
return false;
}
radius = (fut_pt - cur_pt).norm();
fut_t += 0.02;
}
return true;
}
// !SECTION
// SECTION kinodynamic replanning
/**
kinodynamicReplan（正常情况下对轨迹进行规划）
首先从kino_path_finder_->search找到路径，
再到NonUniformBspline::parameterizeToBspline对B样条进行拟合，
再到BsplineOptimizeTraj均匀B样条优化，【防盗标记–盒子君hzj】
再到非均匀B样条迭代时间优化reallocateTime。
分别对应了Path-finding包，Bspline包，Bspline-Optimization包。
*/
bool FastPlannerManager::kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
Eigen::Vector3d start_acc, Eigen::Vector3d end_pt,
Eigen::Vector3d end_vel) {
std::cout << "[kino replan]: -----------------------" << std::endl;
cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << ", "
<< start_acc.transpose() << "\ngoal:" << end_pt.transpose() << ", " << end_vel.transpose()
<< endl;
if ((start_pt - end_pt).norm() < 0.2) {
cout << "Close goal" << endl;
return false;
}
ros::Time t1, t2;
local_data_.start_time_ = ros::Time::now();
double t_search = 0.0, t_opt = 0.0, t_adjust = 0.0;
Eigen::Vector3d init_pos = start_pt;
Eigen::Vector3d init_vel = start_vel;
Eigen::Vector3d init_acc = start_acc;
// kinodynamic path searching
t1 = ros::Time::now();
kino_path_finder_->reset();
/* 找到H A*路径 ：当把起止点状态都设置好后，
就利用kino_path_finder的search函数进行路径寻找。
search函数最开始会以start_acc作为起始点输入进行查找，
如果找不到，则再进行一轮离散化输入的寻找，若都找不到，则路径寻找失败
*/
int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true);
if (status == KinodynamicAstar::NO_PATH) {
cout << "[kino replan]: kinodynamic search fail!" << endl;
// retry searching with discontinuous initial state
kino_path_finder_->reset();
status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, false);
if (status == KinodynamicAstar::NO_PATH) {
cout << "[kino replan]: Can't find path." << endl;
return false;
} else {
cout << "[kino replan]: retry search success." << endl;
}
} else {
cout << "[kino replan]: kinodynamic search success." << endl;
}
plan_data_.kino_path_ = kino_path_finder_->getKinoTraj(0.01);
t_search = (ros::Time::now() - t1).toSec();
// parameterize the path to bspline
double ts = pp_.ctrl_pt_dist / pp_.max_vel_;
vector<Eigen::Vector3d> point_set, start_end_derivatives;
kino_path_finder_->getSamples(ts, point_set, start_end_derivatives);
Eigen::MatrixXd ctrl_pts;
// B样条对轨迹进行拟合
/***
成功寻找到一条路径后，利用NonUniformBspline::parameterizeToBspline()函数
对所找到的路径进行均匀B样条函数的拟合，然后得到相应控制点。
*/
NonUniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);
NonUniformBspline init(ctrl_pts, 3, ts);
// bspline trajectory optimization
// 均匀B样条曲线轨迹优化
t1 = ros::Time::now();
int cost_function = BsplineOptimizer::NORMAL_PHASE;
if (status != KinodynamicAstar::REACH_END) {
cost_function |= BsplineOptimizer::ENDPOINT;
}
// 对控制点进行优化 样条曲线优化轨迹
ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, ts, cost_function, 1, 1);
t_opt = (ros::Time::now() - t1).toSec();
// iterative time adjustment
/****
最后利用非均匀B样条类进行迭代时间调整
将调整后的B样条轨迹赋值给local_data.position_traj.
并利用updateTrajInfo函数对local_data的其他数据进行更新
*/
t1 = ros::Time::now();
NonUniformBspline pos = NonUniformBspline(ctrl_pts, 3, ts);
double to = pos.getTimeSum();
pos.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_);
bool feasible = pos.checkFeasibility(false);
int iter_num = 0;
while (!feasible && ros::ok()) {
// 时间优化
feasible = pos.reallocateTime();
if (++iter_num >= 3) break;
}
// pos.checkFeasibility(true);
// cout << "[Main]: iter num: " << iter_num << endl;
double tn = pos.getTimeSum();
cout << "[kino replan]: Reallocate ratio: " << tn / to << endl;
if (tn / to > 3.0) ROS_ERROR("reallocate error.");
t_adjust = (ros::Time::now() - t1).toSec();
// save planned results
local_data_.position_traj_ = pos;
/****
最后利用非均匀B样条类进行迭代时间调整，
将调整后的B样条轨迹赋值给local_data.position_traj.
并利用updateTrajInfo函数对local_data的其他数据进行更新
*/
double t_total = t_search + t_opt + t_adjust;
cout << "[kino replan]: time: " << t_total << ", search: " << t_search << ", optimize: " << t_opt
<< ", adjust time:" << t_adjust << endl;
pp_.time_search_ = t_search;
pp_.time_optimize_ = t_opt;
pp_.time_adjust_ = t_adjust;
updateTrajInfo();
return true;
}
// !SECTION
// SECTION topological replanning
bool FastPlannerManager::planGlobalTraj(const Eigen::Vector3d& start_pos) {
plan_data_.clearTopoPaths();
// generate global reference trajectory
vector<Eigen::Vector3d> points = plan_data_.global_waypoints_;
if (points.size() == 0) std::cout << "no global waypoints!" << std::endl;
points.insert(points.begin(), start_pos);
// insert intermediate points if too far
vector<Eigen::Vector3d> inter_points;
const double dist_thresh = 4.0;
for (int i = 0; i < points.size() - 1; ++i) {
inter_points.push_back(points.at(i));
double dist = (points.at(i + 1) - points.at(i)).norm();
if (dist > dist_thresh) {
int id_num = floor(dist / dist_thresh) + 1;
for (int j = 1; j < id_num; ++j) {
Eigen::Vector3d inter_pt =
points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num;
inter_points.push_back(inter_pt);
}
}
}
inter_points.push_back(points.back());
if (inter_points.size() == 2) {
Eigen::Vector3d mid = (inter_points[0] + inter_points[1]) * 0.5;
inter_points.insert(inter_points.begin() + 1, mid);
}
// write position matrix
int pt_num = inter_points.size();
Eigen::MatrixXd pos(pt_num, 3);
for (int i = 0; i < pt_num; ++i) pos.row(i) = inter_points[i];
Eigen::Vector3d zero(0, 0, 0);
Eigen::VectorXd time(pt_num - 1);
for (int i = 0; i < pt_num - 1; ++i) {
time(i) = (pos.row(i + 1) - pos.row(i)).norm() / (pp_.max_vel_);
}
time(0) *= 2.0;
time(0) = max(1.0, time(0));
time(time.rows() - 1) *= 2.0;
time(time.rows() - 1) = max(1.0, time(time.rows() - 1));
PolynomialTraj gl_traj = minSnapTraj(pos, zero, zero, zero, zero, time);
auto time_now = ros::Time::now();
global_data_.setGlobalTraj(gl_traj, time_now);
// truncate a local trajectory
double dt, duration;
Eigen::MatrixXd ctrl_pts = reparamLocalTraj(0.0, dt, duration);
NonUniformBspline bspline(ctrl_pts, 3, dt);
global_data_.setLocalTraj(bspline, 0.0, duration, 0.0);
local_data_.position_traj_ = bspline;
local_data_.start_time_ = time_now;
ROS_INFO("global trajectory generated.");
updateTrajInfo();
return true;
}
bool FastPlannerManager::topoReplan(bool collide) {
ros::Time t1, t2;
/* truncate a new local segment for replanning */
ros::Time time_now = ros::Time::now();
double t_now = (time_now - global_data_.global_start_time_).toSec();
double local_traj_dt, local_traj_duration;
double time_inc = 0.0;
Eigen::MatrixXd ctrl_pts = reparamLocalTraj(t_now, local_traj_dt, local_traj_duration);
NonUniformBspline init_traj(ctrl_pts, 3, local_traj_dt);
local_data_.start_time_ = time_now;
if (!collide) { // simply truncate the segment and do nothing
refineTraj(init_traj, time_inc);
local_data_.position_traj_ = init_traj;
global_data_.setLocalTraj(init_traj, t_now, local_traj_duration + time_inc + t_now, time_inc);
} else {
plan_data_.initial_local_segment_ = init_traj;
vector<Eigen::Vector3d> colli_start, colli_end, start_pts, end_pts;
findCollisionRange(colli_start, colli_end, start_pts, end_pts);
if (colli_start.size() == 1 && colli_end.size() == 0) {
ROS_WARN("Init traj ends in obstacle, no replanning.");
local_data_.position_traj_ = init_traj;
global_data_.setLocalTraj(init_traj, t_now, local_traj_duration + t_now, 0.0);
} else {
NonUniformBspline best_traj;
// local segment is in collision, call topological replanning
/* search topological distinctive paths */
ROS_INFO("[Topo]: ---------");
plan_data_.clearTopoPaths();
list<GraphNode::Ptr> graph;
vector<vector<Eigen::Vector3d>> raw_paths, filtered_paths, select_paths;
topo_prm_->findTopoPaths(colli_start.front(), colli_end.back(), start_pts, end_pts, graph,
raw_paths, filtered_paths, select_paths);
if (select_paths.size() == 0) {
ROS_WARN("No path.");
return false;
}
plan_data_.addTopoPaths(graph, raw_paths, filtered_paths, select_paths);
/* optimize trajectory using different topo paths */
ROS_INFO("[Optimize]: ---------");
t1 = ros::Time::now();
plan_data_.topo_traj_pos1_.resize(select_paths.size());
plan_data_.topo_traj_pos2_.resize(select_paths.size());
vector<thread> optimize_threads;
for (int i = 0; i < select_paths.size(); ++i) {
optimize_threads.emplace_back(&FastPlannerManager::optimizeTopoBspline, this, t_now,
local_traj_duration, select_paths[i], i);
// optimizeTopoBspline(t_now, local_traj_duration,
// select_paths[i], origin_len, i);
}
for (int i = 0; i < select_paths.size(); ++i) optimize_threads[i].join();
double t_opt = (ros::Time::now() - t1).toSec();
cout << "[planner]: optimization time: " << t_opt << endl;
selectBestTraj(best_traj);
refineTraj(best_traj, time_inc);
local_data_.position_traj_ = best_traj;
global_data_.setLocalTraj(local_data_.position_traj_, t_now,
local_traj_duration + time_inc + t_now, time_inc);
}
}
updateTrajInfo();
return true;
}
void FastPlannerManager::selectBestTraj(NonUniformBspline& traj) {
// sort by jerk
vector<NonUniformBspline>& trajs = plan_data_.topo_traj_pos2_;
sort(trajs.begin(), trajs.end(),
[&](NonUniformBspline& tj1, NonUniformBspline& tj2) { return tj1.getJerk() < tj2.getJerk(); });
traj = trajs[0];
}
void FastPlannerManager::refineTraj(NonUniformBspline& best_traj, double& time_inc) {
ros::Time t1 = ros::Time::now();
time_inc = 0.0;
double dt, t_inc;
const int max_iter = 1;
// int cost_function = BsplineOptimizer::NORMAL_PHASE | BsplineOptimizer::VISIBILITY;
Eigen::MatrixXd ctrl_pts = best_traj.getControlPoint();
int cost_function = BsplineOptimizer::NORMAL_PHASE;
best_traj.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_);
double ratio = best_traj.checkRatio();
std::cout << "ratio: " << ratio << std::endl;
reparamBspline(best_traj, ratio, ctrl_pts, dt, t_inc);
time_inc += t_inc;
ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, dt, cost_function, 1, 1);
best_traj = NonUniformBspline(ctrl_pts, 3, dt);
ROS_WARN_STREAM("[Refine]: cost " << (ros::Time::now() - t1).toSec()
<< " seconds, time change is: " << time_inc);
}
void FastPlannerManager::updateTrajInfo() {
local_data_.velocity_traj_ = local_data_.position_traj_.getDerivative();
local_data_.acceleration_traj_ = local_data_.velocity_traj_.getDerivative();
local_data_.start_pos_ = local_data_.position_traj_.evaluateDeBoorT(0.0);
local_data_.duration_ = local_data_.position_traj_.getTimeSum();
local_data_.traj_id_ += 1;
}
void FastPlannerManager::reparamBspline(NonUniformBspline& bspline, double ratio,
Eigen::MatrixXd& ctrl_pts, double& dt, double& time_inc) {
int prev_num = bspline.getControlPoint().rows();
double time_origin = bspline.getTimeSum();
int seg_num = bspline.getControlPoint().rows() - 3;
// double length = bspline.getLength(0.1);
// int seg_num = ceil(length / pp_.ctrl_pt_dist);
ratio = min(1.01, ratio);
bspline.lengthenTime(ratio);
double duration = bspline.getTimeSum();
dt = duration / double(seg_num);
time_inc = duration - time_origin;
vector<Eigen::Vector3d> point_set;
for (double time = 0.0; time <= duration + 1e-4; time += dt) {
point_set.push_back(bspline.evaluateDeBoorT(time));
}
NonUniformBspline::parameterizeToBspline(dt, point_set, plan_data_.local_start_end_derivative_,
ctrl_pts);
// ROS_WARN("prev: %d, new: %d", prev_num, ctrl_pts.rows());
}
void FastPlannerManager::optimizeTopoBspline(double start_t, double duration,
vector<Eigen::Vector3d> guide_path, int traj_id) {
ros::Time t1;
double tm1, tm2, tm3;
t1 = ros::Time::now();
// parameterize B-spline according to the length of guide path
int seg_num = topo_prm_->pathLength(guide_path) / pp_.ctrl_pt_dist;
Eigen::MatrixXd ctrl_pts;
double dt;
ctrl_pts = reparamLocalTraj(start_t, duration, seg_num, dt);
// std::cout << "ctrl pt num: " << ctrl_pts.rows() << std::endl;
// discretize the guide path and align it with B-spline control points
vector<Eigen::Vector3d> guide_pt;
guide_pt = topo_prm_->pathToGuidePts(guide_path, int(ctrl_pts.rows()) - 2);
guide_pt.pop_back();
guide_pt.pop_back();
guide_pt.erase(guide_pt.begin(), guide_pt.begin() + 2);
// std::cout << "guide pt num: " << guide_pt.size() << std::endl;
if (guide_pt.size() != int(ctrl_pts.rows()) - 6) ROS_WARN("what guide");
tm1 = (ros::Time::now() - t1).toSec();
t1 = ros::Time::now();
// first phase, path-guided optimization
bspline_optimizers_[traj_id]->setGuidePath(guide_pt);
Eigen::MatrixXd opt_ctrl_pts1 = bspline_optimizers_[traj_id]->BsplineOptimizeTraj(
ctrl_pts, dt, BsplineOptimizer::GUIDE_PHASE, 0, 1);
plan_data_.topo_traj_pos1_[traj_id] = NonUniformBspline(opt_ctrl_pts1, 3, dt);
tm2 = (ros::Time::now() - t1).toSec();
t1 = ros::Time::now();
// second phase, normal optimization
Eigen::MatrixXd opt_ctrl_pts2 = bspline_optimizers_[traj_id]->BsplineOptimizeTraj(
opt_ctrl_pts1, dt, BsplineOptimizer::NORMAL_PHASE, 1, 1);
plan_data_.topo_traj_pos2_[traj_id] = NonUniformBspline(opt_ctrl_pts2, 3, dt);
tm3 = (ros::Time::now() - t1).toSec();
ROS_INFO("optimization %d cost %lf, %lf, %lf seconds.", traj_id, tm1, tm2, tm3);
}
Eigen::MatrixXd FastPlannerManager::reparamLocalTraj(double start_t, double& dt, double& duration) {
/* get the sample points local traj within radius */
vector<Eigen::Vector3d> point_set;
vector<Eigen::Vector3d> start_end_derivative;
global_data_.getTrajByRadius(start_t, pp_.local_traj_len_, pp_.ctrl_pt_dist, point_set,
start_end_derivative, dt, duration);
/* parameterization of B-spline */
Eigen::MatrixXd ctrl_pts;
NonUniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);
plan_data_.local_start_end_derivative_ = start_end_derivative;
// cout << "ctrl pts:" << ctrl_pts.rows() << endl;
return ctrl_pts;
}
Eigen::MatrixXd FastPlannerManager::reparamLocalTraj(double start_t, double duration, int seg_num,
double& dt) {
vector<Eigen::Vector3d> point_set;
vector<Eigen::Vector3d> start_end_derivative;
global_data_.getTrajByDuration(start_t, duration, seg_num, point_set, start_end_derivative, dt);
plan_data_.local_start_end_derivative_ = start_end_derivative;
/* parameterization of B-spline */
Eigen::MatrixXd ctrl_pts;
NonUniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);
// cout << "ctrl pts:" << ctrl_pts.rows() << endl;
return ctrl_pts;
}
void FastPlannerManager::findCollisionRange(vector<Eigen::Vector3d>& colli_start,
vector<Eigen::Vector3d>& colli_end,
vector<Eigen::Vector3d>& start_pts,
vector<Eigen::Vector3d>& end_pts) {
bool last_safe = true, safe;
double t_m, t_mp;
NonUniformBspline* initial_traj = &plan_data_.initial_local_segment_;
initial_traj->getTimeSpan(t_m, t_mp);
/* find range of collision */
double t_s = -1.0, t_e;
for (double tc = t_m; tc <= t_mp + 1e-4; tc += 0.05) {
Eigen::Vector3d ptc = initial_traj->evaluateDeBoor(tc);
safe = edt_environment_->evaluateCoarseEDT(ptc, -1.0) < topo_prm_->clearance_ ? false : true;
if (last_safe && !safe) {
colli_start.push_back(initial_traj->evaluateDeBoor(tc - 0.05));
if (t_s < 0.0) t_s = tc - 0.05;
} else if (!last_safe && safe) {
colli_end.push_back(ptc);
t_e = tc;
}
last_safe = safe;
}
if (colli_start.size() == 0) return;
if (colli_start.size() == 1 && colli_end.size() == 0) return;
/* find start and end safe segment */
double dt = initial_traj->getInterval();
int sn = ceil((t_s - t_m) / dt);
dt = (t_s - t_m) / sn;
for (double tc = t_m; tc <= t_s + 1e-4; tc += dt) {
start_pts.push_back(initial_traj->evaluateDeBoor(tc));
}
dt = initial_traj->getInterval();
sn = ceil((t_mp - t_e) / dt);
dt = (t_mp - t_e) / sn;
// std::cout << "dt: " << dt << std::endl;
// std::cout << "sn: " << sn << std::endl;
// std::cout << "t_m: " << t_m << std::endl;
// std::cout << "t_mp: " << t_mp << std::endl;
// std::cout << "t_s: " << t_s << std::endl;
// std::cout << "t_e: " << t_e << std::endl;
if (dt > 1e-4) {
for (double tc = t_e; tc <= t_mp + 1e-4; tc += dt) {
end_pts.push_back(initial_traj->evaluateDeBoor(tc));
}
} else {
end_pts.push_back(initial_traj->evaluateDeBoor(t_mp));
}
}
// !SECTION
/***
路径的航向角yaw已经确定下来了，但是我们还是要把航向角yaw计算出来，
作为一个属性添加到轨迹之中，因为航向角yaw有时候是作为控制器的输入的，
怎么理解呢，就是像纯跟踪pure_suit这种控制器输入一系列离散的路径点就可以进行路径跟随，
但是PID控制输入一系列离散的路径点显然是不行的，
要先通过航向角yaw规划得到每个路径点的航向位置yaw作为PID控制器输入，
才能实现PID的航向角位置闭环控制
***/
/**取出对应时刻轨迹上的控制点，进而得到轨迹上两两相邻的控制点，
通过两两控制点的相对位置即可计算得到该条轨迹上每个控制点的航向角yaw，
这个航向角yaw也是该轨迹在该点的切线方向
***/
void FastPlannerManager::planYaw(const Eigen::Vector3d& start_yaw) {
ROS_INFO("plan yaw");
auto t1 = ros::Time::now();
// calculate waypoints of heading
//对轨迹进行分段
auto& pos = local_data_.position_traj_; //获取该轨迹的位置
double duration = pos.getTimeSum(); //获取该轨迹的总运行时间
double dt_yaw = 0.3; //设置航向角yaw的时间增量--300ms
int seg_num = ceil(duration / dt_yaw); //计算轨迹分段数，轨迹分段数 = 该轨迹的总运行时间/时间增量
dt_yaw = duration / seg_num; //设置航向角yaw的角度增量，角度增量 = 该轨迹的总运行时间/轨迹分段数
const double forward_t = 2.0;
double last_yaw = start_yaw(0);
vector<Eigen::Vector3d> waypts;
vector<int> waypt_idx;
// seg_num -> seg_num - 1 points for constraint excluding the boundary states
//seg_num->seg_num-1点用于约束，不包括边界状态
//（2）计算路径点waypoints的航向角yaw
for (int i = 0; i < seg_num; ++i) { 遍历所有的轨迹分段
double tc = i * dt_yaw; //迭代计算轨迹现在的运行时刻
Eigen::Vector3d pc = pos.evaluateDeBoorT(tc); //根据轨迹运行时刻，获得B样条的当前的控制点，注意这是当前控制点
double tf = min(duration, tc + forward_t);//迭代计算轨迹下一段的运行时刻
Eigen::Vector3d pf = pos.evaluateDeBoorT(tf); //根据轨迹运行时刻，获得B样条的下一段的控制点，注意这是下一段控制点
Eigen::Vector3d pd = pf - pc; //计算当前控制点与下一段控制点的有向向量
Eigen::Vector3d waypt; //当前控制点与下一段控制点的有向向量达到阈值，就计算yaw
if (pd.norm() > 1e-6) {
waypt(0) = atan2(pd(1), pd(0)); //计算量控制角的夹角，即航向yaw
waypt(1) = waypt(2) = 0.0;
calcNextYaw(last_yaw, waypt(0)); //计算下一个路径点的航向角yaw
} else {
waypt = waypts.back();
}
waypts.push_back(waypt);
waypt_idx.push_back(i);
}
// calculate initial control points with boundary state constraints
Eigen::MatrixXd yaw(seg_num + 3, 1);
yaw.setZero();
Eigen::Matrix3d states2pts;
states2pts << 1.0, -dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw, 1.0, 0.0, -(1 / 6.0) * dt_yaw * dt_yaw, 1.0,
dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw;
yaw.block(0, 0, 3, 1) = states2pts * start_yaw;
Eigen::Vector3d end_v = local_data_.velocity_traj_.evaluateDeBoorT(duration - 0.1);
Eigen::Vector3d end_yaw(atan2(end_v(1), end_v(0)), 0, 0);
calcNextYaw(last_yaw, end_yaw(0));
yaw.block(seg_num, 0, 3, 1) = states2pts * end_yaw;
// solve
bspline_optimizers_[1]->setWaypoints(waypts, waypt_idx);
int cost_func = BsplineOptimizer::SMOOTHNESS | BsplineOptimizer::WAYPOINTS;
yaw = bspline_optimizers_[1]->BsplineOptimizeTraj(yaw, dt_yaw, cost_func, 1, 1);
// update traj info
local_data_.yaw_traj_.setUniformBspline(yaw, 3, dt_yaw);
local_data_.yawdot_traj_ = local_data_.yaw_traj_.getDerivative();
local_data_.yawdotdot_traj_ = local_data_.yawdot_traj_.getDerivative();
vector<double> path_yaw;
for (int i = 0; i < waypts.size(); ++i) path_yaw.push_back(waypts[i][0]);
plan_data_.path_yaw_ = path_yaw;
plan_data_.dt_yaw_ = dt_yaw;
plan_data_.dt_yaw_path_ = dt_yaw;
std::cout << "plan heading: " << (ros::Time::now() - t1).toSec() << std::endl;
}
void FastPlannerManager::calcNextYaw(const double& last_yaw, double& yaw) {
// round yaw to [-PI, PI]
double round_last = last_yaw;
while (round_last < -M_PI) {
round_last += 2 * M_PI;
}
while (round_last > M_PI) {
round_last -= 2 * M_PI;
}
double diff = yaw - round_last;
if (fabs(diff) <= M_PI) {
yaw = last_yaw + diff;
} else if (diff > M_PI) {
yaw = last_yaw + diff - 2 * M_PI;
} else if (diff < -M_PI) {
yaw = last_yaw + diff + 2 * M_PI;
}
}
} // namespace fast_planner
5 8. Topo_PRM.cpp
路径规划： findTopoPaths
void TopologyPRM::findTopoPaths(Eigen::Vector3d start, Eigen::Vector3d end,
vector<Eigen::Vector3d> start_pts, vector<Eigen::Vector3d> end_pts,
list<GraphNode::Ptr>& graph, vector<vector<Eigen::Vector3d>>& raw_paths,
vector<vector<Eigen::Vector3d>>& filtered_paths,
vector<vector<Eigen::Vector3d>>& select_paths) {
ros::Time t1, t2;
double graph_time, search_time, short_time, prune_time, select_time;
/* ---------- create the topo graph ---------- */
t1 = ros::Time::now();
start_pts_ = start_pts;
end_pts_ = end_pts;
// 建立图地图 以起始点和end
graph = createGraph(start, end);
graph_time = (ros::Time::now() - t1).toSec();
/* ---------- search paths in the graph ---------- */
t1 = ros::Time::now();
raw_paths = searchPaths();
search_time = (ros::Time::now() - t1).toSec();
/* ---------- path shortening ---------- */
// for parallel, save result in short_paths_
t1 = ros::Time::now();
//对于并行，将结果保存在短_路径 short_paths_中_
shortcutPaths();
short_time = (ros::Time::now() - t1).toSec();
//
/* ---------- prune equivalent paths ---------- */
t1 = ros::Time::now();
filtered_paths = pruneEquivalent(short_paths_); //已筛选的路径
prune_time = (ros::Time::now() - t1).toSec();
// cout << "prune: " << (t2 - t1).toSec() << endl;
/* ---------- select N shortest paths ---------- */
t1 = ros::Time::now();
select_paths = selectShortPaths(filtered_paths, 1);
select_time = (ros::Time::now() - t1).toSec();
final_paths_ = select_paths;
double total_time = graph_time + search_time + short_time + prune_time + select_time;
std::cout << "\n[Topo]: total time: " << total_time << ", graph: " << graph_time
<< ", search: " << search_time << ", short: " << short_time << ", prune: " << prune_time
<< ", select: " << select_time << std::endl;
}
扫一扫
4151
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
