low.c

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

  // an easy way to pass through the entire tree.  If the node is in use, look at its parent.
  // If the parent has only one child, give all of the altered map squares of the parent to the child, 
  // and dispose of the parent. This collapses those ancestor nodes which have a branching factor of only one.
  for (i = 0; i < ID_NUMBER-1; i++) {
    // These booleans mean (in order) that the ID is in use, it has a parent (ie is not the root of the ancestry tree),
    // and that its parent has only one child (which is necessarily this ID) 
    if ((particleID[i].ID == i) && (particleID[i].parent != NULL) && (particleID[i].parent->numChildren == 1)) {
      // This is a special value for redirections. If the node's parent has already participated in a collapse
      // during this generation, then that node has already been removed from the tree, and we need to go up
      // one more step in the tree. 
      while (particleID[i].parent->generation == -111)
	particleID[i].parent = particleID[i].parent->parent;

      parentNode = particleID[i].parent;

      // Check to make sure that the parent's array is large enough to accomadate all of the entries of the child
      // in addition to its own. If not, we need to increase the dynamic array.
      if (parentNode->size < (parentNode->total + particleID[i].total)) {
	parentNode->size = (int)(ceil((parentNode->size + particleID[i].size)*1.5));
	workArray = (TEntryList *)malloc(sizeof(TEntryList)*parentNode->size);
	if (workArray == NULL) fprintf(stderr, "Malloc failed for workArray\n");

	for (j=0; j < parentNode->total; j++) {
	  workArray[j].x = parentNode->mapEntries[j].x;
	  workArray[j].y = parentNode->mapEntries[j].y;
	  workArray[j].node = parentNode->mapEntries[j].node;
	}
	// Note that parentNode->total hasn't changed- that will grow as the child's entries are added in
	free(parentNode->mapEntries);
	parentNode->mapEntries = workArray;
      }

      // Change all map entries of the parent to have the ID of the child
      // Also check to see if this entry supercedes an entry currently attributed to the parent.
      // Since collapses can merge all of the entries between the parent and the current child into the parent, this check is performed
      // by comparing to see if the generation of the last observation (before the child's update) is at least as recent as parent's
      // generation. If so, note that there is another "dead" entry in the observation array. It will be cleaned up later. If this puts
      // the total number of used slot, minus the number of "dead", below the threshold, shrink the array (which cleans up the dead)
      entry = particleID[i].mapEntries;
      for (j=0; j < particleID[i].total; j++) {
	node = lowMap[entry[j].x][entry[j].y];

	// Change the ID
	node->array[entry[j].node].ID = parentNode->ID;
	node->array[entry[j].node].source = parentNode->total;

	parentNode->mapEntries[parentNode->total].x = entry[j].x;
	parentNode->mapEntries[parentNode->total].y = entry[j].y;
	parentNode->mapEntries[parentNode->total].node = entry[j].node;
	parentNode->total++;

	// Check for pre-existing observation in the parent's list
	if (node->array[entry[j].node].parentGen >= parentNode->generation) {
	  node->array[entry[j].node].parentGen = -1;
	  node->dead++;
	}
      }

      // We do this in a second pass for a good reason. If there are more than one update for a given grid square which uses the child's
      // ID (as a consequence of an earlier collapse), then we want to make certain that the resizing doesn't take place until after all
      // entries have changed their ID appropriately.
      for (j=0; j < particleID[i].total; j++) {
	node = lowMap[entry[j].x][entry[j].y];
	if ((node->total - node->dead)*2.5 < node->size) 
	  LowResizeArray(node, -7);
      }

      // We're done with it- remove the array of updates from the child.
      free(particleID[i].mapEntries);
      particleID[i].mapEntries = NULL;

      // Inherit the path
      tempPath = parentNode->path;
      while (tempPath->next != NULL) 
	tempPath = tempPath->next;
      tempPath->next = particleID[i].path;
      particleID[i].path = NULL;

      // Inherit the number of children
      parentNode->numChildren = particleID[i].numChildren;

      // Subtlety of the ancestry tree: since we only keep pointers up to the parent, we can't exactly change all of the
      // descendents of the child to now point to the parent. What we can do, however, is mark the change for later, and
      // update all of the ancestor particles in a single go, later. That will take a single O(P) pass
      particleID[i].generation = -111;
    }
  }

  // This is the step where we correct for redirections that arise from the collapse of a branch of the ancestry tree
  for (i=0; i < ID_NUMBER-1; i++) 
    if (particleID[i].ID == i) {
      while (particleID[i].parent->generation == -111) 
	particleID[i].parent = particleID[i].parent->parent;
    }

  // Wipe the slate clean, so that we don't get confused by the mechinations of the previous changes
  // from deletes and merges. Updates can make thier own tables, as needed.
  LowInitializeFlags();


  // Add the current savedParticles into the ancestry tree, and copy them over into the 'real' particle array
  j = 0;
  for (i = 0; i < cur_saved_particles_used; i++) {
    // Check for redirection of parent pointers due to collapsing of branches (see above)
    while (savedParticle[i].ancestryNode->generation == -111) 
      savedParticle[i].ancestryNode = savedParticle[i].ancestryNode->parent;

    // A saved particle has ancestryNode denote the parent particle for that saved particle
    // If that parent has only this one child, due to resampling, then we want to perform a collapse
    // of the branch, but it hasn't been done yet, because the savedParticle hasn't been entered into
    // the tree yet. Therefore, we just designate the parent as the "new" entry for this savedParticle.
    // We then update the already created ancestry node as if it were the new node.
    if (savedParticle[i].ancestryNode->numChildren == 1) {
      // Change the generation of the node
      savedParticle[i].ancestryNode->generation = curGeneration;
      // Now that it represents the new node as well, it no longer is considered to have children.
      savedParticle[i].ancestryNode->numChildren = 0;
      // We're copying the savedParticles to the main particle array
      l_particle[j].ancestryNode = savedParticle[i].ancestryNode;

      // Add a new entry to the path of the ancestor node.
      trashPath = (TPath *)malloc(sizeof(TPath));
      trashPath->C = savedParticle[i].C;
      trashPath->D = savedParticle[i].D;
      trashPath->T = savedParticle[i].T;
      trashPath->dist = savedParticle[i].dist;
      trashPath->turn = savedParticle[i].turn;
      trashPath->next = NULL;
      tempPath = l_particle[i].ancestryNode->path;
      while (tempPath->next != NULL)
	tempPath = tempPath->next;
      tempPath->next = trashPath;

      l_particle[j].x = savedParticle[i].x;
      l_particle[j].y = savedParticle[i].y;
      l_particle[j].theta = savedParticle[i].theta;
      l_particle[j].probability = savedParticle[i].probability;
      j++;
    }

    // IF the parent has multiple children, then each child needs its own new ancestor node in the tree
    else if (savedParticle[i].ancestryNode->numChildren > 0) {
      // Find a new entry in the array of ancestor nodes. This is done by taking an unused ID off of the
      // stack, and using that slot. 
      temp = &(particleID[ availableID[cleanID] ]);
      temp->ID = availableID[cleanID];
      // That ID on the top of the stack is now being used.
      cleanID--;

      if (cleanID < 0) {
	fprintf(stderr, " !!! Insufficient Number of Particle IDs : Abandon Ship !!!\n");
	cleanID = 0;
      }

      // This new node needs to have its info filled in
      temp->parent = savedParticle[i].ancestryNode;
      // No updates to the map have been made yet for this node
      temp->mapEntries = NULL;
      temp->total = 0;
      temp->size = 0;
      // The generation of this node is important for collapsing branches of the tree. See above.
      temp->generation = curGeneration;
      temp->numChildren = 0;
      temp->seen = 0;

      // This is where we add a new entry to this node's hypothesized path for the robot
      trashPath = (TPath *)malloc(sizeof(TPath));
      trashPath->C = savedParticle[i].C;
      trashPath->D = savedParticle[i].D;
      trashPath->T = savedParticle[i].T;
      trashPath->dist = savedParticle[i].dist;
      trashPath->turn = savedParticle[i].turn;
      trashPath->next = NULL;
      temp->path = trashPath;

      // Transfer this entry over to the main particle array
      l_particle[j].ancestryNode = temp;
      l_particle[j].x = savedParticle[i].x;
      l_particle[j].y = savedParticle[i].y;
      l_particle[j].theta = savedParticle[i].theta;
      l_particle[j].probability = savedParticle[i].probability;
      j++;
    }
  }

  l_cur_particles_used = cur_saved_particles_used;

  // Here's where we actually go through and update the map for each particle. We had to wait
  // until now, so that the appropriate structures in the ancestry had been created and updated.
  for (i=0; i < l_cur_particles_used; i++) 
    AddToWorldModel(sense, i);

  // Clean up the ancestry particles which disappeared in branch collapses. Also, recover their IDs.
  // We waited until now because we needed to allow for redirection of parents.
  for (i=0; i < ID_NUMBER-1; i++) 
    if (particleID[i].generation == -111) {
      particleID[i].generation = -1;
      particleID[i].numChildren = 0;
      particleID[i].parent = NULL;
      particleID[i].mapEntries = NULL;
      particleID[i].path = NULL;
      particleID[i].seen = 0;
      particleID[i].total = 0;
      particleID[i].size = 0;

      // Recover the ID. 
      cleanID++;
      availableID[cleanID] = i;
      particleID[i].ID = -3;
    }
}



