int main(int argc, char* argv[]) {
if (argc != 3)
{
puts("Usage: <ICP_SLAM init file> <port>");
return -1;
}
// connect to memdb
db = db_connect(argv[2]);
if (db == -1)
{
puts("control failed to connect to memdb.");
return -1;
}
CConfigFile iniFile(argv[1]); // configurations file
// Load configurations
CMetricMapBuilderICP icp_slam;
icp_slam.ICP_options.loadFromConfigFile(iniFile, "MappingApplication");
icp_slam.ICP_params.loadFromConfigFile(iniFile, "ICP");
icp_slam.initialize();
// pathfinding
int resolution = 8;
PathFinder pathFinder(resolution);
deque<TPoint2D> path;
deque<PathCommand> pathCommands;
// timing
sf::Clock globalClock;
double transitionHoverStartTime;
double startReadLidarTime;
double prevTime = 0.0;
double dt = 0.0;
// THE state of the main control
State state = INIT_HOVER;
State nextState;
double delta_distance = 0.0;
double delta_phi = 0.0;
sf::RenderWindow window(sf::VideoMode(800, 600), "2:00am");
window.setVerticalSyncEnabled(true);
sf::Texture texture;
texture.create(800, 600);
Matrix<sf::Color> pixels(600, 800);
sf::Sprite sprite(texture);
sf::Clock fpsClock;
int frameCount = 0;
float prev_gyroz;
deque<float> past_vx;
deque<float> past_vy;
globalClock.restart();
while (window.isOpen()) { // TODO: What will be our terminal case? target location reached?
sf::Event event;
while (window.pollEvent(event))
{
switch (event.type)
{
case sf::Event::Closed:
window.close();
break;
}
}
double curTime = globalClock.getElapsedTime().asSeconds();
dt = curTime - prevTime;
prevTime = curTime;
// read the navadata from the ardrone
char buffer[1024];
while (db_tryget(db, "navdata", buffer, sizeof(buffer)) != -1)
{
unsigned drone_dt_u;
float cur_gyroz;
sscanf(buffer, "%u,%f,%f,%f,%f,%f,%f",
&drone_dt_u, &vx, &vy, &vz, &gyrox, &gyroy, &cur_gyroz);
// compute the angle
gyrox /= 1000;
gyroy /= 1000;
cur_gyroz /= 1000;
cur_gyroz = fmod(cur_gyroz + 360, 360);
gyroz += angle_diff(prev_gyroz, cur_gyroz);
accumPhi = gyroz;
prev_gyroz = cur_gyroz;
// here goes the terrible part....
double drone_dt = (double)drone_dt_u / 1e6;
past_vx.push_back(vx);
past_vy.push_back(vy);
if (past_vx.size() > 19)
{
past_vx.pop_front();
past_vy.pop_front();
}
CObservationOdometryPtr obs = CObservationOdometry::Create();
obs->odometry = CPose2D(accumX/1000.0f, accumY/1000.0f, 0.0f);
obs->hasEncodersInfo = false;
obs->hasVelocities = false;
// icp_slam.processObservation(obs);
}
accumX += median(past_vx) * dt;
accumY += median(past_vy) * dt;
accumDist = sqrt(pow(accumX, 2.0f) + pow(accumY, 2.0f));
if (fabs(accumX) > fabs(accumY) && accumX < 0)
accumDist = -accumDist;
if (fabs(accumY) > fabs(accumX) && accumY < 0)
accumDist = -accumDist;
if (state == INIT_HOVER)
{
if (globalClock.getElapsedTime().asSeconds() >= 10.0)
{
initialize_feedback();
state = READ_LIDAR;
gyroz = accumPhi = 0.0f;
startReadLidarTime = curTime;
}
else
continue;
}
// get the map object and the grid representation of the map
CMultiMetricMap* curMapEst = icp_slam.getCurrentlyBuiltMetricMap();
COccupancyGridMap2DPtr gridMap = curMapEst->m_gridMaps[0];
// Get the position estimates
CPose3D curPosEst = icp_slam.getCurrentPoseEstimation()->getMeanVal();
// (estimated) X,Y coordinates of the robot, and robot yaw angle (direction)
double robx = curPosEst.x();
double roby = curPosEst.y();
double robphi = curPosEst.yaw();
// Convert real coordinate to grid coordinate points
int gridRobX = gridMap->x2idx(robx);
int gridRobY = gridMap->y2idx(roby);
// state machine actions
if (state == READ_LIDAR)
{
// Extract the laser scan info and convert it into a range scan observation to feed into icp-slam
// Need to define 2 values
// 1.) scan: a vector of floats signalling the distances. Each element is a degree
// 2.) validRange: a vector of ints where 1 signals the reading is good and 0 means its bad (and won't be used)
while (db_tryget(db, "ultrasonic", buffer, sizeof(buffer)) != -1)
{
CObservation2DRangeScanPtr obs = CObservation2DRangeScan::Create();
obs->aperture = M_PI * 2;
obs->scan.resize(360, 0);
obs->validRange.resize(360, 0);
float distances[4];
sscanf(buffer, "%f,%f,%f,%f", distances, distances+1, distances+2, distances+3);
printf("Ultra: %f,%f,%f,%f\n", *distances, *(distances+1), *(distances+2), *(distances+3));
for (int k = 0; k < 4; ++k)
{
int sensorIndex = k * 90;
int startIndex = sensorIndex - 20 + 180;
int endIndex = sensorIndex + 20 + 180;
for (int i = startIndex; i < endIndex; ++i)
{
int j = (i + 360) % 360;
if (distances[k] >= 0)
{
obs->scan[j] = distances[k];
obs->validRange[j] = 1;
}
else
{
obs->scan[j] = distances[k];
obs->validRange[j] = 0;
}
}
}
icp_slam.processObservation(obs);
}
if (curTime - startReadLidarTime >= 5.0)
state = PLAN;
}
else if (state == PLAN)
{
// Perform path finding
pathFinder.update(*gridMap);
bool pathFound = true;
pathFound = pathFinder.findPath(TPoint2D(gridRobX, gridRobY), TPoint2D(gridRobX+40, gridRobY), path);
printf("pathFound: %d\tpath length: %lu\n", pathFound, path.size());
/* path.push_back(TPoint2D(gridRobX, gridRobY));
path.push_back(TPoint2D(gridRobX+1000, gridRobY));
path.push_back(TPoint2D(gridRobX+1000, gridRobY+1000));
path.push_back(TPoint2D(gridRobX, gridRobY+1000));
path.push_back(TPoint2D(gridRobX, gridRobY));
*/
float prevAngle = 0.0f;
puts("PATH!!!!");
for (int i = 1; i < path.size(); i++)
{
float dx = path[i].x - path[i-1].x;
float dy = path[i].y - path[i-1].y;
printf("dx: %f, dy: %f\n", dx, dy);
float angle = (float)fmod(atan2(dy, dx)*57.2957795 + 360, 360);
PathCommand command;
command.delta_phi = angle_diff(prevAngle, angle);
command.delta_distance = sqrt(dx*dx + dy*dy) * 0.4 * 1000;
pathCommands.push_back(command);
prevAngle = angle;
}
for (int i = 0; i < path.size(); ++i)
printf("%f, %f\n", pathCommands[i].delta_phi, pathCommands[i].delta_distance);
delta_phi = pathCommands[0].delta_phi;
delta_distance = pathCommands[0].delta_distance;
pathCommands.pop_front();
initialize_feedback();
state = TURNING;
}
else if (state == TURNING)
{
float k = do_feedback_turn(delta_phi);
printf("delta_phi: %f, turning k: %f\n", delta_phi, k);
if (k != 0.0f)
turn(k);
else
{
nextState = MOVE_FORWARD;
state = TRANSITION_HOVER;
transitionHoverStartTime = curTime;
state = MOVE_FORWARD;
}
}
else if (state == MOVE_FORWARD)
{
float k = do_feedback_forward(delta_distance);
printf("delta_distance: %f, forward k: %f\n", delta_distance, k);
if (k != 0.0f)
moveForward(k);
else if (pathCommands.size() == 0)
state = LAND;
else
{
delta_phi = pathCommands[0].delta_phi;
delta_distance = pathCommands[0].delta_distance;
pathCommands.pop_front();
initialize_feedback();
nextState = TURNING;
state = TRANSITION_HOVER;
transitionHoverStartTime = curTime;
}
}
else if (state == HOVER)
{
hover();
}
else if (state == TRANSITION_HOVER)
{
hover();
if (curTime - transitionHoverStartTime > 2.0)
{
state = nextState;
initialize_feedback();
}
}
else if (state == LAND)
{
land();
}
// windows drawing
window.clear(sf::Color::White);
sf::View view;
view.setSize(800, 600);
view.setCenter(gridRobX, gridRobY);
window.setView(view);
// draw the grayscale probability map
int yStart = max(0, gridRobY - 300);
int yEnd = min((int)gridMap->getSizeY(), gridRobY + 300);
int xStart = max(0, gridRobX - 400);
int xEnd = min((int)gridMap->getSizeX(), gridRobX + 400);
for (int y = yStart; y < yEnd; ++y)
{
for (int x = xStart; x < xEnd; ++x)
{
sf::Color &color = pixels(y-yStart, x-xStart);
sf::Uint8 col = gridMap->getCell(x, y) * 255;
color.r = col;
color.g = col;
color.b = col;
}
}
texture.update((sf::Uint8*)pixels.getData());
sprite.setPosition(xStart, yStart);
window.draw(sprite);
// draw the robot's position
sf::CircleShape circle(5);
circle.setPosition(gridRobX-resolution/2, gridRobY-resolution/2);
circle.setOutlineColor(sf::Color::Red);
circle.setFillColor(sf::Color::Red);
window.draw(circle);
// draw the path
std::vector<sf::Vertex> verticies;
verticies.resize(path.size() + 1);
verticies[0].position.x = (gridRobX / resolution) * resolution + resolution/2;
verticies[0].position.y = (gridRobY / resolution) * resolution + resolution/2;
verticies[0].color = sf::Color::Blue;
for (unsigned i = 0; i < path.size(); ++i)
{
verticies[i+1].position.x = path[i].x * resolution + resolution / 2;
verticies[i+1].position.y = path[i].y * resolution + resolution / 2;
verticies[i+1].color = sf::Color::Blue;
}
window.draw(&verticies[0], verticies.size(), sf::LinesStrip);
// draw the grid representation (only the occupied cells)
sf::Color col = sf::Color::Red;
col.a = 128;
for (unsigned y = 0; y < pathFinder.occupancyGrid.height(); ++y)
{
for (unsigned x = 0; x < pathFinder.occupancyGrid.width(); ++x)
{
if (!pathFinder.occupancyGrid(y, x)) continue;
sf::RectangleShape rect;
rect.setPosition(x * resolution, y * resolution);
rect.setSize(sf::Vector2f(resolution, resolution));
rect.setFillColor(col);
window.draw(rect);
}
}
window.display();
frameCount++;
if (fpsClock.getElapsedTime().asSeconds() >= 1.0f)
{
char windowTitle[32];
sprintf(windowTitle, "%d fps", frameCount);
window.setTitle(windowTitle);
fpsClock.restart();
frameCount = 0;
}
}
puts("exiting?!");
return 0;
}
// ------------------------------------------------------
// BenchmarkGridmaps
// ------------------------------------------------------
void BenchmarkGridmaps()
{
randomGenerator.randomize(333);
CMultiMetricMap metricMap;
TSetOfMetricMapInitializers mapInit;
// Create gridmap:
mapInit.loadFromConfigFile( CConfigFile(iniFile), "METRIC_MAPS" );
metricMap.setListOfMaps(&mapInit);
// prepare the laser scan:
CObservation2DRangeScan scan1;
scan1.aperture = M_PIf;
scan1.rightToLeft = true;
scan1.validRange.resize( SCANS_SIZE );
scan1.scan.resize(SCANS_SIZE);
ASSERT_( sizeof(SCAN_RANGES_1) == sizeof(float)*SCANS_SIZE );
memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) );
memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) );
#if 1
CRawlog rawlog;
rawlog.loadFromRawLogFile("/Trabajo/Code/MRPT/share/mrpt/datasets/2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog");
scan1 = *rawlog.getAsObservations(400)->getObservationByClass<CObservation2DRangeScan>();
#endif
ASSERT_( metricMap.m_gridMaps.size() );
COccupancyGridMap2DPtr gridMap = metricMap.m_gridMaps[0];
COccupancyGridMap2D gridMapCopy( *gridMap );
int i, N;
CTicTac tictac;
// test 1: getcell
// ----------------------------------------
if (1)
{
N = 10000000;
cout << "Running test #1: getCell... "; cout.flush();
//COccupancyGridMap2D::cellType cell;
float p=0;
tictac.Tic();
for (i=0;i<N;i++)
{
p += gridMap->getCell( 0, 0 );
}
double T = tictac.Tac();
cout << "-> " << 1e9*T/N << " ns/iter. p=" << p << endl; // the "p" is to avoid optimizing out the entire loop!
}
// test 2: setcell
// ----------------------------------------
if (1)
{
N = 10000000;
cout << "Running test #2: setCell... "; cout.flush();
float p=0.8f;
tictac.Tic();
for (i=0;i<N;i++)
{
gridMap->setCell( 0, 0, p );
}
double T = tictac.Tac();
cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop!
}
// test 3: updateCell
// ----------------------------------------
if (1)
{
N = 1000000;
cout << "Running test #3: updateCell... "; cout.flush();
float p=0.57f;
tictac.Tic();
for (i=0;i<N;i++)
{
gridMap->updateCell( 0, 0, p );
}
double T = tictac.Tac();
cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop!
}
// test 4: updateCell_fast
// ----------------------------------------
if (1)
{
N = 10000000;
cout << "Running test #4: updateCell_fast... "; cout.flush();
float p=0.57f;
COccupancyGridMap2D::cellType logodd_obs = COccupancyGridMap2D::p2l( p );
//float p_1 = 1-p;
COccupancyGridMap2D::cellType *theMapArray = gridMap->getRow(0);
unsigned theMapSize_x = gridMap->getSizeX();
COccupancyGridMap2D::cellType logodd_thres_occupied = COccupancyGridMap2D::OCCGRID_CELLTYPE_MIN+logodd_obs;
tictac.Tic();
for (i=0;i<N;i++)
{
COccupancyGridMap2D::updateCell_fast_occupied( 2, 2, logodd_obs,logodd_thres_occupied, theMapArray, theMapSize_x);
}
double T = tictac.Tac();
cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop!
}
#if 0
for (i=50;i<51;i++)
{
CPose3D pose3D(0.21,0.34,0,-2);
//scan1.validRange.assign(scan1.validRange.size(), false);
//scan1.validRange[i]=true;
gridMap->clear();
gridMap->resizeGrid(-5,20,-15,15);
gridMap->insertObservation( &scan1, &pose3D );
gridMap->saveAsBitmapFile(format("./gridmap_with_widening_%04i.png",i));
}
#endif
// test 5: Laser insertion
// ----------------------------------------
if (1)
{
gridMap->insertionOptions.wideningBeamsWithDistance = false;
N = 3000;
cout << "Running test #5: Laser insert. w/o widen... "; cout.flush();
tictac.Tic();
for (i=0;i<N;i++)
{
#if 1
CPose2D pose(
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-M_PI,M_PI) );
CPose3D pose3D(pose);
#else
CPose3D pose3D;
#endif
gridMap->insertObservation( &scan1, &pose3D );
}
double T = tictac.Tac();
cout << "-> " << 1000*T/N << " ms/iter, scans/sec:" << N/T << endl;
CPose3D pose3D;
gridMap->clear();
gridMap->insertObservation( &scan1, &pose3D );
gridMap->saveAsBitmapFile("./gridmap_without_widening.png");
}
// test 6: Laser insertion without widening
// --------------------------------------------------
if (1)
{
gridMap->insertionOptions.wideningBeamsWithDistance = true;
N = 3000;
cout << "Running test #6: Laser insert. widen... "; cout.flush();
tictac.Tic();
for (i=0;i<N;i++)
{
#if 1
CPose2D pose(
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-M_PI,M_PI) );
CPose3D pose3D(pose);
#else
CPose3D pose3D;
#endif
gridMap->insertObservation( &scan1, &pose3D );
}
double T = tictac.Tac();
cout << "-> " << 1000*T/N << " ms/iter, scans/sec:" << N/T << endl;
CPose3D pose3D;
gridMap->clear();
gridMap->insertObservation( &scan1, &pose3D );
gridMap->saveAsBitmapFile("./gridmap_with_widening.png");
}
// test 7: Grid resize
// ----------------------------------------
if (1)
{
N = 400;
cout << "Running test #7: Grid resize... "; cout.flush();
tictac.Tic();
for (i=0;i<N;i++)
{
*gridMap = gridMapCopy;
gridMap->resizeGrid(-30,30,-40,40);
}
double T = tictac.Tac();
cout << "-> " << 1000*T/N << " ms/iter" << endl;
}
// test 8: Likelihood computation
// ----------------------------------------
if (1)
{
N = 5000;
*gridMap = gridMapCopy;
CPose3D pose3D(0,0,0);
gridMap->insertObservation( &scan1, &pose3D );
cout << "Running test #8: Likelihood... "; cout.flush();
double R = 0;
tictac.Tic();
for (i=0;i<N;i++)
{
CPose2D pose(
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-M_PI,M_PI) );
R+=gridMap->computeObservationLikelihood(&scan1,pose);
}
double T = tictac.Tac();
cout << "-> " << 1000*T/N << " ms/iter" << endl;
}
}
int main(int argc, char* argv[]) {
if (argc < 4)
{
puts("Not enough arguments.");
return -1;
}
ifstream laserLog, robotLog;
string laserLine, robotLine;
CConfigFile iniFile(argv[3]); // configurations file
double accumX = 0.0, accumY = 0.0, accumPhi = 0.0;
// pathfinding
int resolution = 4;
PathFinder pathFinder(resolution);
deque<TPoint2D> path;
// Load configurations
CMetricMapBuilderICP icp_slam;
icp_slam.ICP_options.loadFromConfigFile(iniFile, "MappingApplication");
icp_slam.ICP_params.loadFromConfigFile(iniFile, "ICP");
icp_slam.initialize();
laserLog.open(argv[1]); // log of laser scan
robotLog.open(argv[2]); // log of robot odometer
sf::RenderWindow window(sf::VideoMode(800, 600), "bam!");
sf::Texture texture;
texture.create(800, 600);
Matrix<sf::Color> pixels(600, 800);
sf::Sprite sprite(texture);
window.setVerticalSyncEnabled(true);
bool paused = false;
sf::Clock clock;
unsigned frameCount = 0;
while (laserLog.good() && window.isOpen()) {
sf::Event event;
while (window.pollEvent(event))
{
switch (event.type)
{
case sf::Event::Closed:
window.close();
break;
}
}
double f;
getline(laserLog, laserLine);
getline(robotLog, robotLine);
// Extract the laser scan info and convert it into a range scan observation to feed into icp-slam
CObservation2DRangeScanPtr obs = CObservation2DRangeScan::Create();
stringstream ssLaser(laserLine);
while (ssLaser >> f)
{
obs->scan.push_back(f);
obs->validRange.push_back(1);
}
icp_slam.processObservation(obs);
// Extract the odometer values and convert it into an observation to feed into icp-slam
stringstream ssRobot(robotLine);
ssRobot >> f;
accumX += f;
ssRobot >> f;
accumY += f;
ssRobot >> f;
accumPhi += f;
// Need the ABSOLUTE odometer readings, meaning the accumulated values
CPose2DPtr rawOdo(new CPose2D(accumX, accumY, accumPhi));
CObservationOdometryPtr obs2 = CObservationOdometry::Create();
obs2->odometry = *rawOdo;
obs2->hasEncodersInfo = false;
obs2->hasVelocities = false;
icp_slam.processObservation(obs2);
// Extract current estimates
// NOTE: coordinate points are in METERS
// First get the grid map
CMultiMetricMap* curMapEst = icp_slam.getCurrentlyBuiltMetricMap();
/* Grid representation of the current map.
* Grid X Range: [0, getSizeX()]
* Grid Y Range: [0, getSizeY()]
* Convert from coordinate to grid loc: x2idx(float), y2idx(float)
* Convert from grid loc to coordinate: idx2x(int), idx2y(float)
* Use getCell(int x, int y) to tell if the cell is empty or not. A real value [0,1], which is the probablity that cell is empty
*/
COccupancyGridMap2DPtr gridMap = curMapEst->m_gridMaps[0];
// Get the position estimates
CPose3D curPosEst = icp_slam.getCurrentPoseEstimation()->getMeanVal();
// (estimated) X,Y coordinates of the robot, and robot yaw angle (direction)
double robx = curPosEst.x();
double roby = curPosEst.y();
double robphi = curPosEst.yaw();
// Convert real coordinate to grid coordinate points
int gridRobX = gridMap->x2idx(robx);
int gridRobY = gridMap->y2idx(roby);
cout << "robot location: " << gridRobX << ' ' << gridRobY << '\n';
cout << "gridMap size: " << gridMap->getSizeX() << ' ' << gridMap->getSizeY() << '\n';
// Perform path finding
bool pathFound = true;
pathFinder.update(*gridMap);
pathFound = pathFinder.findPath(TPoint2D(gridRobX, gridRobY), TPoint2D(890, 500), path);
printf("pathFound: %d\tpath length: %d\n", pathFound, path.size());
// windows drawing
window.clear(sf::Color::White);
sf::View view;
view.setSize(800, 600);
view.setCenter(gridRobX, gridRobY);
window.setView(view);
// draw the grayscale probability map
int yStart = max(0, gridRobY - 300);
int yEnd = min((int)gridMap->getSizeY(), gridRobY + 300);
int xStart = max(0, gridRobX - 400);
int xEnd = min((int)gridMap->getSizeX(), gridRobX + 400);
for (int y = yStart; y < yEnd; ++y)
{
for (int x = xStart; x < xEnd; ++x)
{
sf::Color &color = pixels(y-yStart, x-xStart);
sf::Uint8 col = gridMap->getCell(x, y) * 255;
color.r = col;
color.g = col;
color.b = col;
}
}
texture.update((sf::Uint8*)pixels.getData());
sprite.setPosition(xStart, yStart);
window.draw(sprite);
// draw the robot's position
sf::CircleShape circle(5);
circle.setPosition(gridRobX-resolution/2, gridRobY-resolution/2);
circle.setOutlineColor(sf::Color::Red);
circle.setFillColor(sf::Color::Red);
window.draw(circle);
// draw the path
std::vector<sf::Vertex> verticies;
verticies.resize(path.size() + 1);
verticies[0].position.x = (gridRobX / resolution) * resolution + resolution/2;
verticies[0].position.y = (gridRobY / resolution) * resolution + resolution/2;
verticies[0].color = sf::Color::Blue;
for (unsigned i = 0; i < path.size(); ++i)
{
verticies[i+1].position.x = path[i].x * resolution + resolution / 2;
verticies[i+1].position.y = path[i].y * resolution + resolution / 2;
verticies[i+1].color = sf::Color::Blue;
}
window.draw(&verticies[0], verticies.size(), sf::LinesStrip);
// draw the grid representation (only the occupied cells)
/*
sf::Color col = sf::Color::Yellow;
col.a = 128;
for (unsigned y = max(0, (gridRobY-400)/resolution); y < min<int>(pathFinder.occupancyGrid.height(), (gridRobY+400)/resolution); ++y)
for (unsigned x = max(0, (gridRobX-400)/resolution); x < min<int>(pathFinder.occupancyGrid.width(), (gridRobX+400)/resolution); ++x)
{
if (!pathFinder.occupancyGrid(y, x)) continue;
int xx = x * resolution;
int yy = y * resolution;
sf::RectangleShape rect;
rect.setPosition(xx, yy);
rect.setSize(sf::Vector2f(resolution, resolution));
rect.setFillColor(col);
window.draw(rect);
}
*/
window.display();
frameCount++;
if (clock.getElapsedTime().asSeconds() >= 1.0)
{
char timestr[16];
sprintf(timestr, "%d fps", frameCount);
window.setTitle(timestr);
clock.restart();
frameCount = 0;
}
}
return 0;
}
