How to calculate the length of a path

kinetic

asked 2019-08-09 13:04:35 -0500

Dimi

17 ●3 ●5 ●7

updated 2019-08-09 19:12:56 -0500

jayess
6155 ●26 ●85 ●92

Hello to everyone here, I want to get the length of navigation path. The generated path consists of a sequence of coordinates points (x,y) which i can obtain by subscribing to a topic named as "/move_base/GlobalPlanner/plan". I know that if I calculate the distance of every two points and add them up, I will be able to extract the total path's length.

My subscriber looks like:

sub = rospy.Subscriber("/move_base/GlobalPlanner/plan", Path, callback)

What should the structure of my callback be in order to obtain all the poses of path ? Does anyone have python experience of doing that ?

edit retag flag offensive close merge delete

add a comment

1

answered 2019-08-12 03:41:45 -0500

PeteBlackerThe3rd

flag of United Kingdom of Great Britain and Northern Ireland

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

You need to loop through each adjacent pair of Poses in the path message. You can find the structure of this message described here.

for i in range(len(path_msg.poses) - 1):

  # get two geometry_msgs/Point messages of adjacent points
  position_a = path_msg.poses[i].pose.position
  position_b = path_msg.poses[i+1].pose.position

  # calcualte sum of distances here. . .

Hope this helps.

edit flag offensive delete link

Comments

Thanks for your answer Sir. I've managed to solve it by following your template :

def callback(path_msg):
global path_length
path_length = 0
for i in range(len(path_msg.poses) - 1):
    position_a_x = path_msg.poses[i].pose.position.x
    position_b_x = path_msg.poses[i+1].pose.position.x
    position_a_y = path_msg.poses[i].pose.position.y
    position_b_y = path_msg.poses[i+1].pose.position.y

    path_length += np.sqrt(np.power((position_b_x - position_a_x), 2) + np.power((position_b_y- position_a_y), 2))

Dimi ( 2019-08-12 07:59:28 -0500 )edit

Glad you got this working!