astarpp.cpp

一个非常有用的客户端开发程序

         for(int y = low_y; y <= 1; y++)
            for(int x = -1; x <= 1; x++) {
               current_x = flexXInt + x;
               current_y = flexYInt + y;

              
                        if(_values[current_x][current_y] == -1)
                           _values[current_x][current_y] = prior_occ;

                        // Adjust the map 
                        new_prob = 1 - 1 / (1 + (1 - prior_occ) / prior_occ * 
                              flex_prob / (1 - flex_prob) *
                              _values[current_x][current_y] / 
                              (1 - _values[current_x][current_y]));

                        // new_prob = p_filled; immediate display
                        new_prob = MAX(min_prob, MIN(max_prob, new_prob));

                        _values[current_x][current_y] = new_prob;
                    
                  
               }
            }

      }

     
      updateWindow();
//       publishGrid();
      // compareGridWithScan();
//    }

   _flex_history = msg;
   _flex_hist_initialized = true;
  // publishReactiveSensorInformation();
}

void AStarPP::update(rescue_rangescan_ranges_message &msg) 
{
   if (!_geometryInfoLRFReceived) {
      rlogError("LRF Geometry not set: Using defaults from Message ! \n");
      // Trigger geometry message
      rescue_geometry_request_message req;
      req.type =  RESCUE_LRF_TYPE;
      req.robot.id = getRobotId();
      req.robot.ts = getCurrentTime();

      IPC_publishData(RESCUE_GEOMETRY_REQUEST_NAME, &req);
      rlogInfo("GEO Msg for LRF triggered!\n");

      return;
   }

   rangeUpdate(RESCUE_LRF_TYPE, msg.nranges, msg.ranges);
   _scan_history = msg;
   _scan_hist_initialized = true;
}


void AStarPP::lineUpdate(RESCUE_SENSOR_TYPE type, int x1, int y1, int x2, int y2, double range, double maxRange) 
{
   int current_x, current_y, x_diff, y_diff;
   double d, p_filled;
   double new_prob;
   bresenham_param_t b_params;
   int tmp=0;

   get_bresenham_parameters(x1, y1, x2, y2, &b_params);  // Follow line
   do {
      tmp++;
      
      get_current_point(&b_params, &current_x, &current_y);
      if(current_x >= 0 && current_x < _sizeX &&
            current_y >= 0 && current_y < _sizeY) {
         /* Calculate distance from laser */
         x_diff = abs(current_x - x1);
         y_diff = abs(current_y - y1);

         d = hypot(x_diff,y_diff) * _resolution;
           
                  p_filled = getCellProbability(type, range, d, maxRange);
                  if(p_filled == -1) 
                     break;

                  // don't update empty ev. for lines, because scanner/IR might overlook s.th.
                  if((p_filled < 0.5) && (!LINE_UPDATE_EMPTY))    
                     break;

                  if(_values[current_x][current_y] == -1)
                     _values[current_x][current_y] = prior_occ;


                  // Adjust the map 
                  new_prob = 1 - 1 / (1 + (1 - prior_occ) / prior_occ * 
                        p_filled / (1 - p_filled) *
                        _values[current_x][current_y] / 
                        (1 - _values[current_x][current_y]));

                  // new_prob = p_filled; immediate display
                  new_prob = MAX(min_prob, MIN(max_prob, new_prob));

                  _values[current_x][current_y] = new_prob;
      }
   } while(get_next_point(&b_params));
}

