/branches/slam/code/projects/slam/computer/server_main.c - Diff - Colony - Robotics Club

Revision 599

Added by Justin Scheiner about 16 years ago

General work on SLAM

     #include <stdio.h>
     #include <stdlib.h>
     #include <time.h>
     #include <sys/time.h>
     #include "data_requests.h"
     #include "queue.h"
     #include "math.h"
     #define DEFAULT_WIDTH 50
     #define DEFAULT_HEIGHT 50
     #define DEFAULT_WIDTH 512
     #define DEFAULT_HEIGHT 512
     #define MAX_MAPS 30
     #define MAX_ID 20
-...
       short robot_id;
     } Data;
     typedef struct robotScreenShot{
       char robot_current_map;
       double robot_orientation; //-PI to PI
     /**
     * Map oriented data.
     */
     typedef struct Map{
       int data[DEFAULT_WIDTH][DEFAULT_HEIGHT];
       int robot_x;
       int robot_y;
     } RobotScreenShot;
       double robot_orientation;
       short robot_id;
       char correlation_group;
     } Map;
     /*
     This is the game plan if anyone is interested.
     Each robot will go for an unspecified amount of time,
     accruing encoder, BOM, and IR data.  The IR and encoders
     will be used to generate heat maps for each robot, starting
     at the center, decreasing in certainty (i.e. the amount of
     'color' put on during a time unit will be spread over a greater
     area) over time according to the physical limitations of
     the measuring devices.
     Map** map_array;
     int map_count;
     When the uncertainty gets too high, a new map will be allocated for
     the robot.
     The BOM data will be used to orient individual maps to one another.
     Along with physical limitations on the size of the environment, and
     hopefully some brainstorming to do with triangularization and what not,
     the heat maps will become increasingly well oriented.
     Finally, after a currently unspecified amount of time, the maps which have
     been properly oriented (this could probably just be all of them due to the nature
     of the heat maps,) will be summed up into one meta map! Metamap will undergo some
     signal processing, probably in the form of a second order gradient, to get a map
     of magnitudes, which we will try to further reduce to lines.  These lines are the
     walls of the environment.
     In time, it is possible that this "unspecfied amount of time" could become a continuous
     computation, but I haven't put much thought into how this will scale.  It will almost
     definitely depend upon the chosen resolution of the heat map.
     I do want people working with me, this should be a lot of fun,
     I'm coming to accept the fact that I'm not a very good programmer,
     but I think this is a solid method of doing SLAM.
     */
     /**
     * Map oriented data.
     */
     int* map_array[MAX_MAPS][DEFAULT_WIDTH][DEFAULT_HEIGHT];
     short map_ids[MAX_MAPS];
     char map_correlated[MAX_MAPS];
     /**Robot oriented data.**/
     Queue* dataQueue;
     int robot_map[MAX_ID];
     /*These functions simply add the data to the queue.*/
     void bom_handler(short id, BomNode** head);
     void IR_handler(short id, short** data);
     void encoder_handler(short id, unsigned char** data);
     void slam_analyze(void);
     void bom_analyze(BomNode* head,short robot_id);
     void IR_analyze(short* data, short robot_id);
     void encoder_analyze(unsigned char* data, short robot_id);
     void initialize(void);
     int main(void){
       data_requests_init(bom_handler,IR_handler,encoder_handler);
       dataQueue = queue_create();
       initialize();
       slam_analyze();
       return -1;
+    }
     void initialize(void){
       map_array = malloc(MAX_MAPS * sizeof(Map*));
       map_count = 0;
       int i;
       for(i=0; i<MAX_ID;i++){
         robot_map[i] = -1;
+      }
+    }
     void slam_analyze(void){
       Data* current_data;
       while(1){
         if(!queue_is_empty(dataQueue)){
           if(queue_size(dataQueue) < 5){
             //request_all(/*The robot that most needs it*/);
+          }
           current_data = (Data*)queue_remove(dataQueue);
           switch(current_data->type){
-...
+      }
+    }
     int main(void){
       data_requests_init(bom_handler,IR_handler,encoder_handler);
       dataQueue = queue_create();
       slam_analyze();
       return -1;
     void initialize_map(int robot_id){
         int i,j;
         map_array[map_count] = (Map*)malloc(sizeof(Map));
         for(i=0;i<DEFAULT_WIDTH;i++){
             for(j=0;j<DEFAULT_HEIGHT;j++){
               map_array[map_count]->data[i][j] = 0;
+            }
+        }
         map_array[map_count]->robot_x = DEFAULT_WIDTH / 2;
         map_array[map_count]->robot_y = DEFAULT_HEIGHT/ 2;
         map_array[map_count]->robot_orientation = 0;
         map_array[map_count]->robot_id = robot_id;
         map_array[map_count]->correlation_group = -1;
         robot_map[robot_id] = map_count++;
+    }
     void bom_analyze(BomNode* head, short robot_id){
       BomNode* current_node = head;
       if(robot_map[robot_id]==-1) initialize_map(robot_id);
       while(current_node!=NULL){
         current_node = current_node->next;
+      }
+    }
     void IR_analyze(short* data, short robot_id){
       if(robot_map[robot_id]==-1) initialize_map(robot_id);
+    }
     void encoder_analyze(unsigned char* data, short robot_id){
       if(robot_map[robot_id]==-1) initialize_map(robot_id);
+    }
     /*The packet handling functions for data requests.*/
     /*The packet handling functions for nata requests.*/
     void bom_handler(short id, BomNode** head){
       Data* newData = malloc(sizeof(Data));
       newData->type = BOM_TYPE;
       newData->data.u_b = (*head);
       newData->robot_id = id;
       newData->timeStamp = malloc(sizeof(struct timeval));
       gettimeofday(&newData->timeStamp,NULL);
-...
     */
     void rotate_map(int*** map, double radians){
       int i,j;
       double iprime, jprime, xoffset, yoffset;
       double iprime, jprime, xoffset, yoffset, R1, R2, R3, R4, A;
       int i2,j2;
       int new_map[DEFAULT_WIDTH][DEFAULT_HEIGHT];
       int** new_map;
       new_map = malloc(DEFAULT_WIDTH*sizeof(int*));
       for(i=0;i<DEFAULT_WIDTH;i++){
           *(new_map+i) = malloc(DEFAULT_HEIGHT * sizeof(int));
           for(j=0;j<DEFAULT_WIDTH;i++){
               new_map[i][j] = 0;
+          }
-...
              xoffset = iprime - i2;
              yoffset = jprime - j2;
              R1 = sqrt(xoffset*xoffset + yoffset*yoffset)*(double)((*map)[i][j]);
              R2 = sqrt((1-xoffset)*(1-xoffset) + yoffset*yoffset)*(double)((*map)[i][j]);
              R3 = sqrt(xoffset*xoffset + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j]);
              R4 = sqrt((1-xoffset)*(1-xoffset) + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j]);
              A = 1/(R1+R2+R3+R4);
              //If the new coordinates are legal...
              if(i2 < DEFAULT_WIDTH && i2 >= 0 && j2 < DEFAULT_HEIGHT && j2 >= 0)
                  new_map[i2][j2] += (int)(sqrt(xoffset*xoffset + yoffset*yoffset)*(double)((*map)[i][j]));
                  new_map[i2][j2] += (int)((R1*A)*(double)((*map)[i][j]));
              if(i2 + 1 < DEFAULT_WIDTH && i2 + 1 >= 0 && j2 < DEFAULT_HEIGHT && j2 >= 0)
                  new_map[i2+1][j2] += (int)(sqrt((1-xoffset)*(1-xoffset) + yoffset*yoffset)*(double)((*map)[i][j]));
                  new_map[i2+1][j2] += (int)((R2*A)*(double)((*map)[i][j]));
              if(i2 < DEFAULT_WIDTH && i2 >= 0 && j2 + 1 < DEFAULT_HEIGHT && j2 + 1 >= 0)
                  new_map[i2][j2+1] += (int)(sqrt(xoffset*xoffset + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j]));
             if(i2 + 1 < DEFAULT_WIDTH && i2 + 1 >= 0 && j2 + 1 < DEFAULT_HEIGHT && j2 + 1 >= 0)
                  new_map[i2+1][j2+1] += (int)(sqrt((1-xoffset)*(1-xoffset) + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j]));
                  new_map[i2][j2+1] += (int)((R3*A)*(double)((*map)[i][j]));
              if(i2 + 1 < DEFAULT_WIDTH && i2 + 1 >= 0 && j2 + 1 < DEFAULT_HEIGHT && j2 + 1 >= 0)
                  new_map[i2+1][j2+1] += (int)((R4*A)*(double)((*map)[i][j]));
+          }
+      }
       //Incompatible types? FIXME
       (*map) = new_map;
       return;
+    }

Also available in: Unified diff

Project

General

Profile

Colony

Revision 599