How to get the length of navigation path

Hello everyone,I wanna get the length of navigation path when I use nav_msgs/GetPlan.

Do I have to calculate the distance between every two adjacent points in the array (nav_msgs/Path -> geometry_msgs/PoseStamped[] poses) ?

I mean, in real-time. The remaining length varies depending on the robot position.

My poor way is to calculate distance of every two points, and add them up...

Is there a more convenient method ?

edit retag close merge delete

Sort by » oldest newest most voted Yes you hit the nail on the head. You've got to calculate the length of each straight path segment and add them up.

Working out how much of a path is left to traverse is slightly trickier because the robot will probably not be 'exactly' on the path. Path progress can be found by calculating the closest point on the path to the robot's position. This is a bit more work but then allows you to accurately measure how far along the path your robot is.

Although the code to do this is more than a couple of lines you will have no problem at all calculating this in real time.

Hope this helps.

more

I solved this problem by adding neighbors.

This is core of my mind:

rospy.Subscriber("/move_base/NavfnROS/plan", Path, callback)

and in callback:

sum_ += sqrt(pow((data.poses[i+1].pose.position.x - data.poses[i].pose.position.x),2) + pow((data.poses[i+1].pose.position.y - data.poses[i].pose.position.y), 2))

It works well.
:)

more

TifferPelode could you demonstrate precisely that piece of your code (i.e. callback) ?