Closes #325 Updates the cost function to include a comparison via dynamic time warp (dtw) between the global path and the current plan. For each generated plan it finds the closest point on the global path to the bot, and then generates a global path segment about the same length as the path. It then uses dtw to compare the current plan to the global cost.
Tested by launching evgp_sim.launch as well as evgp_planning_demo.launch and observing that the path on the topic /global_path_seg is of similar length to the projected path, and that it begins approximately where the robot begins.
Closes #325 Updates the cost function to include a comparison via dynamic time warp (dtw) between the global path and the current plan. For each generated plan it finds the closest point on the global path to the bot, and then generates a global path segment about the same length as the path. It then uses dtw to compare the current plan to the global cost.
Tested by launching evgp_sim.launch as well as evgp_planning_demo.launch and observing that the path on the topic /global_path_seg is of similar length to the projected path, and that it begins approximately where the robot begins.
Video of it running can be found here.