def get_waypoints(segments):
  """ For each bottom_wall, add waypoint in the center.
  Assumption: different bottom segments are ordered according to how drone should fly through.
  Return list of waypoints.
  """
  waypoints=[]
  for seg in segments:
    if seg.attrib['name'].startswith('wall_bottom_'):
      pose_6d=[float(v) for v in seg.find('pose').text.split(' ')]
      # width = float(seg.find('link').find('collision').find('geometry').find('box').find('size').text.split(' ')[0])
      waypoints.append([pose_6d[0],pose_6d[1]])
      print("Found element: {0} with pose {1},{2}.".format(seg.attrib['name'],
                                                                        pose_6d[0],
                                                                        pose_6d[1]))
  return waypoints