static bool
FeedWayPointLine(WayPointList &way_points, RasterTerrain &terrain,
const TCHAR *line)
{
if (TempString[0] == '\0' ||
TempString[0] == 0x1a || // dos end of file
_tcsstr(TempString, TEXT("**")) == TempString || // Look For Comment
_tcsstr(TempString, TEXT("*")) == TempString) // Look For SeeYou Comment
/* nothing was parsed, return without error condition */
return true;
WAYPOINT *new_waypoint = way_points.append();
if (new_waypoint == NULL)
return false; // failed to allocate
new_waypoint->Details = NULL;
if (!ParseWayPointString(new_waypoint, TempString, terrain)) {
way_points.pop();
return false;
}
if (!WaypointInTerrainRange(new_waypoint, terrain)) {
way_points.pop();
return true;
}
return true;
}
void PathPlanner::slotComputePath(const QVector3D& vehiclePosition, const WayPointList& wayPointList)
{
Q_ASSERT(!wayPointList.isEmpty());
if(!mIsInitialized) initialize();
WayPointList wayPointsWithHighInformationGain = wayPointList;
wayPointsWithHighInformationGain.prepend(WayPoint(vehiclePosition, 0));
qDebug() << __PRETTY_FUNCTION__ << "computing path for waypointlist" << wayPointsWithHighInformationGain.toString();
// mDeviceOccupancyGrid points to device memory filled with grid-values of quint8.
// After fillOccupancyGrid(), empty cells contain a 0, occupied cells contain a 254/255.
// The cell containing the start is set to 1. Then, one thread is launched per cell,
// looking into the neighboring cells. If a neighboring cell (26 3d-neighbors) contains
// a value other than 0 or 254/255, the current cell is set to min(neighbor)+1.
// This is executed often, so that all cells reachable from start get filled with the
// distance TO start
alignPathPlannerGridToColliderCloud();
QTime t;
t.start();
WayPointList computedPath;
copyParametersToGpu(&mParametersPathPlanner);
const bool haveToMapGridOccupancyTemplate = checkAndMapGridOccupancy(mCudaVboResourceGridOccupancyTemplate);
const bool haveToMapGridOccupancyPathPlanner = checkAndMapGridOccupancy(mCudaVboResourceGridPathPlanner);
// Only re-creates the occupancy grid if the collider cloud's content changed
populateOccupancyGrid();
// The first path leads from vehicle position to first waypoint. Clear the occupancy grid above the vehicle!
// This is currently necessary, because we also scan bernd and the fishing rod, occupying our own cells.
clearOccupancyGridAboveVehiclePosition(
mGridOccupancyPathPanner,
vehiclePosition.x(),
vehiclePosition.y(),
vehiclePosition.z(),
&mCudaStream);
// We have freed the occupancy grid a little to make path planning easier/possible. Restore it to the real grid asap.
mRepopulateOccupanccyGrid = true;
// Make some room for waypoints in host memory. The first float4's x=y=z=w will store just the number of waypoints
float* waypointsHost = new float[mMaxWaypoints * 4];
// Find a path between every pair of waypoints
quint32 indexWayPointStart = 0;
quint32 indexWayPointGoal = 1;
quint32 pathNumber = 0;
do
{
mParametersPathPlanner.start = CudaHelper::convert(wayPointsWithHighInformationGain.at(indexWayPointStart));
mParametersPathPlanner.goal = CudaHelper::convert(wayPointsWithHighInformationGain.at(indexWayPointGoal));
qDebug() << __PRETTY_FUNCTION__ << "now computing path from" << indexWayPointStart << ":" << wayPointsWithHighInformationGain.at(indexWayPointStart).toString() << "to" << indexWayPointGoal << ":" << wayPointsWithHighInformationGain.at(indexWayPointGoal).toString();
copyParametersToGpu(&mParametersPathPlanner);
// Copy the populated and dilated occupancy grid into the PathFinder's domain
cudaSafeCall(cudaMemcpy(
mGridOccupancyPathPanner,
mGridOccupancyTemplate,
mParametersPathPlanner.grid.getCellCount(),
cudaMemcpyDeviceToDevice));
// Now start path planning.
markStartCell(mGridOccupancyPathPanner, &mCudaStream);
growGrid(
mGridOccupancyPathPanner,
&mParametersPathPlanner,
&mCudaStream);
// We must set the waypoints-array on the device to a special value because
// a bug can lead to some waypoints not being written. This is ok
// as long as we can detect this. When memsetting with 0, we can,
// otherwise we'd find a waypoint from a previous run and couldn't
// detect this incidence.
cudaSafeCall(cudaMemset(
(void*)mDeviceWaypoints,
255, // interpreted as float, should be NaN!
4 * mMaxWaypoints * sizeof(float)));
retrievePath(
mGridOccupancyPathPanner,
mDeviceWaypoints,
&mCudaStream);
cudaSafeCall(cudaMemcpy(
(void*)waypointsHost,
(void*)mDeviceWaypoints,
4 * mMaxWaypoints * sizeof(float),
cudaMemcpyDeviceToHost));
if(fabs(waypointsHost[0]) < 0.001 && fabs(waypointsHost[1]) < 0.001 && fabs(waypointsHost[2]) < 0.001)
{
qDebug() << __PRETTY_FUNCTION__ << "found NO path from" << indexWayPointStart << ":" << wayPointsWithHighInformationGain.at(indexWayPointStart).toString() << "to" << indexWayPointGoal << ":" << wayPointsWithHighInformationGain.at(indexWayPointGoal).toString();
// When no path was found, we try to find a path to the next waypoint, skipping the problematic one.
indexWayPointGoal++;
}
else
{
qDebug() << __PRETTY_FUNCTION__ << "found path with" << waypointsHost[0] << "waypoints";
// The first waypoint isn't one, it only contains the number of waypoints
for(int i=1; i<=waypointsHost[0]; i++)
{
// workaround for the no-waypoint-bug, which will show values of NaN/NaN/NaN/NaN due to the 255-memset above
if(isnan(waypointsHost[4*i+3]))
{
qDebug() << __PRETTY_FUNCTION__ << "ignoring waypoint that was skpped in retrievePath due to bug in growGrid.";
continue;
}
WayPoint newWayPoint;
if(i == 1)
{
newWayPoint = wayPointsWithHighInformationGain.at(indexWayPointStart);
}
else if(i == (int)waypointsHost[0])
{
newWayPoint = wayPointsWithHighInformationGain.at(indexWayPointGoal);
}
else
{
newWayPoint = WayPoint(QVector3D(waypointsHost[4*i+0], waypointsHost[4*i+1], waypointsHost[4*i+2]), 0);
}
// Append all points of the first path, and then only starting at the second point of the following paths.
// Otherwise, we have the end of path A and the beginning of path B in the list, although they're the same.
if(pathNumber == 0 || i > 1)
{
computedPath.append(newWayPoint);
}
}
qDebug() << __PRETTY_FUNCTION__ << "found path between" << wayPointsWithHighInformationGain.at(indexWayPointStart).toString() << "and" << wayPointsWithHighInformationGain.at(indexWayPointGoal).toString() << ":" << computedPath.toString();
pathNumber++;
indexWayPointStart = indexWayPointGoal;
indexWayPointGoal++;
}
} while(indexWayPointGoal < wayPointsWithHighInformationGain.size());
delete waypointsHost;
if(haveToMapGridOccupancyTemplate) checkAndUnmapGridOccupancy(mCudaVboResourceGridOccupancyTemplate);
// cudaGraphicsUnmapResources(1, &mCudaVboResourceGridPathFinder, 0);
if(haveToMapGridOccupancyPathPlanner) checkAndUnmapGridOccupancy(mCudaVboResourceGridPathPlanner);
// cudaGraphicsUnmapResources(1, &mCudaVboResourceGridOccupancy, 0);
emit path(computedPath.list(), WayPointListSource::WayPointListSourceFlightPlanner);
qDebug() << __PRETTY_FUNCTION__ << "took" << t.elapsed() << "ms.";
}