add footprint_clearing_padding to partially clear obstacles from footprint #105
Conversation
simplescreenrecorder-2024-12-07_11.04.39.mp4I have tested this in simulation, see the above video |
| footprint_center_.y = sum_y / footprint.size(); | ||
| } | ||
|
|
||
| std::vector<geometry_msgs::Point> ObstacleLayer::getReducedFootprint(const std::vector<geometry_msgs::Point>& original_footprint, double reduction_size) |
There was a problem hiding this comment.
reduction_size should be between 0 or 1 right ? Based on you calculations other values would not work as intended.
Maybe adding some check or static_assert would be useful ? Is var reduction_size are going to be determined during runtime or can it be predicted before execution?
also one more thing... make reduction_size const double&
There was a problem hiding this comment.
reduction_size should be between 0 or 1 right ? Based on you calculations other values would not work as intended.
Maybe adding some check or static_assert would be useful
yes I will add the check to ignore reduction_size if it is not between 0 and 1.
Is var reduction_size are going to be determined during runtime or can it be predicted before execution?
before execution it has a default value and it can also be changed dynamically as shown in video
make reduction_size const double&
yes I will change that
|
|
||
| if (distance > 0) | ||
| { | ||
| double scale = (distance - reduction_size) / distance; |
There was a problem hiding this comment.
scale = 1 - red_size/distance right? Is check for valid values needed ?
There was a problem hiding this comment.
if distance is 0 (however it is only possible if centroid of footprint is same as corner point of footprint) it will crash because it will divide by 0. So I have added the check for safety.
There was a problem hiding this comment.
Q: can't we change the signature of the method to be the scale? I think it is more intuitive to has scale directly as argument and it should be flexible to increase / decrease (can be > 1).
btw, can't we use costmap2d::padFootprint?
costmap_2d/cfg/ObstaclePlugin.cfg
Outdated
| gen.add("reduced_footprint_enabled", bool_t, 0, "Enable use of reduced footprint size for clearing lethal obstacle when footprint_clearing_enabled", False) | ||
| gen.add("footprint_reduction_size", double_t, 0, "Size to reduce footprint (meters)", 0.01, 0.0, 0.1) |
There was a problem hiding this comment.
These params are a bit confusing
Idea: add a single param named footprint_clearing_padding that defaults to footprint_padding
Setting to - footprint_reduction_size should achieve the same result
Ensure u add a good description, because the concept is not intuitive at all
There was a problem hiding this comment.
Hi did you mean like this?
there is only extra parameter now
simplescreenrecorder-2024-12-17_15.55.39.mp4
Nisarg236
changed the title
There was a problem hiding this comment.
interesting issue.
Can you check if this gets fixed if we use supercover:
naturerobots/move_base_flex#322
You would need to change mbf branch and rebuild
This will not solve the issue completely because the robot will still rotate when the obstacle is just in front of it and cause collision, however it can improve the condition where sometimes the points are just outside the footprint and they are skipped in marking on the costmap. |
| double dx = p.x - footprint_center_.x; | ||
| double dy = p.y - footprint_center_.y; | ||
|
|
||
| double distance = sqrt(dx * dx + dy * dy); |
There was a problem hiding this comment.
| double distance = sqrt(dx * dx + dy * dy); | |
| const double distance = std::hypot(dx, dy); |
|
|
||
| std::vector<geometry_msgs::Point> ObstacleLayer::getReducedFootprint(const std::vector<geometry_msgs::Point>& original_footprint, double reduction_size) | ||
| { | ||
| std::vector<geometry_msgs::Point> reduced_footprint; |
There was a problem hiding this comment.
| std::vector<geometry_msgs::Point> reduced_footprint; | |
| std::vector<geometry_msgs::Point> reduced_footprint; | |
| reduced_footprint.reserve(original_footprint.size()); |
|
|
||
| if (distance > 0) | ||
| { | ||
| double scale = (distance - reduction_size) / distance; |
There was a problem hiding this comment.
Q: can't we change the signature of the method to be the scale? I think it is more intuitive to has scale directly as argument and it should be flexible to increase / decrease (can be > 1).
btw, can't we use costmap2d::padFootprint?
|
|
||
| reduced_footprint.push_back(new_point); | ||
| } | ||
| return reduced_footprint; |
There was a problem hiding this comment.
R: I think you should save the reduced footprint in the dynamic reconfigure call, so the computation of the new one is called only when you change the parameter (i.e, make this a member variable and in updateFootprint get it)
AB#19208
Dont ignore points that are near to the edge of the robot footprint for safety.
When the obstacle is very close to the robot, some part of the obstacle is inside the footprint and some is outside. The robot cannot move forward as the obstacle is just in front of it but it can rotate in place. As our robot footprint is not a circle, a part of it hits the obstacle when it rotates.