/**
* Überprüft auf eine Kollision
* Return: diff = neues Struct direkt zum Ziel
* return: 0 wenn keine Kollision, 1 sonst
*/
uint8_t calc_Offroad(gps_reducedData_t* diff, gps_reducedData_t* own, gps_reducedData_t* target){
int16_t xCollision, yCollision;
tree_init(CoordinatesToMap(own->x), CoordinatesToMap(own->y));
if (calc_reachability(&xCollision, &yCollision,CoordinatesToMap(own->x), CoordinatesToMap(own->y),CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){
setCoordinates(diff, target->cam_id, target->tag_id, target->x, target->y, target->angle, target->isWorld);
return 0;
}
else{
scan_Obstacles(xCollision, yCollision, own, target, CoordinatesToMap(own->x), CoordinatesToMap(own->y)); //übergibt hier die Kollisionskoordinaten
}
return 1;
}
/** Search for a path between two nodes.
* This function executes an A* search to find an (optimal) path
* from node @p from to node @p to.
* @param from node to search from
* @param to goal node
* @param estimate_func function to estimate the cost from any node to the goal.
* Note that the estimate function must be admissible for optimal A* search. That
* means that for no query may the calculated estimate be higher than the actual
* cost.
* @param cost_func function to calculate the cost from a node to another adjacent
* node. Note that the cost function is directly related to the estimate function.
* For example, the cost can be calculated in terms of distance between nodes, or in
* time that it takes to travel from one node to the other. The estimate function must
* match the cost function to be admissible.
* @param use_constraints true to respect constraints imposed by the constraint
* repository, false to ignore the repository searching as if there were no
* constraints whatsoever.
* @param compute_constraints if true re-compute constraints, otherwise use constraints
* as-is, for example if they have been computed before to check for changes.
* @return ordered vector of nodes which denote a path from @p from to @p to.
* Note that the vector is empty if no path could be found (i.e. there is non
* or it was prohibited when using constraints.
*/
fawkes::NavGraphPath
NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
navgraph::EstimateFunction estimate_func,
navgraph::CostFunction cost_func,
bool use_constraints, bool compute_constraints)
{
if (! reachability_calced_) calc_reachability();
AStar astar;
std::vector<AStarState *> a_star_solution;
if (use_constraints) {
constraint_repo_.lock();
if (compute_constraints && constraint_repo_->has_constraints()) {
constraint_repo_->compute();
}
NavGraphSearchState *initial_state =
new NavGraphSearchState(from, to, this, estimate_func, cost_func,
*constraint_repo_);
a_star_solution = astar.solve(initial_state);
constraint_repo_.unlock();
} else {
NavGraphSearchState *initial_state =
new NavGraphSearchState(from, to, this, estimate_func, cost_func);
a_star_solution = astar.solve(initial_state);
}
std::vector<fawkes::NavGraphNode> path(a_star_solution.size());
NavGraphSearchState *solstate;
for (unsigned int i = 0; i < a_star_solution.size(); ++i ) {
solstate = dynamic_cast<NavGraphSearchState *>(a_star_solution[i]);
path[i] = solstate->node();
}
float cost =
(! a_star_solution.empty())
? a_star_solution[a_star_solution.size() - 1]->total_estimated_cost
: -1;
return NavGraphPath(this, path, cost);
}
/**
* Findet die 2 (Rand)Koordinaten die das Hindernis aus Sicht des Autos abgrenzen. Findet von dort einen Weg zum Ziel
*/
void scan_Obstacles(int8_t xCollision, int8_t yCollision, gps_reducedData_t* own, gps_reducedData_t* target, int8_t x_old, int8_t y_old){
int16_t x_freeL, y_freeL, x_freeR, y_freeR;
int8_t xleft = -1, yleft = -1, xright = -1, yright = -1;
//positiver Pfad
//Annäherung an Hindernis von unten
if(get_coord_obstacle(xCollision, yCollision-1) == 0 && (get_coord_obstacle(xCollision-1, yCollision) == 1 || get_coord_obstacle(xCollision+1, yCollision) == 1)){
move_AroundHorizontal( &xleft, &xright, &yleft, &yright, xCollision, yCollision, 1);
if (xleft >= 0 && yleft >= 0){
if (calc_reachability(&x_freeL, &y_freeL,CoordinatesToMap(own->x), CoordinatesToMap(own->y), xleft, yleft) == 1){
x_freeL = xCollision;
y_freeL = yCollision; //von Hinderniskante zu Auto ist kein direkter Weg möglich. Nehme zwischeschritt
}
}
if (xright >= 0 && yright >= 0){
if (calc_reachability(&x_freeR, &y_freeR, CoordinatesToMap(own->x), CoordinatesToMap(own->y), xright, yright) == 1){
x_freeR = xCollision;
y_freeR = yCollision;
}
}
}
else
//Annäherung an Hindernis von oben
if(get_coord_obstacle(xCollision, yCollision-1) != 0 && get_coord_obstacle(xCollision,yCollision+1) == 0 && (get_coord_obstacle(xCollision-1, yCollision) == 1 || get_coord_obstacle(xCollision+1, yCollision) == 1)){
move_AroundHorizontal( &xleft, &xright, &yleft, &yright, xCollision, yCollision, -1); // -1 = Hindernis von oben
if (xleft >= 0 && yleft >= 0){
if (calc_reachability(&x_freeL, &y_freeL,CoordinatesToMap(own->x), CoordinatesToMap(own->y), xleft, yleft) == 1){
x_freeL = xCollision;
y_freeL = yCollision; //von Hinderniskante zu Auto ist kein direkter Weg möglich. Nehme zwischeschritt
}
}
if (xright >= 0 && yright >= 0){
if (calc_reachability(&x_freeR, &y_freeR, CoordinatesToMap(own->x), CoordinatesToMap(own->y), xright, yright) == 1){
x_freeR = xCollision;
y_freeR = yCollision;
}
}
}
else
//Sonderfall senkrechtes Hindernis
if((get_coord_obstacle(xCollision, yCollision-1) != 0 && get_coord_obstacle(xCollision,yCollision+1) == 1) || isEcke(xCollision,yCollision) == 1 ){
//Überpürung ob Weg Rechts oder Links vom Hinderniss
if (get_coord_obstacle(xCollision-1, yCollision) == 0 || get_coord_obstacle(xCollision-1, yCollision-1) == 0 || get_coord_obstacle(xCollision-1, yCollision +1) == 0){
move_AroundVertical( &xleft, &xright, &yleft, &yright, xCollision, yCollision, 1); //Links
x_freeL = 1;
}
else if (get_coord_obstacle(xCollision+1, yCollision) == 0 || get_coord_obstacle(xCollision+1, yCollision-1) == 0 || get_coord_obstacle(xCollision+1, yCollision +1) == 0)
move_AroundVertical( &xleft, &xright, &yleft, &yright, xCollision, yCollision, -1);
if (xleft >= 0 && yleft >= 0){
if (calc_reachability(&x_freeL, &y_freeL,CoordinatesToMap(own->x), CoordinatesToMap(own->y), xleft, yleft) == 1){
x_freeL = xCollision;
y_freeL = yCollision;
}
}
if (xright >= 0 && yright >= 0){
if (calc_reachability(&x_freeR, &y_freeR, CoordinatesToMap(own->x), CoordinatesToMap(own->y), xright, yright) == 1){
x_freeR = xCollision;
y_freeR = yCollision;
}
}
}
//hier neue Zwischenpunkte abspeichern
if(xleft >= 0 && yleft >= 0 ){
if(x_freeL == xCollision){
tree_insert(x_freeL, y_freeL, x_old, y_old, 1); //zwischenschritt vom zwischenschritt abspeichern
tree_insert(xleft,yleft,x_freeL,y_freeL, 1);
}
else tree_insert(xleft,yleft,x_old,y_old,1); //ansonsten nur die neuen Koordinaten abspeichern
//Prüfen ob Ziel bereits direkt erreichbar
if (calc_reachability(&x_freeL, &y_freeL, xleft, yleft, CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){ // In diesem Fall ist das Ziel erreicht
tree_insert(CoordinatesToMap(target->x), CoordinatesToMap(target->y), xleft,yleft,1);
return;
}
//Hier berechnen wie Dick das Hinderniss ist und gleich neuen Wegpunkt abspeichern -> reduziert Anzahl rekursiver Aufrufe
if(find_Pos_Depth(&x_freeL, &y_freeL, xleft, yleft) == 1){
tree_insert(x_freeL,y_freeL,xleft,yleft,1);
xleft = x_freeL;
yleft = y_freeL;
}
if (calc_reachability(&x_freeL, &y_freeL, xleft, yleft, CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){ // In diesem Fall ist das Ziel erreicht
tree_insert(CoordinatesToMap(target->x), CoordinatesToMap(target->y), xleft,yleft,1);
}
else{
setCoordinates(own, own->cam_id,own->tag_id,MapToCoordinates(xleft),MapToCoordinates(yleft),own->angle,own->isWorld);
scan_Obstacles(x_freeL, y_freeL, own, target, xleft, yleft);
}
}
if(xright >= 0 && yright >= 0){
if(x_freeR == xCollision){
tree_insert(x_freeR, y_freeR, x_old, y_old, 0); //zwischenschritt vom zwischenschritt abspeichern
tree_insert(xright,yright,x_freeR,y_freeR, 0);
}
else tree_insert(xright,yright,x_old,y_old,0);
if (calc_reachability(&x_freeR, &y_freeR, xright, yright, CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){
tree_insert(CoordinatesToMap(target->x), CoordinatesToMap(target->y), xright,yright,0);
return;
}
if(find_Pos_Depth(&x_freeR, &y_freeR, xright, yright) == 1){
tree_insert(x_freeR,y_freeR,xright,yright,0);
xright = x_freeR;
yright = y_freeR;
}
if (calc_reachability(&x_freeR, &y_freeR, xright, yright, CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){
tree_insert(CoordinatesToMap(target->x), CoordinatesToMap(target->y), xright,yright,0);
}
else{
setCoordinates(own, own->cam_id,own->tag_id,MapToCoordinates(xright),MapToCoordinates(yright),own->angle,own->isWorld);
scan_Obstacles(x_freeR, y_freeR, own, target, xright, yright);
}
}
return; //toter Pfad, wird nicht mehr zur Zielfindung benötigt
}
/**
* SensorRange = einschränkung der Reichweite. Verhindert unnötig lange Berechnung
* Return: Distance
*/
uint16_t driven_before(int8_t type, uint16_t SensorRange) {
double diffR;
#ifdef CARTOGRAPHY_SCENARIO
gps_reducedData_t *own;
uint16_t angle, diffL;
int16_t xCollision = 0, yCollision = 0, ownX, ownY;
int32_t newX, newY;
//Aktuelle Orientierung beachten!!! Orientierung in Abhängigkeit der Fahrzeugorientierung und dem aktuell betrachteten Sensor
own = get_ownCoords();
switch(type) {
case RIGHT_SENSOR:
angle = AngleMinus(own->angle, 16383);
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2; //siehe Anforderungen in wallFollow für align = 2
break;
case LEFT_SENSOR:
angle = AnglePlus(own->angle, 16383);
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2;//siehe Anforderungen in wallFollow für align = 2
break;
case FRONT_SENSOR:
angle = own->angle;
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2;
break;
default: angle = own->angle; break;
}
SensorRange = SensorRange / 5;
ownX = CoordinatesToMap(own->x);
ownY = CoordinatesToMap(own->y);
if (Sema != NULL) {
while ( xSemaphoreTake( Sema, ( portTickType ) 10 ) != pdTRUE ) {
os_wait(10);
}
newX = (SensorRange*cos_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownX;
ownX = (2*cos_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownX; //Ein stück aus dem Urpsrung rausgehen
newY = (SensorRange*sin_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownY;
ownY = (2*sin_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownY;
//gibt es ein bereits befahrenes Gebiet/Hindernis das zwischen Sensorwert und Auto liegt?
diffL = calc_reachability(&xCollision, &yCollision, ownX, ownY, (int16_t)newX, (int16_t)newY);
xSemaphoreGive(Sema);
} else return 600; //600 = max. Sensorreichweite
if (diffL == 1) {
diffR = sqrt(abs16(ownX - xCollision)*abs16(ownX - xCollision) + abs16(ownY - yCollision)*abs16(ownY - yCollision));
diffR = diffR*5;
if (diffR < 100) {
Seg_Hex(0x00);
}
else Seg_Hex(0x01);
}
else {
diffR = 600; // 600 für nichts gefunden
}
if (diffR > 600) diffR = 600;
#endif
return (uint16_t) diffR;
}
