I have been looking at the nav2_collision_monitor in the past, and upon reading more, I understand that there are often multiple safety zones around the robot, and with VelocityPolygon these can vary with the speed of the robot,
i.e. the faster the robot goes, the more safety distance it needs, so the guarded polygon gets bigger. Similarly, at low speed we can get away with a much smaller polygon.
However, in MPPI, when consider_footprint is true, CostCritic uses a constant footprint along the whole horizon. inCollision() calls costmap_ros_->getRobotFootprint() at every pose, so the same polygon gets applied at t, t+1, ... all the way out to the end of the rollout.
There is a check for the footprint having changed, but only between controller calls, not along the horizon — on every score() it re-runs findCircumscribedCost() (
|
if (consider_footprint_) { |
). That function then , otherwise it recomputes the inflation-decay cost at the new radius.
So the dynamic footprint case is already acknowledged, just not within a rollout. If the sampled speed changes from t to t+1, the footprint size should probably change accordingly at that time instant, rather than staying constant along the horizon. Unless the robot speed isn't changing by a large degree, I think this is kind of inaccurate?
This would help to navigate the robot from the tight space at lower speed, (since at lower speed, the corresponding footprint polygon would be smaller, thus it would have higher chances to get out from the tight spaces, than trying the same thing with a fixed size polygon ).
I asked about this on Slack a while ago, but I wanted to raise it here to get the community's opinion on whether this would be useful, and whether I'm missing something obvious that would make the change intractable.
I have been looking at the nav2_collision_monitor in the past, and upon reading more, I understand that there are often multiple safety zones around the robot, and with VelocityPolygon these can vary with the speed of the robot,
i.e. the faster the robot goes, the more safety distance it needs, so the guarded polygon gets bigger. Similarly, at low speed we can get away with a much smaller polygon.
However, in MPPI, when consider_footprint is true, CostCritic uses a constant footprint along the whole horizon. inCollision() calls costmap_ros_->getRobotFootprint() at every pose, so the same polygon gets applied at t, t+1, ... all the way out to the end of the rollout.
There is a check for the footprint having changed, but only between controller calls, not along the horizon — on every score() it re-runs findCircumscribedCost() (
navigation2/nav2_mppi_controller/src/critics/cost_critic.cpp
Line 146 in 7e382cb
So the dynamic footprint case is already acknowledged, just not within a rollout. If the sampled speed changes from t to t+1, the footprint size should probably change accordingly at that time instant, rather than staying constant along the horizon. Unless the robot speed isn't changing by a large degree, I think this is kind of inaccurate?
This would help to navigate the robot from the tight space at lower speed, (since at lower speed, the corresponding footprint polygon would be smaller, thus it would have higher chances to get out from the tight spaces, than trying the same thing with a fixed size polygon ).
I asked about this on Slack a while ago, but I wanted to raise it here to get the community's opinion on whether this would be useful, and whether I'm missing something obvious that would make the change intractable.