How to get the length of navigation path

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

TifferPelode
96 ●9 ●12 ●17

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

add a comment

1

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

PeteBlackerThe3rd

flag of United Kingdom of Great Britain and Northern Ireland

9529 ●30 ●143 ●135 https://www.linkedin.c...

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

Comments

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

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

add a comment

1

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

TifferPelode
96 ●9 ●12 ●17

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

Comments

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

Dimi ( 2019-08-09 11:24:55 -0500 )edit

add a comment

How to get the length of navigation path

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

How to get the length of navigation path edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

How to get the length of navigation path