Exemplo n.º 1
0
void GridWorld::updateVertex(GridWorld::Tile*& tile){
	bool isIncosistent = tile->rhs != tile->g; //potential problem with floating point comparison?

	if (isIncosistent && tile->isOpen){
		//tile->h = calculateH(tile);
		tile->key = calculateKey(tile);
		make_heap(open.begin(), open.end(), GridWorld::compareTiles);

	}
	else if (isIncosistent && !tile->isOpen){
		//tile->h = calculateH(tile);
		tile->key = calculateKey(tile);
		tile->isOpen = true;

		open.push_back(tile);
		push_heap(open.begin(), open.end(), GridWorld::compareTiles);

	}
	else if (!isIncosistent && tile->isOpen){
		open.erase(std::find(open.begin(), open.end(), tile));
		make_heap(open.begin(), open.end(), GridWorld::compareTiles);
		tile->isOpen = false;
	}

}
Exemplo n.º 2
0
/* int Dstar::computeShortestPath()
 * --------------------------
 * As per [S. Koenig, 2002] except for 2 main modifications:
 * 1. We stop planning after a number of steps, 'maxsteps' we do this
 *    because this algorithm can plan forever if the start is
 *    surrounded by obstacles.
 * 2. We lazily remove states from the open list so we never have to
 *    iterate through it.
 */
int Dstar::computeShortestPath() {

  list<state> s;
  list<state>::iterator i;

  if (openList.empty()) return 1;

  int k=0;
  while ((!openList.empty()) &&
         (openList.top() < (s_start = calculateKey(s_start))) ||
         (getRHS(s_start) != getG(s_start))) {

    if (k++ > maxSteps) {
      fprintf(stderr, "At maxsteps\n");
      return -1;
    }


    state u;

    bool test = (getRHS(s_start) != getG(s_start));

    // lazy remove
    while(1) {
      if (openList.empty()) return 1;
      u = openList.top();
      openList.pop();

      if (!isValid(u)) continue;
      if (!(u < s_start) && (!test)) return 2;
      break;
    }

    ds_oh::iterator cur = openHash.find(u);
    openHash.erase(cur);

    state k_old = u;

    if (k_old < calculateKey(u)) { // u is out of date
      insert(u);
    } else if (getG(u) > getRHS(u)) { // needs update (got better)
      setG(u,getRHS(u));
      getPred(u,s);
      for (i=s.begin();i != s.end(); i++) {
        updateVertex(*i);
      }
    } else {   // g <= rhs, state has got worse
      setG(u,INFINITY);
      getPred(u,s);
      for (i=s.begin();i != s.end(); i++) {
        updateVertex(*i);
      }
      updateVertex(u);
    }
  }
  return 0;
}
Exemplo n.º 3
0
/* int Dstar::computeShortestPath()
 * --------------------------
 * As per [S. Koenig, 2002] except for 2 main modifications:
 * 1. We stop planning after a number of steps, 'maxsteps' we do this
 *    because this algorithm can plan forever if the start is
 *    surrounded by obstacles. 
 * 2. We lazily remove states from the open list so we never have to
 *    iterate through it.
 */
int Dstar::computeShortestPath() {
  
  list<state> s;
  list<state>::iterator i;

  if (openList.empty()) return 1;

  int k=0;
  while ((!openList.empty()) && 
         (openList.top() < (calculateKey(s_start))) || 
         (!isConsistent(s_start))) {

    if (k++ > maxSteps) {
      fprintf(stderr, "At maxsteps\n");
      return -1;
    }
    
    
    state u;
    
    // check consistency (one of the loop conditions)
    bool test = isConsistent(s_start);
    //(getRHS(s_start) != getG(s_start));
    
    // lazy remove
    while(1) { 
      if (openList.empty()) return 1; // checks outer loop condition #1
      u = openList.top();
      
      if (!queuePop()) continue;
      
      if (!(u < s_start) && test) return 2; // checks outer loop conditions #2,3 still hold
    
      break;
    }
    
    state k_old = u;

    if (k_old < calculateKey(u)) { // u is out of date
      insert(u); // u has been removed already, reinsert into pq with new key
    } else if (getG(u) > getRHS(u)) { // needs update (got better)
      setG(u,getRHS(u));
      getPred(u,s);
      for (i=s.begin();i != s.end(); i++) {
        updateVertex(*i);
      }
    } else {   // g <= rhs, state has got worse
      setG(u,INFINITY);
      getPred(u,s);
      for (i=s.begin();i != s.end(); i++) {
        updateVertex(*i);
      }
      updateVertex(u);
    }
  }
  return 0;
}
Exemplo n.º 4
0
/* void Dstar::init(int sX, int sY, int gX, int gY)
 * --------------------------
 * Init dstar with start and goal coordinates, rest is as per
 * [S. Koenig, 2002]
 */
