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

# How to calculate the length of a path

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 close merge delete

Sort by ยป oldest newest most voted

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.

more

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

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

( 2019-08-12 09:51:34 -0500 )edit

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

more