diff --git a/Control/Jetson/main.py b/Control/Jetson/main.py index 4597505..23a29a0 100755 --- a/Control/Jetson/main.py +++ b/Control/Jetson/main.py @@ -42,14 +42,15 @@ executor_logger.setLevel(logging.DEBUG) #--- 0: + for ix, waypoint in enumerate(waypoints): + target_lat = waypoint['lat'] + target_lon = waypoint['lon'] + executor_logger.info(f"Going to waypoint {ix} at lat: {target_lat}, lon: {target_lon}") + success = __go_to_gps(vehicle_ctrl, lat=target_lat, lon=target_lon) + if not success: + executor_logger.error("Navigate to gps watpoint func returned error, chack logs, aborting") + return False return True def __go_to_gps(vehicle_ctrl, lat: float, lon: float):