// Handles LRF
// Does Grid updates and saves laser scans for visualisation
void AStarPP::rangeUpdate(RESCUE_SENSOR_TYPE type, int num, double ranges[])  
{
   if(DEBUG_LRF)
      rlogDebug("Range Update for type %d with %d ranges\n", type, num);

   struct timeval start;
   struct timeval end;
   gettimeofday(&start, NULL);
   int num_readings = num;
   rescue_geometry_message geometry = _geometryLRF;

   // For COUNT_GRID
   // all cells -1 if > 0

   //Grab data 
   double *laser_range = new double[num_readings];
   double *laser_angle = new double[num_readings];
   double *laser_posX = new double[num_readings];
   double *laser_posY = new double[num_readings];
   for (int i=0; i<num_readings; i++) {
      laser_range[i] = ranges[i];
      laser_angle[i] = geometry.yaw_angles[i];
      laser_posX[i] = geometry.x_coords[i];
      laser_posY[i] = geometry.z_coords[i];
   }

   double start_x = 0;
   double start_y = 0;
   double theta = 0.0;

   double* extra_x = NULL;
   double* extra_y = NULL;
   if(type == RESCUE_LRF_TYPE) {
      extra_x = new double[num_readings];
      extra_y = new double[num_readings];
   }

   for(int i = 0; i < num_readings; i++) {
      if (laser_range[i]<0) {
             rlogError("GOT NEGAIVE RANGE VALUE: %f\n",laser_range[i]);
         //    no unusual error -> LRF uses this as error code
         continue;
      }

      double laser_x = 0.0;
      double laser_y = 0.0;
      // Calculate Starting Point
      laser_x = _sizeX / 2.0 + (laser_posX[i] / _resolution - start_x);
      laser_y = _sizeY / 2.0 + (laser_posY[i] / _resolution - start_y);
      int x1int = (int)floor(laser_x);
      int y1int = (int)floor(laser_y);

      // Calculate Ending Point
      theta = DEG2RAD(270 - laser_angle[i]);
      laser_x = _sizeX / 2.0 + (laser_posX[i] / _resolution - start_x);
      laser_y = _sizeY / 2.0 + (laser_posY[i] / _resolution - start_y);
      int x2int, y2int;

      if(type == RESCUE_LRF_TYPE) {
         int x_end = (int)(laser_x + (laser_range[i]) * cos(theta) / _resolution);
         int y_end = (int)(laser_y + (laser_range[i]) * sin(theta) / _resolution);
         extra_x[i] = x_end;
         extra_y[i] = y_end;
      }

      if( (type == RESCUE_LRF_TYPE)) {       // Laserstrahlen gehen nur bis hinter die Wand
         x2int = (int)(laser_x + (laser_range[i] + wall_thickness) * cos(theta) / _resolution);
         y2int = (int)(laser_y + (laser_range[i] + wall_thickness) * sin(theta) / _resolution);
      } 
      else {
         x2int = (int)(laser_x + (geometry.maxRange + wall_thickness) * cos(theta) / _resolution);
         y2int = (int)(laser_y + (geometry.maxRange + wall_thickness) * sin(theta) / _resolution);
      }


      if(type == RESCUE_LRF_TYPE)
      {
         if((type != RESCUE_LRF_TYPE) || (USE_LRF)) {
            lineUpdate(type, x1int, y1int, x2int, y2int, laser_range[i], geometry.maxRange);
            //rlogInfo("(x1,y1) --> (x2,y2)  (%d,%d) --> (%d,%d) range = %f)\n",x1int, y1int, x2int, y2int,laser_range[i]);
         }
      } 
      else 
         rlogError("rangeUpdate: Can't handle type: %d \n", type);
   }

   if(type == RESCUE_LRF_TYPE) {
      _gw->setPointData(extra_x, extra_y, num_readings);      
   }

   // Find maxVal
   int maxVal = -INT_MAX;
   for (int x=0; x<_sizeX; x++)
      for (int y=0; y<_sizeY; y++)
         if (_values[x][y] > maxVal) 
            maxVal =  (int) _values[x][y];

   gettimeofday(&end, NULL);
   if(DEBUG_TIMING)
      rlogDebug("RangeUpdate(%d) took %ld s and %ld  us\n", type, end.tv_sec - start.tv_sec, end.tv_usec - start.tv_usec);

   /*updateHistogram(); 
   updateFreeSpace();
   */updateWindow();
   //publishGrid();
   //publishReactiveSensorInformation();
   //compareGridWithScan();

   delete [] laser_range;
   delete [] laser_angle;
   delete [] laser_posX;
   delete [] laser_posY;
}

