Ask Your Question

How to get the length of navigation path

asked 2018-05-07 02:19:56 -0600

TifferPelode gravatar image

updated 2018-05-07 02:21:54 -0600

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 flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2018-05-07 05:54:03 -0600

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.

edit flag offensive delete link more


Thanks for your response,I'll try to achieve this.

TifferPelode gravatar image TifferPelode  ( 2018-05-07 06:07:59 -0600 )edit

answered 2018-05-12 02:09:24 -0600

TifferPelode gravatar image

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.

edit flag offensive delete link more


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

Dimi gravatar image Dimi  ( 2019-08-09 11:24:55 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2018-05-07 02:19:56 -0600

Seen: 1,233 times

Last updated: May 12 '18