low.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

  for (x = 0; x < width; x++) 
    for (y = 0; y < height; y++) {
      // The density of each grid square is reported, assuming a full diagonaly traversal of the square.
      // This gives good contrast.
      hit = LowComputeProb(x, y, 1.4, parent->ID);
      // All unknown areas are the same color. You can specify what color that is later
      if (hit == UNKNOWN) 
	map[x][y] = 255;
      else {
	// This specifies the range of grey values for the different squares in the map. Black is occupied.
	map[x][y] = (int) (230 - (hit * 230));
	// This allows us to only print out those sections of the map where there is something interesting happening.
	if (x > lastx)
	  lastx = x;
	if (y > lasty)
	  lasty = y;
	if (x < startx)
	  startx = x;
	if (y < starty)
	  starty = y;
      }
    }

  // If the command was given to print out the set of particles on the map, that's done here.
  if (particles) 
    for (i = 0; i < l_cur_particles_used; i++) 
      if ((l_particle[i].x > 0) && (l_particle[i].x < MAP_WIDTH) && (l_particle[i].y > 0) && (l_particle[i].y < MAP_HEIGHT))
	map[(int) (l_particle[i].x)][(int) (l_particle[i].y)] = 254;

  // And this is where the endpoints of the current scan are visualized, if requested.
  if (overlayX != -1) {
    map[(int) (overlayX)][(int) (overlayY)] = 254;
    for (i = 0; i < SENSE_NUMBER; i++) {
      theta = overlayTheta + sense[i].theta;
      x = (int) (overlayX + (cos(theta) * sense[i].distance));
      y = (int) (overlayY + (sin(theta) * sense[i].distance));

      if ((map[x][y] < 250) || (map[x][y] == 255)) {
	if (sense[i].distance < MAX_SENSE_RANGE) {
	  if (map[x][y] < 200)
	    map[x][y] = 251;
	  else 
	    map[x][y] = 252;
	}
	else
	  map[x][y] = 253;
      }
    }
  }


  // Header file for a ppm
  sprintf(sysCall, "%s.ppm", name);
  printFile = fopen(sysCall, "w");
  fprintf(printFile, "P6\n # particles.ppm \n %d %d\n",
	  lastx-startx+1, lasty-starty+1);
  fprintf(printFile, "255\n");

  // And this is where we finally print out the map to file. Note that there are number of special
  // values which could have been specified, which get special, non-greyscale values. Really,
  // you can play with those colors to your aesthetics.
  for (y = lasty; y >= starty; y--) 
    for (x = startx; x <= lastx; x++) {
      if (map[x][y] == 254) 
	fprintf(printFile, "%c%c%c", 255, 0, 0);
      else if (map[x][y] == 253) 
	fprintf(printFile, "%c%c%c", 0, 255, 200);
      else if (map[x][y] == 252) 
	fprintf(printFile, "%c%c%c", 255, 55, 55);
      else if (map[x][y] == 251) 
	fprintf(printFile, "%c%c%c", 50, 150, 255);
      else if (map[x][y] == 250) 
	fprintf(printFile, "%c%c%c", 250, 200, 200);
      else if (map[x][y] == 0) 
	fprintf(printFile, "%c%c%c", 100, 250, 100);
      else
	fprintf(printFile, "%c%c%c", map[x][y], map[x][y], map[x][y]);
    }
      
  // We're finished making the ppm file, and now convert it to png, for compressed storage and easy viewing.
  fclose(printFile);
  sprintf(sysCall, "convert %s.ppm %s.png", name, name);
  system(sysCall);
  sprintf(sysCall, "chmod 666 %s.ppm", name);
  system(sysCall);
  sprintf(sysCall, "chmod 666 %s.png", name);
  system(sysCall);
  fprintf(stderr, "Map dumped to file\n");
}



//
// The function to call (only once) before LowSlam is called, and initializes all values.
//
void InitLowSlam()
{
  int i;

  // All angle values will remain static
  for (i = 0; i < SENSE_NUMBER; i++) 
    sense[i].theta = (i*M_PI/180.0) - M_PI/2;

  curGeneration = 0;
}



//
// This function cleans up the memory and maps that were used by LowSlam.
// Well, it used to. Now LowSlam takes care of almost all of that by itself. 
//
void CloseLowSlam()
{
}