double AStarPP::getCellProbability(RESCUE_SENSOR_TYPE type, double range, double dist, double maxRange) {
   if(type ==  RESCUE_LRF_TYPE) {
      return getLaserCellProbability(range, dist, maxRange);
   } 
   else {
      rlogError("Can't calculate probability for %d \n", type);
      return -1;
   }
}


void AStarPP::updateWindow() 
{
   if (_gw) { 
      _gw->setOccupancyData( _values, 1.0, _sizeX, _sizeY);
     }
}


void AStarPP::findPlan(PP_Point goal){

//bool foundGoal=false, fail=false;
PP_Point start((_sizeX/2),(_sizeY/2));
goal.x+=(_sizeX/2);
goal.y+=(_sizeY/2);
 struct timeval st;
 struct timeval end;
 gettimeofday(&st, NULL);

_plan=astar.findPlan(start,goal);   

gettimeofday(&end, NULL);
if(DEBUG_TIMING)
      rlogDebug("PATH PLANNING  took %ld s and %ld  us\n",  end.tv_sec - st.tv_sec, end.tv_usec - st.tv_usec);

_gw->setPlanData( _plan);
}

// -1 : no update
double AStarPP::getLaserCellProbability(double range, double dist, double maxRange) {
   double p_filled = -1;
   double max_sure_range = maxRange * rangeConfidence;
   double max_range = maxRange;

   if(dist < range) {       /* Free observation */
      if(dist < max_sure_range)
         p_filled = emp_evidence;
      else if(dist < max_range)
         p_filled = emp_evidence +
            (dist - max_sure_range) / max_range * 
            (prior_occ - emp_evidence);
      else
         p_filled = -1;
   }
   else {                               /* Filled observation */
      if(dist < max_sure_range)
         p_filled = occ_evidence;
      else if(dist < max_range)
         p_filled = occ_evidence + 
            (dist - max_sure_range) / max_range * 
            (prior_occ - occ_evidence);
      else
         p_filled = -1;
   }
   return p_filled;
}


double AStarPP::gauss(double x, double mu, double sigma) {
   return 1/sqrt(2*M_PI)/(sigma)*exp(-0.5*(x-mu)*(x-mu)/(sigma*sigma));
}


int AStarPP::FlexRightClear() {
   if(!_flex_hist_initialized) {
      rlogError("No Flex Hist initialized \n");
      return -1;
   }
   if(_flex_history.num != 2) {
      rlogError("Reactive Sensor Info: Can't handle %d Flex \n", _flex_history.num);
      return -1;
   }

   bool flex_active = (_flex_history.value[1] > 500.0);

   if(DEBUG_REACTIVE) {
      if(flex_active)
         rlogDebug("Flex Right blocked !\n");
      else
         rlogDebug("Flex Right free \n");
   }

   if(flex_active)
      return 0;
   return 1;
}


int AStarPP::FlexLeftClear() {
   if(!_flex_hist_initialized) {
      rlogError("No Flex Hist initialized \n");
      return -1;
   }
   if(_flex_history.num != 2) {
      rlogError("Reactive Sensor Info: Can't handle %d Flex \n", _flex_history.num);
      return -1;
   }

   bool flex_active = (_flex_history.value[0] > 500.0);

   if(DEBUG_REACTIVE) {
      if(flex_active)
         rlogDebug("Flex Left blocked !\n");
      else
         rlogDebug("Flex Left free \n");
   }

   if(flex_active)
      return 0;
   return 1;
}




void AStarPP::randomizeGrid() 
{
   for (int x=0; x<_sizeX; x++)
      for (int y=0; y<_sizeY; y++)
         _values[x][y] = rand()/(double)RAND_MAX;
}


