navigation snippet

gistfile1.cpp

// simulate sensing the cells
for (int i = 0; i < (int)sensecells.size(); i++) {
	int x = CONTXY2DISC(startx, cellsize_m) + sensecells.at(i).x;
	int y = CONTXY2DISC(starty, cellsize_m) + sensecells.at(i).y;
	// ignore if outside the map
	if (x < 0 || x >= size_x || y < 0 || y >= size_y) {
		continue;
	}

	int index = x + y * size_x;
	unsigned char truecost = trueenvironment_navxythetalat.GetMapCost(x, y);
	// update the cell if we haven't seen it before
	if (map[index] != truecost) {
		map[index] = truecost;
		environment_navxythetalat.UpdateCost(x, y, map[index]);
		printf("setting cost[%d][%d] to %d\n", x, y, map[index]);
		bChanges = true;
		// store the changed cells
		nav2dcell.x = x;
		nav2dcell.y = y;
		changedcellsV.push_back(nav2dcell);
	}
}
	double TimeStarted = clock();
	// if necessary notify the planner of changes to costmap
if (bChanges) {
	if (dynamic_cast<ARAPlanner*>(planner) != NULL) {
		((ARAPlanner*)planner)->costs_changed(); //use by ARA* planner (non-incremental)
	}
	else if (dynamic_cast<ADPlanner*>(planner) != NULL) {
		// get the affected states
		environment_navxythetalat.GetPredsofChangedEdges(&changedcellsV, &preds_of_changededgesIDV);
		// let know the incremental planner about them
		((ADPlanner*)planner)->update_preds_of_changededges(&preds_of_changededgesIDV); //use by AD* planner (incremental)
		printf("%d states were affected\n", (int)preds_of_changededgesIDV.size());
	}
}