Hi,
I noticed the global plan passed to the critics is only the section that is within the robot's costmap. And if there are two unconnected sections of the path within the costmap, the code will pass the earlier section to the critics because it starts counting from the first node of the plan.
I found this is somewhat problematic if the robot does a U-turn and gets too close to its previous path. This commit ensures that the section of the path that is passed to the critic is the section that is closest to the robot.
Hi, I noticed the global plan passed to the critics is only the section that is within the robot's costmap. And if there are two unconnected sections of the path within the costmap, the code will pass the earlier section to the critics because it starts counting from the first node of the plan.
I found this is somewhat problematic if the robot does a U-turn and gets too close to its previous path. This commit ensures that the section of the path that is passed to the critic is the section that is closest to the robot.