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;
}
}
/* 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;
}
/* 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;
}
/* 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;
}
/* 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;
}
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;
}
}
/* 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);
}
}
/* 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);
}
/* 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);
}
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});
}
}
// 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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
