Feature: Layer which checks if a robot can fit through a space #74
Conversation
The freespace layer computes the free space above the meshes vertices and marks vertices as lethal if the robot would not fit through e.g. a doorway. Add exporting of cmake targets to mesh_map for depending projects to use. LVR2_DEFINITIONS had to be added to the mesh_map target to reflect LVR2 library configuration.
mesh_layers/src/freespace_layer.cpp
Outdated
| // The freespace_ map contains the actual free space above the vertices. | ||
| // If the robot fits through a gap the cost is 0 and if not its 1 |
There was a problem hiding this comment.
Nice, I think it's great that the cost is decoupled from the available space!
Do you think it could be useful to add a ramp-down value range to the cost function?
I.e. don't jump from 1 to 0, but have a linear part in there. I.e the cost is
- 1, if available space is below
config_.required_freespace additional_free_space_ratioin(0,1), if available space is aboveconfig_.required_freespacebut belowconfig_.more_than_enough_freespace. Withadditional_free_space_ratiobeing close to 1 when available space is close toconfig_.required_freespace. And close to 0 when available space is close toconfig_.more_than_enough_freespace.- 0, if available space is above
config_.more_than_enough_freespace
Here, I drew a picture :D
This would allow modelling that the robot should prefer paths with more space, when possible. Up until the available space is large enough that it does not care anymore.
Preferring paths with more space could be motivated by e.g. exteroceptive sensors like lidars that might not like a limited FoV, or by having uncertainty about the map quality (robot might not fit after all).
Using a sigmoid function instead would smooth the discontinuous parts of the cost functions. That might be useful for some planners if they estimate derivations on the map cost. But I don't know if this is a thing...
mesh_layers/src/freespace_layer.cpp
Outdated
| } | ||
|
|
||
| // Finally calculate the freespace layer | ||
| freespace_ = lvr2::calcNormalClearance(*map_ptr_->mesh(), vertex_normals); |
There was a problem hiding this comment.
I like the term clearance, like in lvr. I think the layer name freespace is not optimal. To me, it invokes the image of a gridmap where there are obstacles (cannot traverse) and freespace (can traverse).
Your layer, however, looks at a specific kind of freespace in the above sense: Parts of the env with enough clearance so the robot can fit.
I think a more specific layer name would be great.
My ideas were:
- headspace
- headroom
But seeing the lvr2 function name here, I think clearance would also be nice.
Might actually be the best choice, so it's consistent between lvr2 and the mesh map. Also, I think clearance is a more common word than the stuff that came to my mind.
|
I would be fine with changing the name, do you have any suggestions? Maybe something like OverheadClearanceLayer? How would you change the cost function? My thought was either the robot fits or it doesn't ^^ |
check out the comments in the code below the main review comment |
|
Oh sorry, i missed the comments. clearance sounds good to me :) I think it makes sense to implement the proposed change to the cost function. However as far as i know the planners do not consider the costs other than checking if the cost is above a certain limit. It would argue to have a |
No worries
great :)
Awesome, your idea on parameters is great! 🥇 |
|
I have implemented the changes and the clearance layer looks like this :) |
|
Thanks for your contribution! |
I have created a new layer for the mesh map which calculates the obstacle free space above each vertex and calculates the cost based on the height of the robot. This is useful to check if the robot can fit through doors or can pass under overhangs in the world. For example if the robot is small enough it could drive under tables.
Let me know if you have any feedback :)