/**
* Limit the frame rate, by sleeping for an amount of milliseconds
* appropriate for maintaining the current desired FPS
*/
void CoreEngine::limitFps(){
int now, time_taken;
float dt;
now = elapsed_time_in_milliseconds();
time_taken = now - last_render_time;
if (time_taken < milliseconds_per_frame - 1){
dt = (milliseconds_per_frame - time_taken - 1) / 1000.0;
seconds_sleep(dt);
}
last_render_time = elapsed_time_in_milliseconds();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "FM2_Path_Planner");
bool enable_fm = true;
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("map_FM", 1000, chatterCallback_occupancy);
ros::Subscriber sub_points = n.subscribe("Init_and_Goal_Points", 1000, chatterCallback_points);
ros::Publisher path_pub = n.advertise<fastmarching::pathFM>("path_FM", 1000);
ros::Duration seconds_sleep(1);
while (ros::ok())
{
if (resolution != -1 && enable_fm && enable_points)
{
fastmarching::pathFM pathFM;
constexpr int ndims_ = 2;
nDGridMap<FMCell, ndims_> grid;
std::vector<int> fm2_sources;
std::array<int, ndims_> dimsize;
for (int i = 0; i < ndims; i++)
dimsize[i] = occ.gridSize[i];
grid.resize(dimsize);
for (int i = 0; i < occ.occupancyGrid.size(); i++)
{
grid.getCell(i).setOccupancy(occ.occupancyGrid[i]);
if (!occ.occupancyGrid[i])
fm2_sources.push_back(i);
}
grid.setLeafSize(resolution);
std::vector<int> init_point;
init_point.push_back(init);
typedef typename std::vector< std::array<double, ndims_> > Path; // A bit of short-hand.
Path path;
std::vector <double> path_velocity; // Velocity of the path
FastMarching2< nDGridMap<FMCell, ndims_> > fm2;
fm2.setEnvironment(&grid);
fm2.setInitialAndGoalPoints(init_point, fm2_sources, goal);
fm2.computeFM2();
fm2.computePath(&path, &path_velocity);
for (int i = 0; i < (int)path_velocity.size(); i++)
{
fastmarching::dims dims;
for (int j = 0; j < (int)path[i].size(); j++)
dims.dims.push_back(path[i][j]*resolution);
pathFM.positions.push_back(dims);
pathFM.vel_rate.push_back(path_velocity[i]);
}
pathFM.enable = true;
path_pub.publish(pathFM);
ROS_INFO("Path published");
enable_fm = false;
GridPlotter::plotMapPath(grid,path);
}
ros::spinOnce();
seconds_sleep.sleep();
}
return 0;
}