void Dstar::init(int sX, int sY, int gX, int gY) {

  cellHash.clear();
  path.clear();
  openHash.clear();
  while(!openList.empty()) openList.pop();

  k_m = 0;

  s_start.x = sX;
  s_start.y = sY;
  s_goal.x  = gX;
  s_goal.y  = gY;

  cellInfo tmp;
  tmp.g = tmp.rhs =  0;
  tmp.cost = C1;

  cellHash[s_goal] = tmp;

  tmp.g = tmp.rhs = heuristic(s_start,s_goal);
  tmp.cost = C1;
  cellHash[s_start] = tmp;
  s_start = calculateKey(s_start);

  s_last = s_start;

}
Exemplo n.º 5
0
/* void Dstar::updateStart(int x, int y)
 * --------------------------
 * Update the position of the robot, this does not force a replan.
 */
void Dstar::updateStart(int x, int y) {

  s_start.x = x;
  s_start.y = y;

  k_m += heuristic(s_last,s_start);

  s_start = calculateKey(s_start);
  s_last  = s_start;

}
Exemplo n.º 6
0
void GridWorld::updateVertex(GridWorld::Tile*& tile){
	bool isIncosistent = tile->rhs != tile->g;

	if (isIncosistent && tile->isOpen){
		tile->key = calculateKey(tile);
		make_heap(open.begin(), open.end(), GridWorld::compareTiles);

	}
	else if (isIncosistent && !tile->isOpen){
		tile->key = calculateKey(tile);
		tile->isOpen = true;

		open.push_back(tile);
		push_heap(open.begin(), open.end(), GridWorld::compareTiles);

	}
	else if (!isIncosistent && tile->isOpen){
		open.erase(std::find(open.begin(), open.end(), tile));
		make_heap(open.begin(), open.end(), GridWorld::compareTiles);
		tile->isOpen = false;
	}

}
Exemplo n.º 7
0
/* void Dstar::updateGoal(int x, int y)
 * --------------------------
 * This is somewhat of a hack, to change the position of the goal we
 * first save all of the non-empty on the map, clear the map, move the
 * goal, and re-add all of non-empty cells. Since most of these cells
 * are not between the start and goal this does not seem to hurt
 * performance too much. Also it free's up a good deal of memory we
 * likely no longer use.
 */
void Dstar::updateGoal(int x, int y) {
   
  list< pair<ipoint2, double> > toAdd;
  pair<ipoint2, double> tp;
  
  ds_ch::iterator i;
  list< pair<ipoint2, double> >::iterator kk;
  
  for(i=cellHash.begin(); i!=cellHash.end(); i++) {
    if (!near(i->second.cost, C1)) {
      tp.first.x = i->first.x;
      tp.first.y = i->first.y;
      tp.second = i->second.cost;
      toAdd.push_back(tp);
    }
  }

  cellHash.clear();
  openHash.clear();

  while(!openList.empty())
    openList.pop();
  
  k_m = 0;
  
  s_goal.x  = x;
  s_goal.y  = y;

  cellInfo tmp;
  tmp.g = tmp.rhs =  0;
  tmp.cost = C1;

  cellHash[s_goal] = tmp;
  insert(s_goal);

  tmp.g = tmp.rhs = heuristic(s_start,s_goal);
  tmp.cost = C1;
  cellHash[s_start] = tmp;
  s_start = calculateKey(s_start);

  s_last = s_start;    

  for (kk=toAdd.begin(); kk != toAdd.end(); kk++) {
    updateCell(kk->first.x, kk->first.y, kk->second);
  }
  

}
Exemplo n.º 8
0
/* void Dstar::insert(state u)
 * --------------------------
 * Inserts state u into openList and openHash.
 */
void Dstar::insert(state u) {

  ds_oh::iterator cur;
  float csum;

  u    = calculateKey(u);
  cur  = openHash.find(u);
  csum = keyHashCode(u);
  // return if cell is already in list. TODO: this should be
  // uncommented except it introduces a bug, I suspect that there is a
  // bug somewhere else and having duplicates in the openList queue
  // hides the problem...
  //if ((cur != openHash.end()) && (close(csum,cur->second))) return;

  openHash[u] = csum;
  openList.push(u);
}
Exemplo n.º 9
0
/* void Dstar::insert(state u) 
 * --------------------------
 * Inserts state u into openList and openHash.
 */
