path_planner.c

Go to the documentation of this file.

#include "path_planner.h"
#define DBG printf


int path_planner(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real , PathPoint ** simple_path, double * angle)
{
        int nbpoints, count;
        double *original_path;
        PathPoint * tmp;        
        
        // Init the map
        if (! ShmapIsMapInit()) {DBG("ERROR: shared map memory is not init.\n"); return PP_ERROR_MAP_NOT_INIT;}

        // Clear previous path
        ShmapClearOldPath();
        
        // If the goal map is not free, abort.
        if (!ShmapIsFreePoint(xgoal_real, ygoal_real)) {DBG("ERROR: the goal cell is an obstacle.\n"); return PP_ERROR_GOAL_IS_OBSTACLE;}
        
        // Call to A* algorithm
        nbpoints = aAlgorithm(xstart_real,ystart_real, xgoal_real, ygoal_real, &original_path);
        if (nbpoints < 0) return PP_ERROR_GOAL_NOT_REACHABLE;

        // DBG("Simplifing the path\n");
        // Calc a simplest path 
        (*simple_path) = (PathPoint *) malloc(sizeof(PathPoint));
        count = path_simplifier(original_path, nbpoints, (*simple_path), angle);
        // Do not send the first point
        tmp=(*simple_path);
        (*simple_path)= (*simple_path)->next;
        free(tmp);
        free(original_path);
        
        // All is OK
        return 1;


}

---