SteveMacenski / spatio_temporal_voxel_layer

Hello!

I believe I have found some bug/unexpected behavior from the spatio temporal voxel layer (actually highly related to nav2 costmap2d as well, was unsure where to post this first).

It seems that we have a few services for recovery behaviors that clear the costmaps in order to remove artifacts, one of them is for example /global_costmap/clear_entirely_global_costmap and the other is /global_costmap/clear_except_global_costmap.

When I call the ClearEntireCostmap I get the expected behavior that all the not currently visible voxels are cleared along with their costmaps, but when I call the ClearCostmapExceptRegion with whatever value for reset_distance parameter (have tried for example 2.0, 1.0, 0.5, 0.0) none of the expected voxels/costmaps are cleared.

Looking into the code I've realized that ClearEntireCostmap will call a reset function that is actually implemented in stvl:

The reset function for one layer (stvl in this case)

Is implemented here:

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp

Lines 545 to 557 in 396b25b

    
           void SpatioTemporalVoxelLayer::reset(void) 
        
           /*****************************************************************************/ 
        
           { 
        
             boost::recursive_mutex::scoped_lock lock(_voxel_grid_lock); 
        
             // reset layer 
        
             Costmap2D::resetMaps(); 
        
             this->ResetGrid(); 
        
             current_ = true; 
        
             observation_buffers_iter it = _observation_buffers.begin(); 
        
             for (; it != _observation_buffers.end(); ++it) { 
        
               (*it)->ResetLastUpdatedTime(); 
        
             } 
        
           }

Called here: https://github.com/ros-planning/navigation2/blob/8a8b4d2ff97b36ae9931be376fe3ddd701d19445/nav2_costmap_2d/src/costmap_2d_ros.cpp#L518-L531

And defined as virtual here: https://github.com/ros-planning/navigation2/blob/b201d058a7b15f1786c57afe69d079ac5d502018/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp#L84-L87

Which so far makes a lot of sense, as this works.

But when looking at how the ClearCostmapExceptRegion clears the costmap it is in quite a different way. It doesn't use anything similar to a reset function, instead it uses this resetMapToValue

That is implemented directly in costmap_2d: https://github.com/ros-planning/navigation2/blob/8a8b4d2ff97b36ae9931be376fe3ddd701d19445/nav2_costmap_2d/src/costmap_2d.cpp#L102-L110

And is defined here not as virtual like the reset one: https://github.com/ros-planning/navigation2/blob/8a8b4d2ff97b36ae9931be376fe3ddd701d19445/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp#L311-L312

So this one cannot be implemented on the stvl level (or any other layer that also implements nav2_costmap_2d::CostmapLayer) as far as I understand this, which seems to be a problem to me. As an end result, we only clear the actual costmap which is immediately filled again because the voxels are still there, and will project to the costmap.

These services are implemented here: https://github.com/ros-planning/navigation2/blob/8a8b4d2ff97b36ae9931be376fe3ddd701d19445/nav2_costmap_2d/src/clear_costmap_service.cpp

Observations:

I have been using and checking the code for Foxy, but from what I have seen this has not been changed or mentioned in other issues/prs.

I do have my stvl_obstacle_layer in the clearable_layers parameter.

Any ideas what could be a good way to solve this? Or is this something expected/am I missing something? Sorry if this should have been posted to navigation2 instead, but as I think this is something that would require changes from stvl as well I thought it could be good to get idead from here.

Thanks!

STVL does not support partial costmap clearing - its all or nothing by design. I'd be happy to field a PR to implement the "except region" options, but not something I'd plan to do myself as I don't have a particular need for it. I think that if part of your data is invalid, likely all of your data should be cleared out.

Hello Steve, thanks for your answer! Good to know that this really was designed like this so I'm not going crazy.

In our case we wanted this to avoid clearing things that are in the known blind spots of our robot. So as a first recovery behavior we clear almost everything but being a bit conservative about the blind spots close to the robot. I agree that usually if part of the data is bad then clearing everything should be the better approach.

We'll discuss how urgent this is for us and if I open a PR on this I'll link this issue!

For now I think this could be closed, then?

Slightly off topic, can you look at this PR ros-planning/navigation2#2772 and associated ticket? Basically, they've found that they think the clearing except region logic is inverted in Nav2 and they're submitting a PR to fix it.

Can you verify this is also your experience?

Checked it. It's not really same experience as me, the clearing except doesn't use that clearArea function... but it's a great finding since it really seemed to be inverted!

	void SpatioTemporalVoxelLayer::reset(void)
	/*****************************************************************************/
	{
	boost::recursive_mutex::scoped_lock lock(_voxel_grid_lock);
	// reset layer
	Costmap2D::resetMaps();
	this->ResetGrid();
	current_ = true;
	observation_buffers_iter it = _observation_buffers.begin();
	for (; it != _observation_buffers.end(); ++it) {
	(*it)->ResetLastUpdatedTime();
	}
	}

ClearCostmapExceptRegion not clearing voxels