The problem of the order of marking and clearing

[tanghaijian1992]

---

// navigation mode: clear observations, mapping mode: save maps and publish
if (!_mapping_mode)
{
_voxel_grid->updateRobotPosition(robot_x, robot_y);
_voxel_grid->ClearFrustums(clearing_observations, cleared_cells);
}
else if (ros::Time::now() - _last_map_save_time > _map_save_duration)
{
_last_map_save_time = ros::Time::now();
time_t rawtime;
struct tm* timeinfo;
char time_buffer[100];
time (&rawtime);
timeinfo = localtime (&rawtime);
strftime(time_buffer, 100, "%F-%r", timeinfo);

costmap_2d::SaveGrid srv;
srv.request.file_name.data = time_buffer;
SaveGridCallback(srv.request, srv.response);

}
double t2 = ros::Time::now().toSec();
ROS_INFO_THROTTLE(costmap_2d::log_interval_,"occlusion dectect time elapsed %f", t2-t1);

// mark observations
_voxel_grid->Mark(marking_observations);

// update the ROS Layered Costmap
UpdateROSCostmap(min_x, min_y, max_x, max_y, cleared_cells);

---

As the code show,when running "voxel_grid->Mark(marking_observations);" ,we do not mark at costmap,we should wait for next round to do it . I think it will have some delay ,why not mark at costmap just after running "voxel_grid->Mark(marking_observations);" ?

[Steve Macenski]

Its an efficiency thing, iterating through the voxel grid isn't "free" so by queuing it up for the next iteration we can substantially save computation. For running this at 10-20-etc hz, 1 timestep isn't a big deal for slow moving robots

[tanghaijian1992]

OK, I see that.Thanks!