Ask Your Question
0

How to calculate the length of a path

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

Dimi gravatar image

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

jayess gravatar image

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

2 Answers

Sort by ยป oldest newest most voted
1

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

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 more

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 gravatar imageDimi ( 2019-08-12 07:59:28 -0500 )edit

Glad you got this working!

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-08-12 09:51:34 -0500 )edit
0

answered 2019-08-11 02:22:34 -0500

femust gravatar image

You need to go through all geometry_msgs/PoseStamped msgs in Path and add segments between consecutive points - so (x2-x1)^2 + (y2-y1)^2

you can go through PoseStamped msgs by something like this: for pose in name_of_the_variable_with_path.poses

edit flag offensive delete link more

Comments

Thank you, I got it !

Dimi gravatar imageDimi ( 2019-08-12 07:56:30 -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

1 follower

Stats

Asked: 2019-08-09 13:03:46 -0500

Seen: 79 times

Last updated: Aug 12