robot-planning.cc

#include "fsmmove.h"
#include "movehelper.h"
#include "robot.h"
#include "trgen.h"
#include "../../common/map_definitions.h"
#include <aalgorithm.h>
#include <path_simplifier.h>
#include <pathqueue.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <sys/stat.h>
#include <math.h>



GraphMapCell graph[MAP_HEIGHT][MAP_WIDTH];
char *map = NULL;
int shmap_id;

FSM_STATE_DECL(path_planning);

FSM_STATE(main_init) {
        // Where we are at the begining?
        ROBOT_LOCK(act_pos);
        robot.act_pos.x = (double)ROBOT_AXIS_TO_BACK_MM/1000.0;
        robot.act_pos.y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2.0)/1000.0;
        robot.act_pos.phi = 0;
        ROBOT_UNLOCK(act_pos);
        FSM_TRANSITION(path_planning);
}

void ShmapInit(void){
        const int shmap_size = sizeof(char) * MAP_WIDTH * MAP_HEIGHT;
        
        int i,j;

        // Allocate memory to shared map
        shmap_id = shmget (SHM_MAP_KEY , shmap_size, IPC_CREAT | IPC_EXCL | S_IRUSR | S_IWUSR);

        /* Attach the shared memory segment.  */
        map = (char*) shmat (shmap_id, 0, 0);

        /* Initialize Map Memory */
        for (j=0;j<MAP_HEIGHT;j++){
                for(i=0;i<MAP_WIDTH;i++){
                        GETMAPPOS(i,j) = MAP_FREE;
                }
        }
        printf("Map initialized\n");

}

void ShmapFree(void){
        /* Deatch the shared memory segment.  */
        shmdt (map);

        /* Deallocate the shared memory segment.  */
        shmctl (shmap_id, IPC_RMID, 0);
}

