std::vector<std::vector<double> > rediscretizePath(
const std::vector<std::vector<double> > &path, double step_size) {
std::vector<std::vector<double> > new_path;
if (path.size() > 0) {
double current_dist = 0;
double move_dist = 0;
bool on_initial_line = true;
new_path.push_back(path[0]);
unsigned int next_point_index = 1;
while (next_point_index < path.size()) {
on_initial_line = true;
current_dist = getLineDistance(new_path[new_path.size() - 1],
path[next_point_index]);
while (current_dist < step_size) {
if (next_point_index == path.size() - 1) {
new_path.push_back(path[path.size() - 1]);
return new_path;
}
current_dist +=
getLineDistance(path[next_point_index], path[next_point_index + 1]);
next_point_index++;
on_initial_line = false;
}
if (!on_initial_line) {
move_dist = step_size -
(current_dist - (getLineDistance(path[next_point_index - 1],
path[next_point_index])));
new_path.push_back(getPointBetween(path[next_point_index - 1],
path[next_point_index], move_dist));
} else {
new_path.push_back(getPointBetween(new_path[new_path.size() - 1],
path[next_point_index], step_size));
}
}
ROS_INFO("Collision utils: Got a new path of %i points",
(unsigned int)new_path.size());
} else {
ROS_INFO("Collision utils: Rediscretize path got a path with no points");
}
return new_path;
}
void GlobalPlanner::increaseResolution(double minDist, double minRot, double minTime) {
std::vector<WaypointWithTime> newWaypoints;
for (int i=0; i < waypoints.size()-1; i+=2) {
newWaypoints.push_back(waypoints[i]);
}
newWaypoints.push_back(waypoints[waypoints.size()-1]);
waypoints = newWaypoints;
newWaypoints.resize(0);
// newWaypoints.push_back(waypoints[0]);
ROS_INFO("0: %f, 1: %f", waypoints[0].yaw, waypoints[1].yaw);
for (int i=1; i < waypoints.size(); ++i) {
double dist = distance(waypoints[i].position, waypoints[i-1].position);
double diffYaw = std::abs(waypoints[i].yaw - waypoints[i-1].yaw);
int factor = (int) std::max(dist / minDist, (diffYaw / minRot)) + 1;
for (int j=1; j <= factor; ++j) {
newWaypoints.push_back(getPointBetween(waypoints[i-1], waypoints[i], j, factor, minTime));
}
}
waypoints = newWaypoints;
}
