[源码阅读]SMUG Planner
A Safe Multi-Goal Planner for Mobile Robots in Challenging Environments
动态规划算法实现函数
1.从起点开始 连接 起点 到 最后一个TOI的所有 POI 并计算路径代价，
2.然后转入到 上一个 TOI （倒数第二个），计算新的 TOI 到刚刚的TOI 内所有 POI 之间的路径 以及代价，
3.计算 当前TOI 每一个POI j 到起点时经过的上一个 TOI 的第k 个POI ，记录下路径代价最小时候的 j 对应的k ，放入一个二维查找表，
4.这个二维查找表的行对应的是一个TOI ，列对应的是 这个 TOI 里第 j 个POI 想连接到起点时候，经过的上一个 TOI 的第k 个 POI ，这个k 使路径代价最小。
5.重复二三四，即得到最终的查找表
6.最后根据查找表得到访问顺序的路径点POI们
7.首先计算起点到第一个TOI 的所有的POI的路径以及代价，找到路径代价最小的对应的第k个POI;
8.因为已经直到了终点也就是起点，到达第一个TOI的最短路径对应的j k POI了，也就是那个查找表，所以根据第7步找到的k 在查找表回找就可以找到起点经过k POI 到终点（起点）的一个路径了，代价最小。
9.再封闭路径。
PlannerStatus Planner::dynamicProgramming(const std::vector>>& goal_sets, // TOIS 和 POIS 每一个 vector 的第一个是 TOI
std::vector> points) { // 所有的 POI
int n_points = points.size(); // POIs num
int n_set = goal_sets.size(); // TOIS num
og::PathGeometricPtr** path_matrix; // 符合几何约束的路径矩阵
path_lengths_ = new double*[n_points];
path_matrix = new og::PathGeometricPtr*[n_points];
for (int i = 0; i < n_points; i++) {
path_lengths_[i] = new double[n_points];
path_matrix[i] = new og::PathGeometricPtr[n_points];
}
// dynamic programming and plannning during steps
auto center_sequence_temp = center_sequence_; // center_sequence_ TSP 求解的 TOI 的顺序 第一个应该是 start
std::vector cost_vector; // 起点 start 到 每一个 POI 的最短路径 代价
// the last one goal to start
int cur_center = center_sequence_temp.back(); //TODO 最后访问的 TOI 还是 第一个访问的 TOI
center_sequence_temp.pop_back();
std::vector> next_point_table(n_set - 1);
int next_point = n_points - 1; // start point
for (int i = 0; i < goal_sets[cur_center].size() - 1; i++) { // 应该不包括 TOI
if (stop_planning_) {
return PREEMPTED;
}
int cur_point = getIdxInVector(cur_center, i); // 每一个TOI 的 POI
// plan p2p
PlannerStatus solved = singleGoalPlan(points[cur_point], points[next_point]);
og::PathGeometric path(ss_->getSpaceInformation()); // plan 两个点(每一个到开始点)之间的路径
std::shared_ptr path_ptr;
switch (solved) {
case PlannerStatus::SOLVED:
path = getSingleSolutionPath();
path_ptr = std::make_shared(path);
path_matrix[cur_point][next_point] = path_ptr; // 记录每一个点到起点的路径的矩阵
path_lengths_[cur_point][next_point] = path_ptr->length(); // 记录每一个点到起点的路径的长度
break;
default:
path_lengths_[cur_point][next_point] = INFINITY;
break;
}
cost_vector.push_back(path_lengths_[cur_point][next_point]); // 记录了当前TOI的每一个POI点到 起点 的路径长度 cost
}
int next_center = cur_center; // 当前TOI 变为 下一个中心TOI
for (int i = 1; i < n_set; i++) {
cur_center = center_sequence_temp.back(); //TODO 最后一个TOI的上一个
center_sequence_temp.pop_back();
std::vector cost_vector_new;
std::vector next_point_list;
for (int j = 0; j < goal_sets[cur_center].size() - 1; j++) { // 这个 for 计算了当前中心的第j 个点经过上一个中心的第k个POI 是到达起点的最短路径代价和k
if (stop_planning_) {
return PREEMPTED;
}
int cur_point = getIdxInVector(cur_center, j); // 新的当前点
double best = INFINITY;
int best_idx;
for (int k = 0; k < goal_sets[next_center].size() - 1; k++) { // 这个 for 找到了 cur center 的第j个POI 到 起点 经过的 上一个 中心的 第 k 个 POI 这时候路径代价最小
int next_point = getIdxInVector(next_center, k);
// plan p2p
PlannerStatus solved = singleGoalPlan(points[cur_point], points[next_point]);
og::PathGeometric path(ss_->getSpaceInformation());
std::shared_ptr path_ptr;
switch (solved) {
case PlannerStatus::SOLVED:
path = getSingleSolutionPath();
path_ptr = std::make_shared(path);
path_matrix[cur_point][next_point] = path_ptr;
path_lengths_[cur_point][next_point] = path_ptr->length();
break;
default:
path_lengths_[cur_point][next_point] = INFINITY;
break;
}
double cur_cost = path_lengths_[cur_point][next_point] + cost_vector[k];
if (cur_cost < best) {
best = cur_cost;
best_idx = k;
}
}
cost_vector_new.push_back(best);
next_point_list.push_back(best_idx); // 只保留了 每一个 j 到 k 到 起点的路径代价最小时候的，k 和最小路径代价
}
next_point_table[n_set - 1 - i] = next_point_list; // 每一个j 到起点对应的最小路径代价的k 的列表 放入一个二维数组
next_center = cur_center; // 更新
cost_vector.clear();
cost_vector.insert(cost_vector.end(), cost_vector_new.begin(), cost_vector_new.end()); // 每一个j 到起点对应的最小路径代价的k 的 路径代价 放入一个二维数组
}
// first step from start
int cur_point = n_points - 1;
double best = INFINITY;
int best_idx;
for (int k = 0; k < goal_sets[next_center].size() - 1; k++) { // 第一个 TOI
if (stop_planning_) {
return PREEMPTED;
}
int next_point = getIdxInVector(next_center, k);
// plan p2p
PlannerStatus solved = singleGoalPlan(points[cur_point], points[next_point]); // 起点 到 第一个 TOI
og::PathGeometric path(ss_->getSpaceInformation());
std::shared_ptr path_ptr;
switch (solved) {
case PlannerStatus::SOLVED:
path = getSingleSolutionPath();
path_ptr = std::make_shared(path);
path_matrix[cur_point][next_point] = path_ptr;
path_lengths_[cur_point][next_point] = path_ptr->length();
break;
default:
path_lengths_[cur_point][next_point] = INFINITY;
break;
}
double cur_cost = path_lengths_[cur_point][next_point] + cost_vector[k];
if (cur_cost < best) {
best = cur_cost;
best_idx = k;
}
} // 起点 到 第一个 TOI 路径最短时 对应的 第 k 个 POI
// the first goal is best_idx
int cur_idx = best_idx;
std::vector dp_sequence{};
for (int i = 0; i < n_set; i++) {
if (i > 0) cur_idx = next_point_table[i - 1][cur_idx]; // 后面这个cur_idx 就是 best_idx 就是 第一个TOI 的k 相对于下一个 TOI 来说 就是 next_point_table 里面的 j
int cur_center = center_sequence_[i]; // next_point_table 记录了[第i个TOI中的][第j个POI]连接的下一个TOI的最佳的k
int next_point = getIdxInVector(cur_center, cur_idx);
dp_sequence.push_back(next_point); // 起点到终点的第 k 个 POI
}
int cur_index = n_points - 1; // 起点
for (int next_index : dp_sequence) { // 将 dp 序列转化为 路径
og::PathGeometricPtr path_ptr_temp = path_matrix[cur_index][next_index];
og::PathGeometric path_temp = *path_ptr_temp;
path_buffer_ompl_.push_back(path_ptr_temp);
complete_path_dp_->append(path_temp);
cur_index = next_index;
}
// close the path
og::PathGeometricPtr path_ptr_temp = path_matrix[cur_index][n_points - 1];
og::PathGeometric path_temp = *path_ptr_temp;
path_buffer_ompl_.push_back(path_ptr_temp);
complete_path_dp_->append(path_temp); // 最后一个 POI 连接到起点
// free memory
for (int i = 0; i < n_points; i++) {
delete[] path_lengths_[i];
delete[] path_matrix[i];
}
delete[] path_lengths_;
delete[] path_matrix;
return PlannerStatus::SOLVED;
}
PlannerStatus Planner::iterativeDP(const std::vector>>& goal_sets,
std::vector> points) {
ompl::time::point start_misc5 = ompl::time::now();
int n_points = points.size(); // POIs num
int n_set = goal_sets.size(); // TOIs num
og::PathGeometricPtr** path_matrix; // 记录POI 之间的路径
path_lengths_ = new double*[n_points]; // 记录POI 之间的路径代价
path_matrix = new og::PathGeometricPtr*[n_points];
for (int i = 0; i < n_points; i++) {
path_lengths_[i] = new double[n_points];
path_matrix[i] = new og::PathGeometricPtr[n_points];
}
auto center_sequence_temp = center_sequence_;
double best_cost = INFINITY;
std::vector best_point_in_set; // 最好的POI
int n_planned_paths = 0; // 规划的路径数量
double total_dp_time = 0; // 计算时间
while (true) {
// start dp
ompl::time::point dp_start_time = ompl::time::now();
auto center_sequence_temp = center_sequence_;
std::vector cost_vector;
// the last one goal to start
int cur_center = center_sequence_temp.back();
center_sequence_temp.pop_back();
std::vector> next_point_table(n_set - 1); // j POI 连接 k POI 的查找表
int next_point = n_points - 1; // start
for (int i = 0; i < goal_sets[cur_center].size() - 1; i++) {
int cur_point = getIdxInVector(cur_center, i);
if (!path_matrix[cur_point][next_point]) {
// this means obstacle free path is not planned yet, use euclidean cost instead
cost_vector.push_back(points[cur_point].distance(points[next_point]));
} else {
// this means obstacle free path is planned, use path cost
cost_vector.push_back(path_lengths_[cur_point][next_point]);
}
}
int next_center = cur_center;
for (int i = 1; i < n_set; i++) {
cur_center = center_sequence_temp.back();
center_sequence_temp.pop_back();
std::vector cost_vector_new;
std::vector next_point_list;
for (int j = 0; j < goal_sets[cur_center].size() - 1; j++) {
int cur_point = getIdxInVector(cur_center, j);
double best = INFINITY;
int best_idx;
for (int k = 0; k < goal_sets[next_center].size() - 1; k++) {
int next_point = getIdxInVector(next_center, k);
double cur_cost = 0;
if (!path_matrix[cur_point][next_point]) {
// this means obstacle free path is not planned yet, use euclidean cost instead
cur_cost = points[cur_point].distance(points[next_point]) + cost_vector[k];
} else {
// this means obstacle free path is planned, use path cost
cur_cost = path_lengths_[cur_point][next_point] + cost_vector[k];
}
if (cur_cost < best) {
best = cur_cost;
best_idx = k;
}
}
cost_vector_new.push_back(best);
next_point_list.push_back(best_idx);
}
next_point_table[n_set - 1 - i] = next_point_list;
next_center = cur_center;
cost_vector.clear();
cost_vector.insert(cost_vector.end(), cost_vector_new.begin(), cost_vector_new.end());
} // 同 DP 计算两两POI 之间的 COST，以及记录 j k 对应关系
// first step from start
int cur_point = n_points - 1; // start
double best = INFINITY;
int best_idx;
for (int k = 0; k < goal_sets[next_center].size() - 1; k++) {
double cur_cost = 0;
int next_point = getIdxInVector(next_center, k);
if (!path_matrix[cur_point][next_point]) {
// this means obstacle free path is not planned yet, use euclidean cost instead
cur_cost = points[cur_point].distance(points[next_point]) + cost_vector[k];
} else {
// this means obstacle free path is planned, use path cost
cur_cost = path_lengths_[cur_point][next_point] + cost_vector[k];
}
if (cur_cost < best) {
best = cur_cost;
best_idx = k;
}
}
// the first goal is best_idx
int cur_idx = best_idx;
std::vector point_in_set(n_set);
for (int i = 0; i < n_set; i++) {
if (i > 0) cur_idx = next_point_table[i - 1][cur_idx];
int cur_center = center_sequence_[i]; // 该去参观的第 i 个 TOI
point_in_set[cur_center] = getIdxInVector(cur_center, cur_idx); // 参观第 i 个 TOI 的时候，应该取得那一个 POI
} // 得到顺序访问的 k POI
double dp_time = ompl::time::seconds(ompl::time::now() - dp_start_time);
total_dp_time += dp_time;
// TODO IDP 不同于DP的地方
// 上面得到的是路径代价最小时候的访问POI 序列（包括，规划了路径和不规划的路径的POI组合）
// 下面就可以通过判断有没有规划路径，没有规划就给他规划一条路径，这样可以达到这个从起点到终点的经过的所有POI 之间都规划了路径，而这个cost
// 记录了这个已经规划了所有经过的POI 之间的路径的整个路径代价值
// while 的下一次循环又会找到一个访问组合POI序列，下面又会给这个组合的所有POI之间进行路径规划，并记录cost.
// 通过保留最小的 cost 的路径（这个路径的POI组合）就可以得到最小路径代价的访问POI 路径了。
// 直到没有需要规划的POI 之间的路径为止。
// obstacle free path planning according to point_in_set
std::vector path_before;
std::vector is_planned_one_iteration;
og::PathGeometric path_after = og::PathGeometric(ss_->getSpaceInformation());
bool need_to_plan = false;
double cost = 0.0;
int last_point = n_points - 1; // start
for (int i = 0; i <= n_set; i++) {
int cur_point;
if (i == n_set) {
cur_point = n_points - 1; // 又回到了起点
} else {
int cur_set = center_sequence_[i]; // 要参观的第 i 个 TOI
cur_point = point_in_set[cur_set]; // 要参观的第 i 个 TOI 的第 k 个 POI
}
// check if path is already planned
if (!path_matrix[last_point][cur_point]) { // 如果两个POI 之间没有规划过路径
need_to_plan = true; // 需要规划路径
n_planned_paths++; // 规划路径计数++
PlannerStatus solved = singleGoalPlan(points[last_point], points[cur_point]);
og::PathGeometric path(ss_->getSpaceInformation());
std::shared_ptr path_ptr;
switch (solved) {
case PlannerStatus::SOLVED:
path = getSingleSolutionPath();
path_ptr = std::make_shared(path);
path_matrix[last_point][cur_point] = path_ptr;
path_lengths_[last_point][cur_point] = path_ptr->length();
break;
default:
path_lengths_[last_point][cur_point] = INFINITY;
break;
}
path_after.append(path); // 填入规划的路径
is_planned_one_iteration.push_back(false); // 没有被规划过
path_before.push_back(
og::PathGeometric(ss_->getSpaceInformation(), points[last_point].get(), points[cur_point].get())); // 填入由两个状态确定的一个路径实例
} else {
path_after.append(*path_matrix[last_point][cur_point]);
is_planned_one_iteration.push_back(true); // 之前被规划过
path_before.push_back(*path_matrix[last_point][cur_point]);
}
cost += path_lengths_[last_point][cur_point]; // 此时给一个最佳POI 组合规划了一个完整的路径，并记录了这个 路径 cost
last_point = cur_point;
}
idp_paths_before_plan_.push_back(path_before);
idp_paths_after_plan_.push_back(path_after);
is_planned_.push_back(is_planned_one_iteration);
if (cost < best_cost) { // 时刻记录 cost 最小的那个路径
best_cost = cost;
best_point_in_set = point_in_set;
}
if (!need_to_plan) { // 没有 POI 之间存在没有规划的路径的情况了
break;
}
}
// glue solution
int cur_index = n_points - 1;
for (int next_set : center_sequence_) {
int next_index = best_point_in_set[next_set];
og::PathGeometricPtr path_ptr_temp = path_matrix[cur_index][next_index];
og::PathGeometric path_temp = *path_ptr_temp;
path_buffer_ompl_.push_back(path_ptr_temp);
complete_path_idp_->append(path_temp);
cur_index = next_index;
}
// close the path
og::PathGeometricPtr path_ptr_temp = path_matrix[cur_index][n_points - 1];
og::PathGeometric path_temp = *path_ptr_temp;
path_buffer_ompl_.push_back(path_ptr_temp);
complete_path_idp_->append(path_temp);
// free memory
for (int i = 0; i < n_points; i++) {
delete[] path_lengths_[i];
delete[] path_matrix[i];
}
delete[] path_lengths_;
delete[] path_matrix;
return PlannerStatus::SOLVED;
}