#if 0
void path_planner(int xstart,int ystart, int xgoal, int ygoal)
{
        int i,j;
        int tmpx, tmpy, nbpoints, count;
        double *original_path;
        PathPoint * simple_path = (PathPoint *) malloc(sizeof(PathPoint));
        PathPoint * tmp = NULL;
        PathPoint * toFree = NULL;
        NodeQueue list;
        Node n;
        list.first= NULL;
        list.last=NULL;
        

        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
        //tc.maxv *= 2.5;
        //tc.maxacc *= 2.5;

        // Allocate new trajectory
        robot_trajectory_new(&tc, 0);

        //Initialisation of graph structure
        for(i=0;i<MAP_HEIGHT;i++){
                for (j=0;j<MAP_WIDTH;j++){
                        graph[i][j].h=-1;
                        graph[i][j].f=-1;
                        graph[i][j].g=0xFFFF;
                        graph[i][j].bx=-1;
                        graph[i][j].by=-1;
                        graph[i][j].processed=0;
                }
        }
        DBG("Searching a path between (%f,%f) and (%f,%f)\n",
                 GRAPHX2FIELD(xstart), GRAPHY2FIELD(ystart), GRAPHX2FIELD(xgoal), GRAPHY2FIELD(ygoal));

        // Call to A* algirithm
        if (aAlgorithm(xstart,ystart, xgoal, ygoal)) DBG("PATH CALCULATED!!\n");
        else DBG("ERROR : The path planner can not find a path!");
#define PATH_SIMPLIFIER
#ifdef PATH_SIMPLIFIER
        /* Initialize Map Memory */
        for (j=0;j<MAP_HEIGHT;j++){
                for(i=0;i<MAP_WIDTH;i++){
                        GETMAPPOS(i,j) = MAP_FREE;
                }
        }
        // Count original path length 
        i = xgoal;j = ygoal; nbpoints =1;

        DBG("PATH : (%d,%d) ", i ,j);
        while ( ! ((i== xstart) && (j == ystart)) ) {
                tmpx = graph[j][i].bx;
                tmpy = graph[j][i].by;
                nbpoints++;
                i=tmpx;j=tmpy;
                DBG("(%d,%d) ", i ,j);
                GETMAPPOS(i,j) = MAP_PATH;
        }
        DBG("\nTotal Points : %d \n", nbpoints);
        GETMAPPOS(xgoal,ygoal) = MAP_GOAL;
        GETMAPPOS(xstart,ystart) = MAP_START;
        

        // Memory allocation for the simple path 
        original_path = (double *) malloc(2 * sizeof(double) * (nbpoints-1));

        //Put the path in a LIFO list
        i = xgoal;j = ygoal;

        while ( ! ((i== xstart) && (j == ystart)) ) {
                tmpx = graph[j][i].bx;
                tmpy = graph[j][i].by;
                pushNodeLast( &list, tmpx, tmpy);
                i=tmpx;j=tmpy;
        }

        count = 0;
        while(!queueIsEmpty( &list)){
                popNode( &list, &n);
                *(original_path+count*2) = GRAPHX2FIELD(n.x);
                *(original_path+count*2+1) = GRAPHY2FIELD(n.y);
                count++;
        }

        
        // Calc a simplest path 
        count = path_simplifier(original_path, nbpoints-1, simple_path);
        tmp = simple_path;
        DBG("Nb of points of a simpler path %d :", count);
        while(tmp!= NULL){      
                printf("(%f,%f) ", tmp->x, tmp->y);
                robot_trajectory_add_point( tmp->x, tmp->y);
                toFree = tmp;
                tmp = tmp->next;
                //Free allocated memory
                free(toFree);
        }
        DBG("\n");


#endif

#ifndef PATH_SIMPLIFIER
        //Put the path in a LIFO list
        i = xgoal;j = ygoal;

        while ( ! ((i== xstart) && (j == ystart)) ) {
                tmpx = graph[j][i].bx;
                tmpy = graph[j][i].by;
                pushNodeLast( &list, tmpx, tmpy);
                i=tmpx;j=tmpy;
        }


        while(!queueIsEmpty( &list)){
                popNode( &list, &n);
                robot_trajectory_add_point( GRAPHX2FIELD(n.x), GRAPHY2FIELD(n.y));
        }
#endif
        // add the path to the robot trajectory
        robot_trajectory_add_final_point(GRAPHX2FIELD(xgoal), GRAPHY2FIELD(ygoal));
        //robot_trajectory_add_final_point((rand()%(PLAYGROUND_WIDTH_MM-50)+25)/1000.0,
        //                                (rand()%(PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);


}
#endif
FSM_STATE(path_planning)
{       
        int startx, starty, goalx, goaly;
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        if (map == NULL) ShmapInit();
                        robot_trajectory_stop();
                        ROBOT_LOCK(act_pos);
                        startx=(int) floor(robot.act_pos.x); starty = (int) floor(robot.act_pos.y);
                        ROBOT_UNLOCK(act_pos);
                        goalx = (int) floor((rand()%(PLAYGROUND_WIDTH_MM-50)+25)/100);
                        goaly = (int) floor((rand()%(PLAYGROUND_HEIGHT_MM-50)+25)/100);
                        DBG("Moving from (%d,%d) to (%d, %d)\n",startx, starty,goalx, goaly);
                        path_planner( startx, starty,goalx, goaly);
                        break;
                case EV_TRAJECTORY_DONE_AND_CLOSE:
                        FSM_TIMER(1000);
                        break;
                case EV_TIMER:
                        FSM_TRANSITION(path_planning);
                        break;
                case EV_START:
                        // do nothing
                        break;
                case EV_EXIT:
                        ShmapFree();
                        break;
                default: 
                        break;
        }
}

int main()
{
        srand(time(0));
        /* robot initialization */
        robot_init();

        //FSM(MAIN)->debug_states = 1;
        FSM(MOT)->debug_states = 1;

        robot.fsm[FSM_ID_MAIN].fnc_act = &fsm_state_main_init;
        robot.fsm[FSM_ID_MOT].fnc_act = &fsm_state_move_init;
//      robot.fsm[FSM_ID_LOC].fnc_act = &fsm_loc_mcl_init;
        robot.fsm[FSM_ID_DET].fnc_act = &fsm_state_det_start;
        robot.fsm[FSM_ID_PICKUP].fnc_act = &fsm_state_start_pickup;

        /* Start threads and wait */
        robot_start();
        robot_wait();

        /* clean up */
        robot_destroy();

        return 0;
}

---