void AStarPP::buildTestGrid() 
{
   for (int x=0; x<_sizeX; x++)
      for (int y=0; y<_sizeY; y++)
         if (x==4 && (y>=0 && y<70) )
            _values[x][y] = 1;
         else
            _values[x][y] = 0;
}


double** AStarPP::getDataPtr()
{
   return _values;
}

float AStarPP::invSqrt(float x) {
   float xHalf = 0.5 * x;
   int i = *(int*)&x;
   i = 0x5f375a86 - (i>>1);
   x = *(float*)&i;
   x = x*(1.5 - xHalf*x*x);
   return x;
}




void AStarPP::updateWallBalance()
{
   double parRight = 0;
   double lastDistRight = -1;
   double countRight = 0;

   double parLeft = 0;
   double lastDistLeft = -1;
   double countLeft = 0;

   for (int y = _sizeY/4; y < (_sizeY*3)/4; y++) {
      double distFoundRight = -1;
      for (int x = _sizeX/2; x < _sizeX; x++) {
         if(_values[x][y] > 0) {
            distFoundRight = x - _sizeX/2;
            break;
         }
      }
      if(distFoundRight != -1) {
         if(lastDistRight == -1)
            lastDistRight = distFoundRight;
         else {
            parRight += distFoundRight - lastDistRight;
            countRight++;
         }
      }

      double distFoundLeft = -1;
      for (int x = _sizeX/2; x >= 0; x--) {
         if(_values[x][y] > 0) {
            distFoundLeft = _sizeX/2 - x;
            break;
         }
      }
      if(distFoundLeft != -1) {
         if(lastDistLeft == -1)
            lastDistLeft = distFoundLeft;
         else {
            parLeft += distFoundLeft - lastDistLeft;
            countLeft++;
         }
      }
   }

   if(countRight != 0)
      parRight /= countRight;
   else
      parRight = 0;

   if(countLeft != 0)
      parLeft /= countLeft;
   else
      parLeft = 0;

   double middleDistRight = 0;
   double middleDistLeft = 0;
   double countRight2 = 0;
   double countLeft2 = 0;
   for (int y=_sizeY/4; y< (_sizeY*3)/4; y++) {
      double distFoundRight = -1;
      for (int x=_sizeX/2; x<_sizeX; x++) {
         if(_values[x][y] > 0) {
            distFoundRight = x - _sizeX/2;
            break;
         }
      }
      if(distFoundRight != -1) {
         middleDistRight += distFoundRight;
         countRight2++;
      }

      double distFoundLeft = -1;
      for (int x=_sizeX/2; x>=0; x--) {
         if(_values[x][y] > 0) {
            distFoundLeft = _sizeX/2 - x;
            break;
         }
      }
      if(distFoundLeft != -1) {
         middleDistLeft += distFoundLeft;
         countLeft2++;
      }
   }
   if(countRight2 != 0)
      middleDistRight /= countRight2;
   else
      middleDistRight = 0;
   if(countLeft2 != 0)
      middleDistLeft /= countLeft2;
   else
      middleDistLeft = 0;

   rescue_wall_detection_message msg;

   msg.wallParallelnessRight = parRight;
   msg.wallParallelnessLeft = parLeft;
   msg.middleDistRight = middleDistRight;
   msg.middleDistLeft = middleDistLeft;
   msg.robot.id = getRobotId();
   msg.robot.ts = getCurrentTime();

   IPC_publishData(RESCUE_WALL_DETECTION_NAME, &msg);
}

/*********************************************************************
 * (C) Copyright 2006 Albert Ludwigs University Freiburg
 *     Institute of Computer Science
 *
 * All rights reserved. Use of this software is permitted for
 * non-commercial research purposes, and it may be copied only
 * for that use. All copies must include this copyright message.
 * This software is made available AS IS, and neither the authors
 * nor the Albert Ludwigs University Freiburg make any warranty
 * about the software or its performance.
 *********************************************************************/