/* main */
int main (int argc, char** argv)
{
if (argc < 3)
usage(argv[0]);
// settings
double gamma = 1; // penalty for discontinuity in space
// (weight for probability difference pairwise cost)
// > 1 does not seem to make a difference
double unknown = 0.2; // prior; below 0.5 makes sense
// command line arguments
if (argc >= 4)
gamma = atof(argv[3]);
if (argc >= 5)
unknown = atof(argv[4]);
// open file
OccupancyGrid occgrid;
occgrid.load(argv[1]);
occgrid.graphcut(gamma, unknown);
// save; use extension (.bt, .ot) to determine file format
bool binary = (string(argv[2]).find_last_of(".bt") > -1);
occgrid.save(argv[2], binary);
cout << "Result saved as " << argv[2];
if (binary)
cout << " (binary)";
cout << endl;
return 0;
}
bool valid(const vec2i& p) {
return ( p.x() >= 0 &&
p.y() >= 0 &&
size_t(p.x()) < grid.nx() &&
size_t(p.y()) < grid.ny() &&
grid(p.x(), p.y()) );
}
int main(int argc, const char *argv[])
{
typedef double EnergyType;
global_vis_.enable_show();
global_vis2_.enable_show();
// parse arguments ///////////////////////////////////////////
// Declare the supported options.
po::options_description desc("Run dual decomposition");
desc.add_options()
("help", "produce help message")
("width", po::value<double>(), "Width")
("height", po::value<double>(), "Height")
("resolution", po::value<double>()->required(), "Size of square cell in the map")
("step", po::value<EnergyType>()->default_value(50), "step size for algorithm")
("dir", po::value<std::string>()->default_value("Data/player_sim_with_reflectance"), "Data directory")
("clock", po::value<double>()->default_value(400), "Max clock")
;
po::positional_options_description pos;
pos.add("resolution", 1);
po::variables_map vm;
po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm);
po::notify(vm);
EnergyType step = vm["step"].as<EnergyType>();
std::string directory = vm["dir"].as<std::string>();
double max_clock = CLOCKS_PER_SEC * vm["clock"].as<double>();
// end of parse arguments ////////////////////////////////////
OccupancyGrid occupancyGrid = loadOccupancyGrid(vm);
global_vis_.init(occupancyGrid.height(), occupancyGrid.width());
global_vis2_.init(occupancyGrid.height(), occupancyGrid.width(), "d");
typedef gtsam::Index SampleSpaceType;
typedef gtsam::Index vertex_descriptor;
typedef G<OccupancyGrid, EnergyType, SampleSpaceType, vertex_descriptor> OccupancyGridGraph;
typedef hash_property_map< std::pair<vertex_descriptor, vertex_descriptor>, SampleSpaceType> MultiAssignment;
typedef DisagreementTracker<OccupancyGridGraph, MultiAssignment> DisagreementMap;
typedef hash_property_map< typename OccupancyGridGraph::MessageTypes::key_type, EnergyType> Messages;
typedef SlaveMinimizer<OccupancyGridGraph,
DDLaserFactor<DisagreementMap, Messages>, Messages, DisagreementMap> SlaveMinimizer_;
OccupancyGridGraph ogg(occupancyGrid);
SlaveMinimizer_ slvmin(ogg);
typedef typename OccupancyGridGraph::SampleSpaceMap SampleSpaceMap;
SampleSpaceMap ssm = get(sample_space_t(), ogg);
Messages messages(0);
MultiAssignment multiassign(0);
DisagreementMap disagreement_map(ogg, multiassign);
typedef SubgradientDualDecomposition<OccupancyGridGraph, SlaveMinimizer_,
SampleSpaceMap, DisagreementMap, Messages, SampleSpaceType, EnergyType> SubgradientDualDecompositionType;
SubgradientDualDecompositionType subg_dd(ogg, slvmin, ssm, disagreement_map, messages, step);
iterate_dualdecomposition<OccupancyGridGraph, SubgradientDualDecompositionType, DisagreementMap, SampleSpaceType>(ogg, subg_dd, disagreement_map, 70, max_clock);
std::stringstream ss;
ss << directory << "/dualdecomposition.png";
global_vis_.save(ss.str());
}
int main(int argc, char** argv) {
std::string inputfile;
float radius;
float initx, inity, initTheta, goalx, goaly, goalr, goalTheta;
int depth;
bool heuristic;
float inflate;
goalTheta = 0;
parseArgs(argc, argv, inputfile, radius, initx, inity, initTheta, goalx, goaly, goalr, goalTheta, depth, heuristic, inflate);
// Load grid
OccupancyGrid grid;
grid.load(inputfile);
if (grid.empty()) {
std::cerr << "error loading grid!\n";
return 1;
}
// expand obstacles
if (radius > 0) {
OccupancyGrid tmp;
expandObstacles(grid, radius, tmp, false);
grid = tmp;
}
// Run search
DubinSearch helper;
GridChecker* checker = new GridChecker(&grid);
Dubin* goal = helper.search(initx, inity, initTheta, goalx, goaly, goalr, goalTheta, checker, depth, heuristic, inflate);
if (goal==NULL) {
cout<<"Search failed"<<endl;
std::string filename = "DebugImage.svg";
helper.makeSVG(NULL,filename,grid,goalx,goaly,goalr);
return -1;
}
else {
cout<<"Search successful after "<<helper.getCount()<<" expansions"<<endl;
std::string filename = "DebugImage.svg";
helper.makeSVG(goal,filename,grid,goalx,goaly,goalr);
}
return 0;
}
void OccupancyPredictor::updatePrediction(const OccupancyGrid &grid)
{
int minX, maxX, minY, maxY;
grid.getBounds(minX, maxX, minY, maxY);
for (int x = minX; x < maxX; ++x)
{
for (int y = minY; y < maxY; ++y)
{
if (grid.getCellProbability(QPointF(x, y)) > 0.8)
{
_prediction[x][y].occupied++;
}
else
{
_prediction[x][y].free++;
}
}
}
}
//Updates an occupancy map by modifying cells along the beam using beamval and modifying the end-cell using endval.
void _mapUpdateTrace(OccupancyGrid& occ_map, GridRayTrace& trace, logit_val beamval, logit_val endval)
{
int i, j;
while(trace.getNextPoint(i, j))
{
//ROS_WARN("%d, %d", i, j);
if(0 <= i && i < occ_map.getWidth() && 0 <= j && j < occ_map.getHeight())
{
if(trace.hasNext())
{
occ_map.valueAt(i, j) += beamval;
}
else
{
occ_map.valueAt(i, j) += endval;
}
}
}
}
int main(int argc, char** argv) {
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGB);
glutInitWindowSize(640, 480);
if (argc < 2) { usage(1); }
grid.load(argv[1]);
if (grid.empty()) {
std::cerr << "Error loading grid: " << argv[1] << "\n";
exit(1);
}
glutCreateWindow("Biped plan demo");
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutKeyboardFunc(keyboard);
glutMouseFunc(mouse);
glutMotionFunc(motion);
glClearColor(1,1,1,1);
glGenTextures(1, &grid_texture);
glBindTexture(GL_TEXTURE_2D, grid_texture);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
std::vector<unsigned char> rgbbuf(grid.nx() * grid.ny() * 4);
int offs = 0;
for (size_t y=0; y<grid.ny(); ++y) {
for (size_t x=0; x<grid.nx(); ++x) {
rgbbuf[offs++] = grid(x,y);
rgbbuf[offs++] = grid(x,y);
rgbbuf[offs++] = grid(x,y);
rgbbuf[offs++] = 255;
}
}
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA,
grid.nx(), grid.ny(), 0, GL_RGBA,
GL_UNSIGNED_BYTE, &(rgbbuf[0]));
quadric = gluNewQuadric();
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glutMainLoop();
return 0;
}
GridRayTrace::GridRayTrace(tf::Vector3 start, tf::Vector3 end, const OccupancyGrid& grid_ref)
: _x_store(), _y_store()
{
//Transform (x,y) into map frame
start = grid_ref.toGridFrame(start);
end = grid_ref.toGridFrame(end);
double x0 = start.getX(), y0 = start.getY();
double x1 = end.getX(), y1 = end.getY();
//ROS_WARN("start: (%f, %f) -> end: (%f, %f)", x0, y0, x1, y1);
bresenham
(
floor(x0), floor(y0),
floor(x1), floor(y1),
_x_store, _y_store
);
_cur_idx = 0;
_max_idx = std::min(_x_store.size(), _y_store.size());
}
bool can_move(TetPiece const& p, int dx, int dy, OccupancyGrid const& og)
{
Point2 pos = p.get_position();
pos.x -= 1;
pos.y -= 1;
vector<Point2> const& blocks = p.piece_blocks();
for (size_t i = 0; i < blocks.size(); i++)
{
int r_index = pos.y + dy + blocks[i].y;
int c_index = pos.x + dx + blocks[i].x;
if (!og.is_valid_cell(r_index, c_index))
return false;
occupancy_cell cell = og.get_cell(r_index, c_index);
if (cell.v != -1)
return false;
}
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "occupancy_grid");
OccupancyGrid map;
_map = ↦
map.init();
map.mapInit();
ros::Rate loop_rate(20.0);
while(map.n.ok())
{
ros::spinOnce();
map.gridVisualize();
loop_rate.sleep();
}
save_maps(1);
return 0;
}
void reshape(int w, int h) {
width = w;
height = h;
glViewport(0, 0, width, height);
float ga = float(grid.nx())/grid.ny();
float wa = float(w)/h;
float wh = grid.ny();
float ww = grid.nx();
if (ga > wa) {
wh *= ga/wa;
} else {
ww *= wa/ga;
}
float cx = 0.5*grid.nx();
float cy = 0.5*grid.ny();
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(cx - 0.5 * ww, cx + 0.5*ww,
cy + 0.5 * wh, cy - 0.5*wh);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glGetIntegerv(GL_VIEWPORT, viewport);
glGetDoublev(GL_PROJECTION_MATRIX, projection);
glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
}
void DubinSearch::makeSVG(Dubin* goal, std::string filename, OccupancyGrid& grid,
float goalx, float goaly, float goalr, int zoom){
//TODO
FILE* svg = fopen(filename.c_str(), "w");
if (!svg) {
std::cerr << "error opening svg for output! \n";
return;
}
int width = grid.nx() * zoom;
int height = grid.ny() * zoom;
fprintf(svg, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
fprintf(svg, "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n");
fprintf(svg, " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n");
fprintf(svg, "<svg version=\"1.1\" xmlns=\"http://www.w3.org/2000/svg\"\n");
fprintf(svg, " xmlns:xlink=\"http://www.w3.org/1999/xlink\"\n");
fprintf(svg, " width=\"%upx\" height=\"%upx\" viewBox=\"0 0 %u %u\" >\n",
width, height, width, height);
// Draw background and obstacle
int f;
for (size_t y=0; y<grid.ny(); ++y) {
for (size_t x=0; x<grid.nx(); ++x) {
if (grid(x,y)==0){
f = 0;
}
else{
f = 255;
}
fprintf(svg, " <rect x=\"%d\" y=\"%d\" width=\"%d\" height=\"%d\" "
"fill=\"#%02x%02x%02x\" />\n",
(int)x*zoom, (int)y*zoom, zoom, zoom, f, f, f);
}
}
fprintf(svg, " <circle cx=\"%f\" cy=\"%f\" r=\"%f\" stroke=\"red\" stroke-width=\"2\" fill=\"none\"/>\n",
goalx*zoom, goaly*zoom, goalr*zoom);
fprintf(svg, " <g stroke=\"#bfbfbf\" stroke-width=\"1.5\">\n");
// Draw nodes and lines expanded
Dubin* curr;
Dubin* pred;
while (!_poppedNodes.empty()){
curr = _poppedNodes.back();
float cx = (curr->x + 0.5)*zoom;
float cy = (curr->y + 0.5)*zoom;
pred = curr->predecessor;
float r = 0.2 * zoom;
if(pred!=NULL){
float cxp = (pred->x + 0.5)*zoom;
float cyp = (pred->y + 0.5)*zoom;
float r = curr->archRadius * zoom;
if (r ==0) {
fprintf(svg, " <line x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" %s/>\n",
cx, cy, cxp, cyp, "");
} else {
fprintf(svg, " <path d=\"M %f %f A %f %f 0 0 %d %f %f \" fill=\"none\"/>\n",
cxp, cyp, fabs(r), fabs(r), int(r>0), cx, cy);
}
}
fprintf(svg, " <circle cx=\"%f\" cy=\"%f\" r=\"%f\" fill=\"%s\" %s/>\n",
cx, cy, r, "#ffffff","");
_poppedNodes.pop_back();
}
while (!_queue.empty()){
curr = _queue.back();
float cx = (curr->x + 0.5)*zoom;
float cy = (curr->y + 0.5)*zoom;
pred = curr->predecessor;
float r = 0.5 * zoom;
if(pred!=NULL){
float cxp = (pred->x + 0.5)*zoom;
float cyp = (pred->y + 0.5)*zoom;
float r = curr->archRadius * zoom;
if (r ==0) {
fprintf(svg, " <line x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" %s/>\n",
cx, cy, cxp, cyp, "");
} else {
fprintf(svg, " <path d=\"M %f %f A %f %f 0 0 %d %f %f \" fill=\"none\"/>\n",
cxp, cyp, fabs(r), fabs(r), int(r>0), cx, cy);
}
}
fprintf(svg, " </g>\n");
fprintf(svg, " <g stroke=\"#0000ff\" stroke-width=\"1\">\n");
fprintf(svg, " <circle cx=\"%f\" cy=\"%f\" r=\"%f\" fill=\"%s\" %s/>\n",
cx, cy, r, "#ffffff","");
fprintf(svg, " </g>\n");
fprintf(svg, " <g stroke=\"#7f7f7f\" stroke-width=\"1\">\n");
_queue.pop_back();
}
fprintf(svg, " </g>\n");
fprintf(svg, " <g stroke=\"#7f007f\" stroke-width=\"3\">\n");
// Draw found path
curr = goal;
while (curr!=NULL){
float cx = (curr->x + 0.5)*zoom;
float cy = (curr->y + 0.5)*zoom;
pred = curr->predecessor;
float r = 0.2 * zoom;
if(pred!=NULL){
float cxp = (pred->x + 0.5)*zoom;
float cyp = (pred->y + 0.5)*zoom;
float r = curr->archRadius * zoom;
if (r ==0) {
fprintf(svg, " <line x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" %s/>\n",
cx, cy, cxp, cyp, "");
} else {
fprintf(svg, " <path d=\"M %f %f A %f %f 0 0 %d %f %f \" fill=\"none\"/>\n",
cxp, cyp, fabs(r), fabs(r), int(r>0), cx, cy);
}
}
fprintf(svg, " <circle cx=\"%f\" cy=\"%f\" r=\"%f\" fill=\"%s\" %s/>\n",
cx, cy, r, "#ffffff","");
curr = pred;
}
fprintf(svg, " </g>\n");
fprintf(svg, "</svg>\n");
fclose(svg);
cout<<"Made Debug Image"<<endl;
}
void display() {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, grid_texture);
glColor3ub(255,255,255);
glBegin(GL_QUADS);
glTexCoord2f(1, 0);
glVertex2f(grid.nx(), 0);
glTexCoord2f(0, 0);
glVertex2f(0, 0);
glTexCoord2f(0, 1);
glVertex2f(0, grid.ny());
glTexCoord2f(1, 1);
glVertex2f(grid.nx(), grid.ny());
glEnd();
glDisable(GL_TEXTURE_2D);
if (valid(init)) {
glPushMatrix();
glColor3ub(63,255,63);
glTranslated(init.x(), init.y(), 0);
gluDisk(quadric, 0, goalr, 32, 1);
glRotated(initTheta*180/M_PI, 0, 0, 1);
glBegin(GL_TRIANGLES);
glVertex2f(6, 0);
glVertex2f(0, 2);
glVertex2f(0, -2);
glEnd();
glPopMatrix();
}
if (valid(goal)) {
glPushMatrix();
glColor3ub(255,63,63);
glTranslated(goal.x(), goal.y(), 0);
gluDisk(quadric, 0, goalr, 32, 1);
glPopMatrix();
}
draw(searchResult, true);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluOrtho2D(0, width, 0, height);
std::ostringstream ostr;
ostr << "Goal pos: (" << goal.x() << ", " << goal.y() << ")\n"
<< "Init pos: (" << init.x() << ", " << init.y() << ")\n"
<< "Init theta: " << initTheta*180/M_PI << "\n"
<< "XY inflation: " << inflate_h << "\n"
<< "Z inflation: " << inflate_z << "\n"
<< "Max depth: " << maxDepth << "\n"
<< "View depth: " << viewDepth << "\n"
<< "Auto-plan: " << (auto_plan ? "on" : "off") << "\n"
<< "\n"
<< "Plan cost: " << (searchResult ? searchResult->costToCome : 0) << "\n"
<< "Plan time: " << (searchResult ? planTime : 0) << "\n";
if (show_help) {
ostr << "\n\n"
<< " Left-click init pos\n"
<< "Shift+click init theta\n"
<< "Right-click goal pos\n\n"
<< " 1/2 max depth\n"
<< " 3/4 view depth\n"
<< " -/+ XY inflation\n"
<< " A auto-plan\n"
<< " Enter re-plan\n"
<< " ESC quit\n"
<< " ? hide help";
} else {
ostr << "\nPress ? to toggle help.";
}
std::string str = ostr.str();
void* font = GLUT_BITMAP_8_BY_13;
const int tx = 8;
const int ty = 13;
const int th = ty+2;
int maxwidth = 0;
int linewidth = 0;
int rh = th;
for (size_t i=0; i<str.length(); ++i) {
char c = str[i];
if (c == '\n') {
maxwidth = std::max(maxwidth, linewidth);
linewidth = 0;
rh += th;
} else {
linewidth += tx;
}
}
maxwidth = std::max(maxwidth, linewidth);
int rw = maxwidth + 20;
rh += 10;
glColor4ub(191,191,255,225);
glEnable(GL_BLEND);
glBegin(GL_QUADS);
glVertex2f( 0, height);
glVertex2f(rw, height);
glVertex2f(rw, height-rh);
glVertex2f( 0, height-rh);
glEnd();
glDisable(GL_BLEND);
int rx = 10;
int ry = height-th;
glColor3ub(0,0,0);
glRasterPos2i(rx, ry);
for (size_t i=0; i<str.length(); ++i) {
char c = str[i];
if (c == '\n') {
ry -= th;
glRasterPos2i(rx, ry);
} else {
glutBitmapCharacter(font, str[i]);
}
}
glPopMatrix();
glutSwapBuffers();
}
int main(int argc, char** argv){
double cruising_speed = 0.1;
char* baseframe;
char* childframe;
if (argc < 4) {
ROS_ERROR("(%i) usage: hallwaydrive <speed> <base frame> <child frame>\n", argc);
return -1;
}
else {
cruising_speed = atof(argv[1]);
baseframe = argv[2];
childframe = argv[3];
}
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;
ros::Rate loop_rate(5);
ros::Time current_time;
ros::Publisher speedController = n.advertise<Speeds>("speeds_bus", 50);
/*message_filters::Subscriber<Position2D> pos2DSub(n, "/pos2d_bus", 1);
pos2DSub.registerCallback(roombaOdomCallback);
message_filters::Subscriber<LaserScan> laserSub(n, "/scan", 1);
laserSub.registerCallback(laserCallback);*/
ros::Subscriber pos2DSub = n.subscribe("pos2d_bus", 100, roombaOdomCallback);
ros::Subscriber laserSub = n.subscribe("scan", 100, laserCallback);
while(n.ok()){
current_time = ros::Time::now();
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(pose.angle);
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = baseframe;
odom_trans.child_frame_id = childframe;
odom_trans.transform.translation.x = pose.x;
odom_trans.transform.translation.y = pose.y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = baseframe;
//set the position
odom.pose.pose.position.x = pose.x;
odom.pose.pose.position.y = pose.y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//ROS_INFO("Odometry: (%.3lf, %.3lf) at %.3lf radians\n", pose.x, pose.y, pose.angle);
double speed = 0.0, angle = 0.0;
/*if (grid.closeObstacle) {
double closestAngle = grid.getObstacleLaserAngle();
ROS_INFO("Close Obstacle at angle %.3lf at dist %lf", closestAngle, grid.minDist);
//If there's an obstacle really close, turn away from that obstacle
if (closestAngle < 0)
angle = closestAngle + 3.141/2.0;
else
angle = closestAngle - 3.141/2.0;
angle /= (1.0 + grid.minDist*4.0);
}
else {*/
Point centroid = grid.GetCentroid();
angle = centroid.getAngle();
angle = angle / 3.141;
angle = angle*angle*angle;
angle *= 3.141;
//}
speed = cruising_speed / (1.0 + 5.0 * fabs(angle));
//Publish the speed message
Speeds speedMsg;
speedMsg.forward = speed;
speedMsg.rotate = angle;
if(!n.hasParam("/hallwaydrive/automatic")) {
ROS_ERROR("Unable to find parameter automatic");
return -1;
}
int automatic;
n.param("/hallwaydrive/automatic", automatic, 0);
//ROS_INFO("automatic = (%i)", automatic);
if (automatic == 1) { //Only send commands if the user wants
//automatic control; otherwise they will interfere with the controller GUI
speedController.publish(speedMsg);
}
else if (automatic == 0) {
double forward = 0.0, turnrate = 0.0;
n.param("/hallwaydrive/forward", forward, 0.05);
n.param("/hallwaydrive/turnrate", turnrate, 0.0);
speedMsg.forward = forward;
speedMsg.rotate = turnrate;
speedController.publish(speedMsg);
}
//ROS_INFO("speed = %.3f, turn = %.3f\n", speed, angle);
odom.child_frame_id = childframe;
//set the velocity
//Note: max speed of robot is 500mm/sec, so scale to 0.5
odom.twist.twist.linear.x = speed*0.5*cos(pose.angle);
odom.twist.twist.linear.y = speed*0.5*sin(pose.angle);
odom.twist.twist.angular.z = angle;
//publish the odometry message
odom_pub.publish(odom);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
void laserCallback(const boost::shared_ptr<const LaserScan>& s) {
grid.fill(s);
grid.findCloseObstacleBin(s);
}
