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; }