void Dstar::insert(state u) {

  ds_oh::iterator cur;
  cur  = openHash.find(u);
  int num;

  if (cur == openHash.end()) {
    num = 1;
    ivec2 val;
    val.v[0] = val.v[1] = 1;
    openHash[u] = val;
  } else {
    cur->second.v[0]++;
    cur->second.v[1]++; // = cur->second.v[0];
    num = cur->second.v[1];
  }
  
  calculateKey(u);
  u.num = num;
  openList.push(u);
} 
Exemplo n.º 10
0
void Planner::updateVertex(list<Uelem> &U, Grid &u, vector<vector<int> > &g, vector<vector<int> > &rhs, Map &map,int km)
{
    // update the vertex value and update the priority queue
    if(u!=map.goal)
    {
        vector<Grid> neighbour = findNeighbour(map, u, 8);
        vector<float> dist;
        
        for_each(neighbour.begin(),neighbour.end(),[&](Grid &it){dist.push_back((float)g[it.y][it.x]+calculateDistance(it,u));});
        
        rhs[u.y][u.x]=*min_element(dist.begin(),dist.end());
    }
    
    auto pt =find_if(U.begin(),U.end(),[u](Uelem &pt){return pt.point==u;});
    if (pt!=U.end()) U.erase(pt);
    if (g[u.y][u.x]!=rhs[u.y][u.x])
    {
        Key knew = calculateKey(u,g,rhs,map.start,0);
        auto pt = find_if(U.begin(),U.end(),[&](Uelem &a){return a.key<knew;});
        U.insert(pt,{u,knew});
    }
}
Exemplo n.º 11
0
// This sets up the global data structures to perform a new search from scratch.
// It will clean up any old things that were part of a previous search. 
// This must be called after the map has changed size or the goal changes. 
// It assumes that raw_map, robot and goal pose all exist.
// Returns true when successful
// if reuse_map == true, then the map info is saved (saves on map dilation time, but only should be used for a goal change)
bool initialize_search(bool reuse_map)
{  
   printf("Initializing Search to Goal, building search tree. \n");
    
   // in case of re-initialization, clean up old values
   if(primaryCellPathHeap != NULL)
     cpDeleteHeap(primaryCellPathHeap);
   if(secondaryCellPathHeap != NULL)
     cpDeleteHeap(secondaryCellPathHeap);
   if(heapNode != NULL)
     deleteHeap(); 
   if(graph != NULL)
   {
     if(reuse_map)
       deleteGraphAndMap(false);
     else
       deleteGraphAndMap();  
   }
   else
     reuse_map = false; // if graph is null, then map is most likely not allocated

   // init map and graph
   buildGraphAndMap(raw_map->height, raw_map->width);

   // load raw_map data into the new search map, dilating it by 1/2 robot rad + safety distance
   if(!reuse_map)
     populate_map_from_raw_map();
   
   // remember start and goal locations
   double resolution = raw_map->resolution;
   int goalN = goal_pose->y/resolution; // need to add global offset ability
   if(goalN > (int)raw_map->height)
     goalN = (int)raw_map->height;
   if(goalN < 0)
     goalN = 0;
   
   int goalM = goal_pose->x/resolution; // need to add global offset ability
   if(goalM > (int)raw_map->width)
     goalM = (int)raw_map->width;
   if(goalM < 0)
     goalM = 0;
        
   int robotN = robot_pose->y/resolution; // need to add global offset ability
   if(robotN > (int)raw_map->height)
     robotN = (int)raw_map->height;
   if(robotN < 0)
     robotN = 0;
   
   int robotM = robot_pose->x/resolution; // need to add global offset ability
   if(robotM > (int)raw_map->width)
     robotM = (int)raw_map->width;
   if(robotM < 0)
     robotM = 0;
    
   // find the exact coordinates of the robot
   robot_pos_x = robot_pose->x/resolution; // need to add global offset ability
   if(robot_pos_x > raw_map->width)
     robot_pos_x = raw_map->width;
   if(robot_pos_x < 0)
     robot_pos_x = 0;
   
   robot_pos_y = robot_pose->y/resolution; // need to add global offset ability
   if(robot_pos_y > raw_map->width)
     robot_pos_y = raw_map->width;
   if(robot_pos_y < 0)
     robot_pos_y = 0;

   // init heap
   buildHeap(); 

   // init search values
   s_start = &graph[robotN][robotM];
   s_start->g = LARGE;
   s_start->rhs = LARGE;
   
   s_goal = &graph[goalN][goalM];
   s_goal->g = LARGE;
   s_goal->rhs = 0;
   insert(s_goal,calculateKey(s_goal));  

   // compute the initial field costs
   computeShortestPath();

   // create the cellPath search and look ahead heaps
   primaryCellPathHeap = cpBuildHeap();
   secondaryCellPathHeap = cpBuildHeap();
   
   return true;
}
Exemplo n.º 12
0
vector<Grid> Planner::dStarLite(Map &map)
{
    // this is the implementation of the D star lite algorithm
    
    vector<Grid> wayPoint;

    
    // Initialization
    vector<vector<int> > g(map._rows,vector<int>(map._cols,map._rows*map._cols+1));
    vector<vector<int> > rhs(map._rows,vector<int>(map._cols,map._rows*map._cols+1));
    km =0;
    rhs[map.goal.y][map.goal.x] =0;
    Key n = calculateKey(map.goal,g,rhs,map.start,0);
    U.push_back({map.goal,n});
    
    
    // Computer current shortest path
    // Update the states of the map if the first element in the priority list can be updated or the goal is not reached
    while(U.front().key<calculateKey(map.goal,g,rhs,map.start,0)||(rhs[map.start.y][map.start.x]!=g[map.start.y][map.start.x]))
          {
              // take the key value and postion of the first element in the priority queue
              Key kold = U.front().key;
              Grid u = U.front().point;
              
              U.pop_front();
              Key knew = calculateKey(u,g,rhs,map.start,km); // calculate the new key value
              
              // update map if old value is different from the new value
              if (kold<knew)
              {
                  // if the new key is larger, the cost of the edge of the grid might be change
                  // the current grid should be updated and re-expanded
                  // insert it in the priority queue
                  auto pt =find_if(U.begin(),U.end(),[knew](Uelem &u){return u.key<knew;});
                  U.insert(pt,{u,knew});
              }
              else if (g[u.y][u.x]>rhs[u.y][u.x])
              {
                  // if the grid is overconstraint, there are new shorter paths detected
                  g[u.y][u.x] = rhs[u.y][u.x];
                  
                  // update all its neighbour value
                  vector<Grid> neightbour = findNeighbour(map, u, 8);
                  for(auto &n:neightbour)
                  {
                      updateVertex(U,n,g,rhs,map,km);
                  }
              }
              else
              {
                  // if the grid is underconstraint, the grid it self and its neightbour should all be updated
                  g[u.y][u.x] = map._cols*map._rows;
                  vector<Grid> neightbour = findNeighbour(map, u, 8);
                  for(auto &n:neightbour)
                  {
                      updateVertex(U,n,g,rhs,map,km);
                  }
                  updateVertex(U,u,g,rhs,map,km);
              }
              
                  
              
              
              
          }
    
    return wayPoint;
}
Exemplo n.º 13
0
bool GridWorld::computeShortestPath(){
	if (open.empty()){
		std::cout << "ERROR: No tiles to expand on... can't do anything" << std::endl;
		return false;
	}


	while (!open.empty() && (compareKeys(open.front()->key, goal->key = calculateKey(goal)) || goal->rhs != goal->g)){
		Tile* current = open.front();
		//Notice that CURRENT wasn't pop/removed yet

		//std::cout << "Expanding:";
		//current->info();
		//std::cout << std::endl;
		KeyPair k_new = calculateKey(current);

		if (compareKeys(current->key, k_new)){
			//Tiles under this branch will have to be updated as incremental search has happened
			current->key = k_new;
			make_heap(open.begin(), open.end(), GridWorld::compareTiles);

		}
		else if (current->g > current->rhs){
			//Majority of the tiles will fall under this conditional branch as
			//it undergoes normal A* pathfinding

			current->g = current->rhs;

			open.erase(open.begin());
			make_heap(open.begin(), open.end(), GridWorld::compareTiles);
			current->isOpen = false;

			std::vector<Tile*> neighbours(getNeighbours(current));
			for (int i = 0; i < neighbours.size(); i++){
				if (neighbours[i] != start && neighbours[i]->rhs > current->g + calculateC(current, neighbours[i])){
					neighbours[i]->successor = current;
					neighbours[i]->rhs = current->g + calculateC(current, neighbours[i]);
					updateVertex(neighbours[i]);
				}
			}

		}
		else {
			//Tiles under this branch will need to be updated during incremental search
			current->g = PF_INFINITY;

			//Update CURRENT
			if (current != start && current->successor == current){
				TilePair minSucc(getMinSuccessor(current));
				current->rhs = minSucc.second;
				current->successor = current->rhs == PF_INFINITY ? 0 : minSucc.first;
			}
			updateVertex(current);

			//Update NEIGHBOURS
			std::vector<Tile*> neighbours(getNeighbours(current));
			for (int i = 0; i < neighbours.size(); i++){
				if (neighbours[i] != start && neighbours[i]->successor == current){
					TilePair minSucc(getMinSuccessor(neighbours[i]));
					neighbours[i]->rhs = minSucc.second;
					neighbours[i]->successor = neighbours[i]->rhs == PF_INFINITY ? 0 : minSucc.first;
				}
				updateVertex(neighbours[i]);
			}

		}
		//Uncomment this to see CURRENT'S new values
		//std::cout << "Expanded:";
		//current->info();
		//std::cout << std::endl;
	}
	return true;
}
Exemplo n.º 14
0
int 
main(int argc, char **argv) 
{  
  printf( "skv_client::entering main \n" ); fflush( stdout );

  if( argc != 2 )
    {
    printf("skv_test_n_inserts_retrieves::main():: ERROR:: argc: %d", argc);
    exit(1);
    }
    
  char* ConfigFile = argv[ 1 ];

  int Rank = 0;
  int NodeCount = 1;


  /*****************************************************************************
   * Init MPI
   ****************************************************************************/ 
#ifndef SKV_CLIENT_UNI
  MPI_Init( &argc, &argv );
  MPI_Comm_rank( MPI_COMM_WORLD, &Rank );
  MPI_Comm_size( MPI_COMM_WORLD, &NodeCount );
  printf(" %d: MPI_Init complete\n", Rank);
#endif
  /****************************************************************************/ 



  /*****************************************************************************
   * Init the SKV Client
   ****************************************************************************/ 
  skv_status_t status = SKV_Init( Rank,
#ifndef SKV_CLIENT_UNI
                                    MPI_COMM_WORLD,
#endif
                                    0,
                                    ConfigFile,
                                    &Client );

  if( status == SKV_SUCCESS )
    {
    BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: SKV Client Init succeded ");
    }
  else
    {
    BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: SKV Client Init FAILED ");
    }  
  /****************************************************************************/ 



  /*****************************************************************************
   * Connect to the SKV Server
   ****************************************************************************/ 
  BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: About to connect ");

  status = SKV_Connect( Client, ConfigFile, 0 );

  if( status == SKV_SUCCESS )
    {
    BegLogLine( SKV_TEST_LOG,  "skv_test_n_inserts_retrieves::main():: SKV Client connected to server");
    }
  else
    {
    BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: SKV Client FAILED to connect ");
    }
  /****************************************************************************/ 




  /*****************************************************************************
   * Open a test PDS
   ****************************************************************************/
  char MyTestPdsName[ SKV_MAX_PDS_NAME_SIZE ];
  bzero( MyTestPdsName, SKV_MAX_PDS_NAME_SIZE );

  if( Rank >= 0 )
    {
      /* struct timespec ts; */

      //clock_gettime( CLOCK_REALTIME, &ts );

      //sprintf( MyTestPdsName, "TestPds_%08X_%08X", ts.tv_sec, ts.tv_nsec );
      sprintf( MyTestPdsName, "TestPds" );
    }
#if 0
  BegLogLine( SKV_TEST_LOG )
    << "skv_test_n_inserts_retrieves::main():: Before MPI_Bcast() "
    << EndLogLine;

  MPI_Bcast( MyTestPdsName,
             SKV_MAX_PDS_NAME_SIZE,
             MPI_CHAR, 
             0,
             MPI_COMM_WORLD );

  BegLogLine( SKV_TEST_LOG )
    << "skv_test_n_inserts_retrieves::main():: About to open pds name: "
    << " MyTestPdsName: " << MyTestPdsName
    << EndLogLine;

  MPI_Barrier( MPI_COMM_WORLD );
#endif

  BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: About to open pds");

  skv_pds_hdl_t  MyPDSId;
  status = SKV_Open( Client,
                      MyTestPdsName,
                      (skv_pds_priv_t) (SKV_PDS_READ | SKV_PDS_WRITE),
                      (skv_cmd_open_flags_t) SKV_COMMAND_OPEN_FLAGS_CREATE,
                      & MyPDSId );

  if( status == SKV_SUCCESS )
    {
    BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: SKV Client successfully opened");
    }
  else
    {
    BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: SKV Client FAILED to open");
    }
  /****************************************************************************/


  int* DataSizes = NULL;
  int  DataSizeCount = 0;
  getDataSizes( & DataSizes, & DataSizeCount, 0, MAX_TEST_SIZE_EXP );

#ifndef SKV_CLIENT_UNI
  MPI_Barrier( MPI_COMM_WORLD );
#endif

  uint64_t NUMBER_OF_TRIES = 0;
  uint64_t NUMBER_OF_SNODES = SKVTestGetServerCount();

  for( int sizeIndex=SKV_TEST_START_INDEX; sizeIndex < DataSizeCount; sizeIndex++ )
    {
      int testDataSize = DataSizes[ sizeIndex ];

      uint64_t total_records = (uint64_t)STORAGE_SPACE_PER_SNODE / (uint64_t) testDataSize * (uint64_t)NUMBER_OF_SNODES;
      NUMBER_OF_TRIES = total_records / NodeCount;

      if( NUMBER_OF_TRIES > MAX_NUMBER_OF_TRIES_PER_SRV * NUMBER_OF_SNODES )
        NUMBER_OF_TRIES = MAX_NUMBER_OF_TRIES_PER_SRV * NUMBER_OF_SNODES;

      printf( "running test with data size: %d\n", testDataSize );

      /*****************************************************************************
       * Allocate Insert/Retrieve data arrays
       ****************************************************************************/
      skv_async_command_helper_t commandHelpers[ NUMBER_OF_TRIES ];

#ifdef DO_CHECK
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {

          commandHelpers[ t ].mBufferSize = testDataSize;
          commandHelpers[ t ].mBuffer = (char *) malloc( testDataSize );
          if( commandHelpers[ t ].mBuffer == NULL )    
            {
            BegLogLine( 1, "error allocating command handles");
            exit(1);
            }

          /*****************************************************************************
           * Insert Key / Value
           ****************************************************************************/
          for( int i=0; i < testDataSize; i++ )
            {
              char ch = i;
              commandHelpers[ t ].mBuffer[ i ] = calculateValue( Rank, ch, t );
            }
        }
#else
      char* Buffer = (char *) malloc( testDataSize );
#endif

      double InsertTimeStart = MPI_Wtime();
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {      
          // int Key = Rank * NUMBER_OF_TRIES + t;
          int Key = calculateKey( Rank, sizeIndex, t, NUMBER_OF_TRIES );
          // int Key = ( Rank * DataSizeCount * NUMBER_OF_TRIES) 
          //   + (sizeIndex * NUMBER_OF_TRIES ) 
          //   + t;

          BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: About to Insert");

          status = SKV_Iinsert( Client,
                                 &MyPDSId,
                                 (char *) &Key,
                                 sizeof( int ),
#ifdef DO_CHECK
                                 commandHelpers[ t ].mBuffer,
                                 commandHelpers[ t ].mBufferSize,
#else
                                 Buffer, 
                                 testDataSize,
#endif
                                 0,
                                 SKV_COMMAND_RIU_FLAGS_NONE,
                                 & ( commandHelpers[ t ].mCommandHdl ) );

          if( status == SKV_SUCCESS )
            {

#ifdef DO_CHECK
              uintptr_t *buffer = (uintptr_t*)commandHelpers[ t ].mBuffer;
#else
              uintptr_t *buffer = (uintptr_t*)Buffer;
#endif
              BegLogLine( SKV_TEST_LOG, "insert key: ");
              BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: SKV Client successfully posted Insert command ");
            }
          else
            {
            BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: SKV Client FAILED to Insert: ");

            }  

#ifdef SLOWDOWN
          usleep(SLOWDOWN);
#endif
        }
      /****************************************************************************/



      BegLogLine( SKV_TEST_LOG, "Waiting for Insert Completion");

      /****************************************************************************
       * Wait for insert commands to finish
       ****************************************************************************/
      skv_client_cmd_ext_hdl_t CommandHdl;
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {
          CommandHdl = commandHelpers[ t ].mCommandHdl;
          status = SKV_Wait( Client, CommandHdl );
          BegLogLine( SKV_TEST_LOG, "Insert command completed: ");
        }

      double InsertTime = MPI_Wtime() - InsertTimeStart;
      /****************************************************************************/


#ifndef DONT_RETRIEVE
      // for( int nRet = 0; nRet < 10000; nRet++ ) {
      /*****************************************************************************
       * Retrieve Key / Value
       ****************************************************************************/
#ifdef DO_CHECK
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {
          bzero( commandHelpers[ t ].mBuffer, commandHelpers[ t ].mBufferSize );
        }
#endif

      double RetrieveTimeStart = MPI_Wtime();      
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {
          int RetrieveSize = -1;
          // int Key = Rank * NUMBER_OF_TRIES + t;
          int Key = calculateKey( Rank, sizeIndex, t, NUMBER_OF_TRIES );
          // int Key = ( Rank * DataSizeCount * NUMBER_OF_TRIES) 
          //   + (sizeIndex * NUMBER_OF_TRIES ) 
          //   + t;
#ifndef DO_CHECK
          int RetrivedSize = 0;
#endif
          status = SKV_Iretrieve( Client, &MyPDSId,
                                   (char *) &Key,
                                   (int) sizeof( int ),
#ifdef DO_CHECK
                                   commandHelpers[ t ].mBuffer,
                                   commandHelpers[ t ].mBufferSize,
                                   & commandHelpers[ t ].mRetrieveBufferSize,
#else
                                   Buffer, 
                                   testDataSize,
                                   & RetrivedSize,
#endif
                                   0,
                                   SKV_COMMAND_RIU_FLAGS_NONE,
                                   & (commandHelpers[ t ].mCommandHdl) );

          if( status == SKV_SUCCESS )
            {
            BegLogLine( SKV_TEST_LOG, "skv_test_n_inserts_retrieves::main():: SKV Client successfully posted retrieve command:");
            }
          else
            {
            BegLogLine( 1, "skv_test_n_inserts_retrieves::main():: SKV Client FAILED to Retieve: ");
            }  

#ifdef SLOWDOWN
          usleep(SLOWDOWN);
#endif
        }
      /****************************************************************************/




      /****************************************************************************
       * Wait for retrieve commands to finish
       ****************************************************************************/
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {
          CommandHdl = commandHelpers[ t ].mCommandHdl;
          status = SKV_Wait( Client, CommandHdl );

          BegLogLine( SKV_TEST_LOG, "Retrieve command completed: ");

          if( status != SKV_SUCCESS )
            {
            BegLogLine( 1, "Retrieve ERROR: ");
            exit(1);
            }
        }

      double RetrieveTime = MPI_Wtime() - RetrieveTimeStart;
      /****************************************************************************/
      // }
      // double RetrieveTimeStart = 0.0;
      // double RetrieveTime = -10;

#ifdef DO_CHECK
      /*****************************************************************************
       * Check results
       ****************************************************************************/
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {
          skv_async_command_helper_t* commandHelper = & commandHelpers[ t ];

          int TestFailed = 0;
          for( int i=0; i < commandHelper->mBufferSize; i++ )
            {
              char ch = i;
              if( commandHelper->mBuffer[ i ] !=  calculateValue( Rank, ch, t ) )
                {
                BegLogLine( SKV_TEST_LOG, "Retrieve Result does NOT match: ");
                TestFailed = 1;
                break;
                }
            }

          if( TestFailed )
            {
            BegLogLine( 1,  "SKV Client Result Match FAILED :-(" );

            exit(1);
            }
          else
            {
            BegLogLine( SKV_TEST_LOG, "SKV Client Result Match SUCCESSFUL :-)");
            }
        }
#endif
      /****************************************************************************/

#else // DONT_RETRIEVE
      double RetrieveTimeStart = 1;      
      double RetrieveTime = 1000000000;
#endif // DONT_RETRIEVE

      BegLogLine( SKV_TEST_LOG, "Removing Data");

      double RemoveTimeStart = MPI_Wtime();      
      /****************************************************************************/
      /* REMOVE content for next try **/
      /****************************************************************************/
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {
          // int Key = Rank * NUMBER_OF_TRIES + t;
          int Key = calculateKey( Rank, sizeIndex, t, NUMBER_OF_TRIES );

          status = SKV_Iremove( Client,
                                 &MyPDSId,
                                 (char *) &Key,
                                 (int) sizeof( int ),
                                 SKV_COMMAND_REMOVE_FLAGS_NONE,
                                 & ( commandHelpers[ t ].mCommandHdl ) );

          if( (status != SKV_SUCCESS) && (status != SKV_ERRNO_ELEM_NOT_FOUND) )
            {
            BegLogLine( 1, "ERROR: posting remove: " );
            exit(1);
            }

          BegLogLine( SKV_TEST_LOG, "Remove command posted: ");

        }


      /****************************************************************************
       * Wait for retrieve commands to finish
       ****************************************************************************/
      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {
          CommandHdl = commandHelpers[ t ].mCommandHdl;
          status = SKV_Wait( Client, CommandHdl );

          BegLogLine( SKV_TEST_LOG, "Remove command completed: ");
        }

      double RemoveTime = MPI_Wtime() - RemoveTimeStart;
      /****************************************************************************/




      double InsertAvgTime   = ( InsertTime / NUMBER_OF_TRIES );
      double RetrieveAvgTime = ( RetrieveTime / NUMBER_OF_TRIES );
      double RemoveAvgTime   = ( RemoveTime / NUMBER_OF_TRIES );

      double GlobalInsertAvgTime = 0.0;
      double GlobalRetrieveAvgTime = 0.0;
      double GlobalRemoveAvgTime = 0.0;
      MPI_Reduce( & InsertAvgTime, & GlobalInsertAvgTime, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD );
      MPI_Reduce( & RetrieveAvgTime, & GlobalRetrieveAvgTime, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD );
      MPI_Reduce( & RemoveAvgTime, & GlobalRemoveAvgTime, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD );

      GlobalInsertAvgTime = GlobalInsertAvgTime / (1.0 * NodeCount);
      GlobalRetrieveAvgTime = GlobalRetrieveAvgTime / (1.0 * NodeCount);
      GlobalRemoveAvgTime = GlobalRemoveAvgTime / (1.0 * NodeCount);

      double InsertBandwidth   = ( testDataSize / ( GlobalInsertAvgTime * 1024.0 * 1024.0 ) );
      double RetrieveBandwidth = ( testDataSize / ( GlobalRetrieveAvgTime * 1024.0 * 1024.0 ) );

      printf("%s %2.0lf %s %d %s %ld %s %lf %s %lf %s %lf %s %lf %s %lf\n",
             "skv_test_n_inserts_retrieves::main():: TIMING: log2(ValueSize): ", log2( (double) testDataSize ),
             " ValueSize: ",               testDataSize,
             " TryCount: ",                NUMBER_OF_TRIES,
             " InsertAvgTime: ",           GlobalInsertAvgTime,
             " InsertBandwidth: ",         InsertBandwidth,
             " RetrieveAvgTime: ",         GlobalRetrieveAvgTime,
             " RetrieveBandwidth: ",       RetrieveBandwidth,
             " RemoveAvgTime: ",           GlobalRemoveAvgTime);

      for( int t=0; t < NUMBER_OF_TRIES; t++ )
        {

#ifdef DO_CHECK
          free( commandHelpers[ t ].mBuffer );
          commandHelpers[ t ].mBuffer = NULL;
#else
          free( Buffer );
          Buffer = NULL;
#endif
        }

#ifndef SKV_CLIENT_UNI
      MPI_Barrier( MPI_COMM_WORLD );
#endif
    }

  SKV_Disconnect( Client );
  SKV_Finalize( Client );

#ifndef SKV_CLIENT_UNI
  MPI_Finalize();
#endif

  return 0;
}
Exemplo n.º 15
0
bool CWebSocketV8::Handshake(const char* data, size_t length, std::string &response)
{
  std::string strHeader(data, length);
  const char *value;
  HttpParser header;
  if (header.addBytes(data, length) != HttpParser::Done)
  {
    CLog::Log(LOGINFO, "WebSocket [hybi-10]: incomplete handshake received");
    return false;
  }

  // The request must be GET
  value = header.getMethod();
  if (value == NULL || strnicmp(value, WS_HTTP_METHOD, strlen(WS_HTTP_METHOD)) != 0)
  {
    CLog::Log(LOGINFO, "WebSocket [hybi-10]: invalid HTTP method received (GET expected)");
    return false;
  }

  // The request must be HTTP/1.1 or higher
  size_t pos;
  if ((pos = strHeader.find(WS_HTTP_TAG)) == std::string::npos)
  {
    CLog::Log(LOGINFO, "WebSocket [hybi-10]: invalid handshake received");
    return false;
  }

  pos += strlen(WS_HTTP_TAG);
  std::istringstream converter(strHeader.substr(pos, strHeader.find_first_of(" \r\n\t", pos) - pos));
  float fVersion;
  converter >> fVersion;

  if (fVersion < 1.1f)
  {
    CLog::Log(LOGINFO, "WebSocket [hybi-10]: invalid HTTP version %f (1.1 or higher expected)", fVersion);
    return false;
  }

  std::string websocketKey, websocketProtocol;
  // There must be a "Host" header
  value = header.getValue("host");
  if (value == NULL || strlen(value) == 0)
  {
    CLog::Log(LOGINFO, "WebSocket [hybi-10]: \"Host\" header missing");
    return true;
  }

  // There must be a base64 encoded 16 byte (=> 24 byte as base64) "Sec-WebSocket-Key" header
  value = header.getValue(WS_HEADER_KEY_LC);
  if (value == NULL || (websocketKey = value).size() != 24)
  {
    CLog::Log(LOGINFO, "WebSocket [hybi-10]: invalid \"Sec-WebSocket-Key\" received");
    return true;
  }

  // There might be a "Sec-WebSocket-Protocol" header
  value = header.getValue(WS_HEADER_PROTOCOL_LC);
  if (value && strlen(value) > 0)
  {
    std::vector<std::string> protocols = StringUtils::Split(value, ",");
    for (std::vector<std::string>::iterator protocol = protocols.begin(); protocol != protocols.end(); ++protocol)
    {
      StringUtils::Trim(*protocol);
      if (*protocol == WS_PROTOCOL_JSONRPC)
      {
        websocketProtocol = WS_PROTOCOL_JSONRPC;
        break;
      }
    }
  }

  CHttpResponse httpResponse(HTTP::Get, HTTP::SwitchingProtocols, HTTP::Version1_1);
  httpResponse.AddHeader(WS_HEADER_UPGRADE, WS_HEADER_UPGRADE_VALUE);
  httpResponse.AddHeader(WS_HEADER_CONNECTION, WS_HEADER_UPGRADE);
  httpResponse.AddHeader(WS_HEADER_ACCEPT, calculateKey(websocketKey));
  if (!websocketProtocol.empty())
    httpResponse.AddHeader(WS_HEADER_PROTOCOL, websocketProtocol);

  char *responseBuffer;
  int responseLength = httpResponse.Create(responseBuffer);
  response = std::string(responseBuffer, responseLength);
  
  m_state = WebSocketStateConnected;

  return true;
}
Exemplo n.º 16
0
bool GridWorld::computeShortestPath(){
	if (open.empty()){
		std::cout << "ERROR: No tiles to expand on... can't do anything" << std::endl;
		return false;
	}

	double currentRHS, otherG, previousG;

	while (!open.empty() && (compareKeys(open.front()->key, start->key = calculateKey(start)) || start->rhs != start->g)){
		Tile* current = open.front();
		//Notice that CURRENT wasn't pop/removed yet..

		KeyPair k_old = current->key;
		KeyPair k_new = calculateKey(current);

		currentRHS = current->rhs;
		otherG = current->g;

		/*std::cout << "Expanding:";
		current->info();
		std::cout << std::endl;*/

		if (compareKeys(k_old, k_new)){
			//This branch updates tile that were already in the OPEN list originally
			//This branch tends to execute AFTER the else branch
			current->key = k_new;
			make_heap(open.begin(), open.end(), GridWorld::compareTiles);

		}
		else if (otherG > currentRHS){
			//Majority of the execution will fall under this conditional branch as
			//it is undergoing normal A* pathfinding

			current->g = current->rhs;
			otherG = currentRHS;

			open.erase(open.begin());
			make_heap(open.begin(), open.end(), GridWorld::compareTiles);
			current->isOpen = false;

			std::vector<Tile*> neighbours(getNeighbours(current));
			for (int i = 0; i < neighbours.size(); i++){
				if (neighbours[i] != 0){
					if (neighbours[i] != goal){
						neighbours[i]->rhs = std::min(neighbours[i]->rhs, calculateC(current, neighbours[i]) + otherG);
					}
					updateVertex(neighbours[i]);
				}
			}

		}
		else {
			//Execution of this branch updates the tile during incremental search

			previousG = otherG;
			current->g = PF_INFINITY;

			//Update CURRENT'S RHS
			if (current != goal){
				current->rhs = getMinSuccessor(current).second;
			}
			updateVertex(current);

			//Update NEIGHBOUR'S RHS to their minimum successor
			std::vector<Tile*> neighbours(getNeighbours(current));
			for (int i = 0; i < neighbours.size(); i++){
				if (neighbours[i] != 0){
					if (neighbours[i]->rhs == (calculateC(current, neighbours[i]) + previousG) && neighbours[i] != goal){
						neighbours[i]->rhs = getMinSuccessor(neighbours[i]).second;
					}
					updateVertex(neighbours[i]);
				}
			}

		}
		//Uncomment this to see CURRENT'S new values
		//std::cout << "Expanded:";
		//current->info();
		//std::cout << std::endl;
	}
	return true;
}