ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

How to get the length of navigation path

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

TifferPelode gravatar image

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

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 -0500

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 -0500 )edit

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

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 -0500 )edit

Question Tools



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

Seen: 2,436 times

Last updated: May 12 '18