measure distance between robot position and goal including waypoints
hello, I need to measure the distance between the robot's current position and the goal planned by the global planner and if there are waypoints between the start and the goal, they must be added to the total path, I've already subscribed to the amcl_pose to get the robot's position and move_base/GlobalPlanner/plan to get the goal.
how to add waypoints coordinates in between?
Hello, you need to provide more information. 1- Are the coordinates of the Waypoints already included in the path planned by your global planner? -> in this case, the distance will be the sum of each two adjacent poses in the plan.poses array Or 2-Do you want to add Waypoints coordinates in between to the global path? -> here you have to develop a plugin for your global planner which will take care of concatenating the plans going through the Waypoints until the final goal tuto : http://wiki.ros.org/navigation/Tutori...