Ask Your Question
1

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
1

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

Comments

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

TifferPelode gravatar imageTifferPelode ( 2018-05-07 06:07:59 -0500 )edit
0

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

Comments

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

Dimi gravatar imageDimi ( 2019-08-09 11:24:55 -0500 )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

3 followers

Stats

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

Seen: 727 times

Last updated: May 12 '18