//
// The main function for performing SLAM at the low level. The first argument will return 
// whether there is still information to be processed by SLAM (set to 1). The second and third
// arguments return the corrected odometry for the time steps, and the corresponding list of
// observations. This can be used for the higher level SLAM process when using hierarchical SLAM.
//
void LowSlam(TPath **path, TSenseLog **obs)
{
  int cnt;
  int i, j, overflow = 0;
  char name[32];
  TPath *tempPath;
  TSenseLog *tempObs;
  TAncestor *lineage;

  // Initialize the worldMap
  LowInitializeWorldMap();

  // Initialize the ancestry and particles
  cleanID = ID_NUMBER - 2;    // ID_NUMBER-1 is being used as the root of the ancestry tree.

  // Initialize all of our unused ancestor particles to look unused.
  for (i = 0; i < ID_NUMBER; i++) {
    availableID[i] = i;

    l_particleID[i].generation = -1;
    l_particleID[i].numChildren = 0;
    l_particleID[i].ID = -1;
    l_particleID[i].parent = NULL;
    l_particleID[i].mapEntries = NULL;
    l_particleID[i].path = NULL;
    l_particleID[i].seen = 0;
    l_particleID[i].total = 0;
    l_particleID[i].size = 0;
  }

  // Initialize the root of our ancestry tree.
  l_particleID[ID_NUMBER-1].generation = 0;
  l_particleID[ID_NUMBER-1].numChildren = 1;
  l_particleID[ID_NUMBER-1].size = 0;
  l_particleID[ID_NUMBER-1].total = 0;
  l_particleID[ID_NUMBER-1].ID = ID_NUMBER-1;
  l_particleID[ID_NUMBER-1].parent = NULL;
  l_particleID[ID_NUMBER-1].mapEntries = NULL;

  // Create all of our starting particles at the center of the map.
  for (i = 0; i < PARTICLE_NUMBER; i++) {
    l_particle[i].ancestryNode = &(l_particleID[ID_NUMBER-1]);
    l_particle[i].x = MAP_WIDTH / 2;
    l_particle[i].y = MAP_HEIGHT / 2;
    l_particle[i].theta = 0.001;
    l_particle[i].probability = 0;
    children[i] = 0;
  }
  // We really only use the first particle, since they are all essentially the same.
  l_particle[0].probability = 1;
  l_cur_particles_used = 1;
  children[0] = SAMPLE_NUMBER;

  // We don't need to initialize the savedParticles, since Localization will create them for us, and they first are used in 
  // UpdateAncestry, which is called after Localization. This statement isn't necessary, then, but serves as a sort of placeholder 
  // when reading the code.
  cur_saved_particles_used = 0;

  overflow = 1;

  for (i=0; i < START_ITERATION; i++)
    ReadLog(readFile, logfile_index, sense);

  curGeneration = 0;
  // Add the first thing that you see to the worldMap at the center. This gives us something to localize off of.
  ReadLog(readFile, logfile_index, sense);
  AddToWorldModel(sense, 0);
  curGeneration = 1;

  // Make a record of what the first odometry readings were, so that we can compute relative movement across time steps.
  lastX = odometry.x;
  lastY = odometry.y;
  lastTheta = odometry.theta;


  // Get our observation log started.
  (*obs) = (TSenseLog *)malloc(sizeof(TSenseLog));
  for (i=0; i < SENSE_NUMBER; i++) {
    (*obs)->sense[i].distance = sense[i].distance;
    (*obs)->sense[i].theta = sense[i].theta;
  }
  (*obs)->next = NULL;

  cnt = 0;
  while (curGeneration < LEARN_DURATION) {
    // Collect information from the data log. If either reading returns 1, we've run out of log data, and
    // we need to stop now.
    if (ReadLog(readFile, logfile_index, sense) == 1)
      overflow = 0;
    else 
      overflow = 1;

    // We don't necessarily want to use every last reading that comes in. This allows us to make certain that the 
    // robot has moved at least a minimal amount (in terms of meters and radians) before we try to localize and update.
    if ((sqrt(SQUARE(odometry.x - lastX) + SQUARE(odometry.y - lastY)) < 0.10) && (fabs(odometry.theta - lastTheta) < 0.04))
      overflow = 0;

    if (overflow > 0) {
      overflow--;

      // Wipe the slate clean 
      LowInitializeFlags();

      // Apply the localization procedure, which will give us the N best particles
      Localize(sense);

      // Add these maintained particles to the FamilyTree, so that ancestry can be determined, and then prune dead lineages
      UpdateAncestry(sense, l_particleID);

      // Update the observation log (used only by hierarchical SLAM)
      tempObs = (*obs);
      while (tempObs->next != NULL)
	tempObs = tempObs->next;
      tempObs->next = (TSenseLog *)malloc(sizeof(TSenseLog));
      if (tempObs->next == NULL) fprintf(stderr, "Malloc failed in making a new observation!\n");
      for (i=0; i < SENSE_NUMBER; i++) {
	tempObs->next->sense[i].distance = sense[i].distance;
	tempObs->next->sense[i].theta = sense[i].theta;
      }
      tempObs->next->next = NULL;

      curGeneration++;

      // Remember these odometry readings for next time. This is what lets us know the incremental motion.
      lastX = odometry.x;
      lastY = odometry.y;
      lastTheta = odometry.theta;
    }
  }


  // Find the most likely particle. Return its path
  j = 0;
  for (i=0; i < l_cur_particles_used; i++) 
    if (l_particle[i].probability > l_particle[j].probability)
      j = i;

  (*path) = NULL;
  i = 0;
  lineage = l_particle[j].ancestryNode;
  while ((lineage != NULL) && (lineage->ID != ID_NUMBER-1)) {
    tempPath = lineage->path;
    i++;
    while (tempPath->next != NULL) {
      i++;
      tempPath = tempPath->next;
    }
    tempPath->next = (*path);

    (*path) = lineage->path;
    lineage->path = NULL;
    lineage = lineage->parent;
  }

  // Print out the map.
  sprintf(name, "map");
  j = 0;
  for (i = 0; i < l_cur_particles_used; i++)
    if (l_particle[i].probability > l_particle[j].probability)
      j = i;
  PrintMap(name, l_particle[j].ancestryNode, FALSE, -1, -1, -1);
  sprintf(name, "rm map.ppm");
  system(name);

  // Clean up the memory being used.
  DisposeAncestry(l_particleID);
  LowDestroyMap();
}