in robot_ws/src/cloudwatch_robot/nodes/monitor_distance_to_goal.py [0:0]
def calc_path_distance(self, msg):
points = [(p.pose.position.x, p.pose.position.y) for p in msg.poses]
array = np.array(points, dtype=np.dtype('f8', 'f8'))
return sum((np.linalg.norm(p0 - p1) for p0, p1 in izip(array[:-2], array[1:])))