1. 初始化函数(TopologyPRM::init)
初始化参数值
void TopologyPRM::init(ros::NodeHandle& nh) {
// 地图
graph_.clear();
eng_ = default_random_engine(rd_());
rand_pos_ = uniform_real_distribution<double>(-1.0, 1.0);
// init parameter
//碰撞膨胀参数
nh.param("topo_prm/sample_inflate_x", sample_inflate_(0), -1.0);
nh.param("topo_prm/sample_inflate_y", sample_inflate_(1), -1.0);
nh.param("topo_prm/sample_inflate_z", sample_inflate_(2), -1.0);
//安全阈值
nh.param("topo_prm/clearance", clearance_, -1.0);
nh.param("topo_prm/short_cut_num", short_cut_num_, -1);
nh.param("topo_prm/reserve_num", reserve_num_, -1);
nh.param("topo_prm/ratio_to_short", ratio_to_short_, -1.0);
nh.param("topo_prm/max_sample_num", max_sample_num_, -1);
//最大采样时间
nh.param("topo_prm/max_sample_time", max_sample_time_, -1.0);
//最大路径
nh.param("topo_prm/max_raw_path", max_raw_path_, -1);
nh.param("topo_prm/max_raw_path2", max_raw_path2_, -1);
nh.param("topo_prm/parallel_shortcut", parallel_shortcut_, false);
//分辨率
resolution_ = edt_environment_->sdf_map_->getResolution();
offset_ = Eigen::Vector3d(0.5, 0.5, 0.5) - edt_environment_->sdf_map_->getOrigin() / resolution_;
for (int i = 0; i < max_raw_path_; ++i) {
casters_.push_back(RayCaster());
}
}
2. TopologyPRM::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;
// 创建节点
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();
// 路径修剪
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;
}
2.1采样节点:TopologyPRM::createGraph(Eigen::Vector3d start, Eigen::Vector3d end)
根据起始点和终点的位置信息,采样地图节点
list<GraphNode::Ptr> TopologyPRM::createGraph(Eigen::Vector3d start, Eigen::Vector3d end) {
// std::cout << "[Topo]: searching----------------------" << std::endl;
/* init the start, end and sample region */
graph_.clear();
// collis_.clear();
GraphNode::Ptr start_node = GraphNode::Ptr(new GraphNode(start, GraphNode::Guard, 0));
GraphNode::Ptr end_node = GraphNode::Ptr(new GraphNode(end, GraphNode::Guard, 1));
graph_.push_back(start_node);
graph_.push_back(end_node);
// sample region
//采样区域
sample_r_(0) = 0.5 * (end - start).norm() + sample_inflate_(0); //x方向采样的半径
sample_r_(1) = sample_inflate_(1); // y方向采样半径
sample_r_(2) = sample_inflate_(2); // z方向采样半径
// transformation
// 姿态改变参数
translation_ = 0.5 * (start + end);
Eigen::Vector3d xtf, ytf, ztf, downward(0, 0, -1);
xtf = (end - translation_).normalized();
ytf = xtf.cross(downward).normalized();
ztf = xtf.cross(ytf);
rotation_.col(0) = xtf;
rotation_.col(1) = ytf;
rotation_.col(2) = ztf;
int node_id = 1;
/* ---------- main loop ---------- */
int sample_num = 0; // 采样数目
double sample_time = 0.0; //采样时间
Eigen::Vector3d pt;
ros::Time t1, t2;
// 采样时间或者采样节点数目达到,停止
while (sample_time < max_sample_time_ && sample_num < max_sample_num_) {
t1 = ros::Time::now();
/** 函数2.1.1 **/
pt = getSample();
++sample_num;
double dist;
Eigen::Vector3d grad; //梯度
// edt_environment_->evaluateEDTWithGrad(pt, -1.0, dist, grad);
/** 函数2.1.2 **/
dist = edt_environment_->evaluateCoarseEDT(pt, -1.0); //距离esdf地图最近的障碍物的距离
if (dist <= clearance_) { //最小距离小于安全距离
sample_time += (ros::Time::now() - t1).toSec();
continue;
}
/* find visible guard */
// 查找梯度点
/** 函数2.1.3 **/
vector<GraphNode::Ptr> visib_guards = findVisibGuard(pt);
if (visib_guards.size() == 0) {
GraphNode::Ptr guard = GraphNode::Ptr(new GraphNode(pt, GraphNode::Guard, ++node_id));
graph_.push_back(guard);
} else if (visib_guards.size() == 2) {
/* try adding new connection between two guard */
// vector<pair<GraphNode::Ptr, GraphNode::Ptr>> sort_guards =
// sortVisibGuard(visib_guards);
// 在两点之间添加新的节点
/** 函数2.1.4 **/
bool need_connect = needConnection(visib_guards[0], visib_guards[1], pt);
if (!need_connect) {
sample_time += (ros::Time::now() - t1).toSec();
continue;
}
// new useful connection needed, add new connector
// 如果需要新的节点,添加新的节点
GraphNode::Ptr connector = GraphNode::Ptr(new GraphNode(pt, GraphNode::Connector, ++node_id));
graph_.push_back(connector);
// connect guards
visib_guards[0]->neighbors_.push_back(connector);
visib_guards[1]->neighbors_.push_back(connector);
connector->neighbors_.push_back(visib_guards[0]);
connector->neighbors_.push_back(visib_guards[1]);
}
sample_time += (ros::Time::now() - t1).toSec();
}
/* print record */
std::cout << "[Topo]: sample num: " << sample_num;
// 修剪节点图
pruneGraph();
// std::cout << "[Topo]: node num: " << graph_.size() << std::endl;
return graph_;
// return searchPaths(start_node, end_node);
}
2.1.1 getsample()
通过平移和旋转得到采样节点
/***
返回参数pt x y z
*/
Eigen::Vector3d TopologyPRM::getSample() {
//获取采样点
/* sampling */
Eigen::Vector3d pt;
pt(0) = rand_pos_(eng_) * sample_r_(0);//x
pt(1) = rand_pos_(eng_) * sample_r_(1);//y
pt(2) = rand_pos_(eng_) * sample_r_(2);//z
// 通过旋转+ 平移得到新的坐标点
pt = rotation_ * pt + translation_;//最终位置
return pt;//返回采样节点的位置
}
2.1.2 evaluateCoarseEDT(Eigen::Vector3d& pos, double time)
判断采样得点是否在障碍物上,并返回最小距离
double EDTEnvironment::evaluateCoarseEDT(Eigen::Vector3d& pos, double time) {
double d1 = sdf_map_->getDistance(pos);
if (time < 0.0) {
return d1;
} else {
double d2 = minDistToAllBox(pos, time);
return min(d1, d2);
}
}
2.1.3 TopologyPRM::findVisibGuard(Eigen::Vector3d pt)
前采样点与任一个guard都不可见时,则把它当做新的guard添加到gragh中,如果有且只有两个可见guard,则要利用needConnection函数判断是否要添加新的connector。
vector<GraphNode::Ptr> TopologyPRM::findVisibGuard(Eigen::Vector3d pt) {
vector<GraphNode::Ptr> visib_guards;
Eigen::Vector3d pc;
int visib_num = 0;
/* find visible GUARD from pt */
for (list<GraphNode::Ptr>::iterator iter = graph_.begin(); iter != graph_.end(); ++iter) {
if ((*iter)->type_ == GraphNode::Connector) continue;
// 根据pt得到guard 点
if (lineVisib(pt, (*iter)->pos_, resolution_, pc)) {
visib_guards.push_back((*iter));
++visib_num;
if (visib_num > 2) break;
}
}
return visib_guards;
}
2.1.3.1 TopologyPRM::lineVisib(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, double thresh,Eigen::Vector3d& pc, int caster_id)
将两个节点连接起来,利用利用raycast步进和ESDF中的距离信息来逐步检验连线上是否有障碍物,有障碍物则者两个节点不可见。
bool TopologyPRM::lineVisib(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, double thresh,
Eigen::Vector3d& pc, int caster_id) {
Eigen::Vector3d ray_pt;
Eigen::Vector3i pt_id; //位置id
double dist;
casters_[caster_id].setInput(p1 / resolution_, p2 / resolution_);
while (casters_[caster_id].step(ray_pt)) {
pt_id(0) = ray_pt(0) + offset_(0);
pt_id(1) = ray_pt(1) + offset_(1);
pt_id(2) = ray_pt(2) + offset_(2);
dist = edt_environment_->sdf_map_->getDistance(pt_id);
if (dist <= thresh) { // 距离小于阈值
edt_environment_->sdf_map_->indexToPos(pt_id, pc);
return false;
}
}
return true;
}
2.1.4 TopologyPRM::needConnection()
即判断当前路径path1与path2长度关系,则要判断path1是否比path2短,则替换之前两个guard的connector的位置,返回true。否则返回false。
bool TopologyPRM::needConnection(GraphNode::Ptr g1, GraphNode::Ptr g2, Eigen::Vector3d pt) {
vector<Eigen::Vector3d> path1(3), path2(3);
path1[0] = g1->pos_;
path1[1] = pt;
path1[2] = g2->pos_;
path2[0] = g1->pos_;
path2[2] = g2->pos_;
vector<Eigen::Vector3d> connect_pts; // 连接点得位置
bool has_connect = false;
for (int i = 0; i < g1->neighbors_.size(); ++i) {
for (int j = 0; j < g2->neighbors_.size(); ++j) {
if (g1->neighbors_[i]->id_ == g2->neighbors_[j]->id_) {
path2[1] = g1->neighbors_[i]->pos_;
// 判断轨迹是否相同
bool same_topo = sameTopoPath(path1, path2, 0.0);
if (same_topo) {
// get shorter connection ?
if (pathLength(path1) < pathLength(path2)) {
g1->neighbors_[i]->pos_ = pt;
// ROS_WARN("shorter!");
}
return false;
}
}
}
}
return true;
}
2.1.4.1 TopologyPRM::sameTopoPath(const vector<Eigen::Vector3d >& path1, const vector<Eigen::Vector3d >& path2, double thresh)
比较两条路径cost是否相同,是否有障碍物
bool TopologyPRM::sameTopoPath(const vector<Eigen::Vector3d>& path1,
const vector<Eigen::Vector3d>& path2, double thresh) {
// calc the length
double len1 = pathLength(path1);
double len2 = pathLength(path2);
double max_len = max(len1, len2);
int pt_num = ceil(max_len / resolution_); // 计算点数
// std::cout << "pt num: " << pt_num << std::endl;
// 离散path的路径点
vector<Eigen::Vector3d> pts1 = discretizePath(path1, pt_num);
vector<Eigen::Vector3d> pts2 = discretizePath(path2, pt_num);
Eigen::Vector3d pc;
for (int i = 0; i < pt_num; ++i) {
// 判断轨迹上是否有障碍物
if (!lineVisib(pts1[i], pts2[i], thresh, pc)) {
return false;
}
}
return true;
}
2.1.4.2 TopologyPRM::discretizePath(const vector<Eigen::Vector3d >& path, int pt_num)
将整条轨迹从几个节点离散化为更稠密的一系列路径点。
vector<Eigen::Vector3d> TopologyPRM::discretizePath(const vector<Eigen::Vector3d>& path, int pt_num) {
vector<double> len_list;
len_list.push_back(0.0);
// 计算路径点之间的距离
for (int i = 0; i < path.size() - 1; ++i) {
double inc_l = (path[i + 1] - path[i]).norm();
len_list.push_back(inc_l + len_list[i]);
}
// calc pt_num points along the path
double len_total = len_list.back();
// 每段轨迹距离
double dl = len_total / double(pt_num - 1);
double cur_l;
vector<Eigen::Vector3d> dis_path;
for (int i = 0; i < pt_num; ++i) {
cur_l = double(i) * dl;
// find the range cur_l in
int idx = -1;
for (int j = 0; j < len_list.size() - 1; ++j) {
if (cur_l >= len_list[j] - 1e-4 && cur_l <= len_list[j + 1] + 1e-4) {
idx = j;
break;
}
}
2.1.5 搜索路径 TopologyPRM::searchPaths()
通过深度优先搜索算法DFS,在节点图中搜索有用的路径,按节点数目对路径进行排序,选择节点数目较少的路径进行保留,并存储在raw_paths_中。
vector<vector<Eigen::Vector3d>> TopologyPRM::searchPaths() {
raw_paths_.clear();
vector<GraphNode::Ptr> visited;
visited.push_back(graph_.front());
depthFirstSearch(visited);
// sort the path by node number
// 根据节点得到路径
int min_node_num = 100000, max_node_num = 1;
vector<vector<int>> path_list(100);
for (int i = 0; i < raw_paths_.size(); ++i) {
if (int(raw_paths_[i].size()) > max_node_num) max_node_num = raw_paths_[i].size();
if (int(raw_paths_[i].size()) < min_node_num) min_node_num = raw_paths_[i].size();
path_list[int(raw_paths_[i].size())].push_back(i);
}
// select paths with less nodes
// 选择节点数较少的路径
vector<vector<Eigen::Vector3d>> filter_raw_paths;
for (int i = min_node_num; i <= max_node_num; ++i) {
bool reach_max = false;
for (int j = 0; j < path_list[i].size(); ++j) {
filter_raw_paths.push_back(raw_paths_[path_list[i][j]]);
if (filter_raw_paths.size() >= max_raw_path2_) {
reach_max = true;
break;
}
}
if (reach_max) break;
}
std::cout << ", raw path num: " << raw_paths_.size() << ", " << filter_raw_paths.size();
raw_paths_ = filter_raw_paths;
return raw_paths_;
}
2.1.4.1TopologyPRM::depthFirstSearch(vector<GraphNode::Ptr >& vis)
得到深度优先算法的路径
void TopologyPRM::depthFirstSearch(vector<GraphNode::Ptr>& vis) {
GraphNode::Ptr cur = vis.back();
for (int i = 0; i < cur->neighbors_.size(); ++i) {
// check reach goal
// 检查是否达到终点
if (cur->neighbors_[i]->id_ == 1) {
// add this path to paths set
// 添加路径点
vector<Eigen::Vector3d> path;
for (int j = 0; j < vis.size(); ++j) {
path.push_back(vis[j]->pos_);
}
path.push_back(cur->neighbors_[i]->pos_);
raw_paths_.push_back(path);
if (raw_paths_.size() >= max_raw_path_) return;
break;
}
}
for (int i = 0; i < cur->neighbors_.size(); ++i) {
// skip reach goal
// 跳过已访问的路径点
if (cur->neighbors_[i]->id_ == 1) continue;
// skip already visited node
bool revisit = false;
// 查询一行中下一个节点
for (int j = 0; j < vis.size(); ++j) {
if (cur->neighbors_[i]->id_ == vis[j]->id_) {
revisit = true;
break;
}
}
if (revisit) continue;
// recursive search
// 保存搜索
vis.push_back(cur->neighbors_[i]);
depthFirstSearch(vis);
if (raw_paths_.size() >= max_raw_path_) return;
vis.pop_back();
}
}
2.1.6 路径裁剪shotcutPaths()
void TopologyPRM::shortcutPaths() {
short_paths_.resize(raw_paths_.size());
if (parallel_shortcut_) {
vector<thread> short_threads;
for (int i = 0; i < raw_paths_.size(); ++i) {
//多线程运行shortcutPath 调用函数 参数
short_threads.push_back(thread(&TopologyPRM::shortcutPath, this, raw_paths_[i], i, 1));
}
for (int i = 0; i < raw_paths_.size(); ++i) {
short_threads[i].join();
}
} else {
for (int i = 0; i < raw_paths_.size(); ++i) shortcutPath(raw_paths_[i], i);
}
}
2.1.6.1 void TopologyPRM::shortcutPath(vectorEigen::Vector3d path, int path_id, int iter_num)
// 路径 路径id
缩短路径。对于原始上的每一个点,都与short_path的最后一个点(初始化时为原始路径的起点)连线并利用lineVisb来衡量可见性。若不可见,则将线上不可见的点往外推至一个新的位置。推的方向与连线垂直并和ESDF梯度方共面。并把新的位置点push_back进short_path中。直到结束循环把终点Push_back进short_path中。最后判断当前short_path是否比原来的路径短,若短,则取代原来的路径,若不是,则保持不变。
void TopologyPRM::shortcutPath(vector<Eigen::Vector3d> path, int path_id, int iter_num) {
vector<Eigen::Vector3d> short_path = path;
vector<Eigen::Vector3d> last_path;
for (int k = 0; k < iter_num; ++k) {
last_path = short_path; //将last_path改为最短的路径
// 将路径稠密化
vector<Eigen::Vector3d> dis_path = discretizePath(short_path);
if (dis_path.size() < 2) {
short_paths_[path_id] = dis_path;
return;
}
/* visibility path shortening */
// 障碍物id 梯度 方向 推力方向
Eigen::Vector3d colli_pt, grad, dir, push_dir;
double dist;
short_path.clear();
short_path.push_back(dis_path.front());
for (int i = 1; i < dis_path.size(); ++i) {
//利用lineVisb来衡量可见性。若不可见,则将线上不可见的点往外推至一个新的位置。
if (lineVisib(short_path.back(), dis_path[i], resolution_, colli_pt, path_id)) continue;
// 梯度
edt_environment_->evaluateEDTWithGrad(colli_pt, -1, dist, grad);
if (grad.norm() > 1e-3) {
grad.normalize();
//障碍物距离
dir = (dis_path[i] - short_path.back()).normalized();
push_dir = grad - grad.dot(dir) * dir; //
// 连线垂直的梯度方向面
push_dir.normalize();
colli_pt = colli_pt + resolution_ * push_dir;
}
// 添加点
short_path.push_back(colli_pt);
}
short_path.push_back(dis_path.back());
/* break if no shortcut */
double len1 = pathLength(last_path);
double len2 = pathLength(short_path);
if (len2 > len1) {
// ROS_WARN("pause shortcut, l1: %lf, l2: %lf, iter: %d", len1, len2, k +
// 1);
short_path = last_path;
break;
}
}
short_paths_[path_id] = short_path;
}
2.1.7 TopologyPRM::pruneEquivalent(vector<vector<Eigen::Vector3d >>& paths)
原有路径集中的每一条路径与添加路径进行比较。
vector<vector<Eigen::Vector3d>> TopologyPRM::pruneEquivalent(vector<vector<Eigen::Vector3d>>& paths) {
vector<vector<Eigen::Vector3d>> pruned_paths;
if (paths.size() < 1) return pruned_paths;
/* ---------- prune topo equivalent path ---------- */
// output: pruned_paths
vector<int> exist_paths_id;
exist_paths_id.push_back(0);
// 与现有路径进行比较
for (int i = 1; i < paths.size(); ++i) {
// compare with exsit paths
bool new_path = true;
for (int j = 0; j < exist_paths_id.size(); ++j) {
// compare with one path
bool same_topo = sameTopoPath(paths[i], paths[exist_paths_id[j]], 0.0);
// 添加路径与之前路径一样,则为false
if (same_topo) {
new_path = false;
break;
}
}
if (new_path) {
exist_paths_id.push_back(i);
}
}
// save pruned paths
for (int i = 0; i < exist_paths_id.size(); ++i) {
pruned_paths.push_back(paths[exist_paths_id[i]]);
}
std::cout << ", pruned path num: " << pruned_paths.size();
return pruned_paths;
}
2.1.8 sameTopoPath()
两条路径通过稠密处理,比较是否相同,如果相同则输出false,否则为true
bool TopologyPRM::sameTopoPath(const vector<Eigen::Vector3d>& path1,
const vector<Eigen::Vector3d>& path2, double thresh) {
// calc the length
double len1 = pathLength(path1);
double len2 = pathLength(path2);
double max_len = max(len1, len2);
int pt_num = ceil(max_len / resolution_);
// std::cout << "pt num: " << pt_num << std::endl;
// 对路径进行稠密处理
vector<Eigen::Vector3d> pts1 = discretizePath(path1, pt_num);
vector<Eigen::Vector3d> pts2 = discretizePath(path2, pt_num);
Eigen::Vector3d pc;
for (int i = 0; i < pt_num; ++i) {
if (!lineVisib(pts1[i], pts2[i], thresh, pc)) {
return false;
}
}
return true;
}
2.1.9 7. selectShortPaths()
将路径集中的其它路径与最短路径进行比较,产生一个长度比值,该比值小于一定阈值时,将该路径进行放入新的路径集中保留,直到达到最大路径数目或循环结束。
vector<vector<Eigen::Vector3d>> TopologyPRM::selectShortPaths(vector<vector<Eigen::Vector3d>>& paths,
int step) {
/* ---------- only reserve top short path ---------- */
vector<vector<Eigen::Vector3d>> short_paths;
vector<Eigen::Vector3d> short_path; // 最短路径点
double min_len; //最短路径长度
for (int i = 0; i < reserve_num_ && paths.size() > 0; ++i) {
int path_id = shortestPath(paths);
if (i == 0) {
short_paths.push_back(paths[path_id]);
min_len = pathLength(paths[path_id]);
paths.erase(paths.begin() + path_id);
} else {
// 路径长度比较
double rat = pathLength(paths[path_id]) / min_len;
if (rat < ratio_to_short_) {
short_paths.push_back(paths[path_id]);
paths.erase(paths.begin() + path_id);
} else {
break;
}
}
}
std::cout << ", select path num: " << short_paths.size();
/* ---------- merge with start and end segment ---------- */
for (int i = 0; i < short_paths.size(); ++i) {
short_paths[i].insert(short_paths[i].begin(), start_pts_.begin(), start_pts_.end());
short_paths[i].insert(short_paths[i].end(), end_pts_.begin(), end_pts_.end());
}
for (int i = 0; i < short_paths.size(); ++i) {
shortcutPath(short_paths[i], i, 5);
short_paths[i] = short_paths_[i];
}
//并再做一遍shortcutPath和pruneEquivalent得到最终路径。
short_paths = pruneEquivalent(short_paths);
return short_paths;
}