//
// ReadLog
//
// Reads back into the sensor data structures the raw readings that were stored to file by WriteLog (above)
// Reads a single line from the file, and interprets it by its delineator (either Laser or Odometry). If the line
// is not delineated for some reason, the function prints out an error message, and advances to the next line.
// While there is still information in the file, it will return 0. When it reaches the end of the file, it will return 1.
//
int ReadLog(carmen_FILE *logFile, carmen_logfile_index_p logfile_index, 
	    TSense &sense) {
  int i, max;
  char line[4096];
  int laser;
  carmen_robot_laser_message laser_msg;

  if (carmen_logfile_eof(logfile_index)) {
    fprintf(stderr, "End of Log File.\n");
    return 1;
  }

  do {
    laser = carmen_logfile_read_next_line(logfile_index, logFile,
					  4095, line);
  } while (strncmp(line, "ROBOTLASER1 ", 12) != 0);

  memset(&laser_msg, 0, sizeof(laser_msg));
  carmen_string_to_robot_laser_message(line, &laser_msg);

  max = laser_msg.num_readings;
  if (max > SENSE_NUMBER)
    max = SENSE_NUMBER;
      
  // Now read in the whole list of laser readings.
  for (i = 0; i < max; i++) {
    sense[i].theta = (i*M_PI/max) - M_PI/2;
    sense[i].distance = laser_msg.range[i] * MAP_SCALE;
    if (sense[i].distance > MAX_SENSE_RANGE)
      sense[i].distance = MAX_SENSE_RANGE;
  }
  // Read x and y coordinates. 
  odometry.x = laser_msg.laser_pose.x;
  odometry.y = laser_msg.laser_pose.y;
  // Read the facing angle of the robot.
  odometry.theta = laser_msg.laser_pose.theta;
  odometry.theta = carmen_normalize_theta(odometry.theta);

  return 0;
}



//
// PrintMap
//
// name - name of map file
// parent - a pointer to the specific particle you want to print out the map for
// particles - flag to indicate if position of all current particles should be shown on the map
// overlay* - if you want the specific particle's latest information which is just being entered into the
//            map to show up as different colors, you need to specify the position of the particle here. 
//            Values of -1 here turn this off.
//
void PrintMap(char *name, TAncestor *parent, int particles, double overlayX, double overlayY, double overlayTheta)
{
  FILE *printFile;
  int x, y, i;
  int width, height;
  int startx, starty, lastx, lasty;
  char sysCall[128];
  double hit, theta;

  width = MAP_WIDTH;
  height = MAP_HEIGHT;

  for(x=0; x < width; x++)
    for(y=0; y<height; y++)
      map[x][y] = 0;

  lastx = 0;
  lasty = 0;
  startx = width-1;
  starty = height-1;