robot_goto.cc

#define FSM_MAIN
#include "fsmmove.h"
#include "movehelper.h"
#include "robot.h"
#include "trgen.h"
#include <path_planner.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <sys/stat.h>
#include <math.h>
#include <map.h>


FSM_STATE_DECL(robot_goto_test);

FSM_STATE(main_init) {
        ShmapInit(1);
        // Where we are at the begining?
        ROBOT_LOCK(act_pos);
        robot.act_pos.x = 0.5;
        robot.act_pos.y = 0.5;
        robot.act_pos.phi = 0;
        ROBOT_UNLOCK(act_pos);
        FSM_TRANSITION(robot_goto_test);
}



FSM_STATE(robot_goto_test)
{
        double goalx, goaly;
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        ShmapSetRectangleType(0.0, 0.0, 0.2, 2.1, MAP_WALL_CSPACE);
                        ShmapSetRectangleType(0.0, 0.0, 3.0, 0.2, MAP_WALL_CSPACE);
                        ShmapSetRectangleType(0.0, 2.0, 3.0, 2.1, MAP_WALL_CSPACE);
                        ShmapSetRectangleType(2.9, 0.0, 3.0, 2.1, MAP_WALL_CSPACE);
                        ShmapSetRectangleType(1.3, 0.8, 1.7, 1.2, MAP_WALL_CSPACE);
                        ShmapSetRectangleType(1.8, 1.5, 2.3, 1.8, MAP_WALL_CSPACE);
                        ShmapSetRectangleType(1.4, 0.9, 1.6, 1.1, MAP_WALL);
                        ShmapSetRectangleType(1.9, 1.6, 2.2, 1.7, MAP_WALL);
                        do {
                                goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
                                goaly = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
                        } while (!ShmapIsFreePoint(goalx, goaly));
                        
                        robot_goto( goalx, goaly);
                        break;
                case EV_MOTION_DONE:
                        FSM_TIMER(1000);
                        break;
                case EV_GOAL_IS_TMP_OBSTACLE:
                case EV_GOAL_IS_OBSTACLE:
                case EV_GOAL_NOT_REACHABLE:
                        DBG("Goal is not reachable: %s\n",fsm_event_str(FSM_EVENT));
                        FSM_TIMER(10000);
                        break;
                case EV_TIMER:
                        ShmapClearOldPath();
                        FSM_TRANSITION(robot_goto_test);
                        break;
                case EV_START:
                        // do nothing1
                        break;
                case EV_EXIT:
                        //ShmapFree();
                        break;
                default:
                        break;
        }
}

int main()
{
        /* robot initialization */
        robot_init();
        srand(time(0));
        FSM(MAIN)->debug_states = 1;
        FSM(MOTION)->debug_states = 1;

        FSM(MAIN)->state = &fsm_state_main_init;
        FSM(MOTION)->state = &fsm_state_move_init;
//      FSM(LOC)->state = &fsm_loc_mcl_init;
        FSM(DET)->state = &fsm_state_det_start;
        FSM(PICKUP)->state = &fsm_state_start_pickup;

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

        /* clean up */
        robot_destroy();

        return 0;
}

---