ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

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


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