From 77966ca9c7fbd75599b9b59e6cfafa5dcbb88731 Mon Sep 17 00:00:00 2001 From: Max Date: Tue, 21 Sep 2021 13:23:54 +0100 Subject: [PATCH] Update --- Control/Jetson/dronekit_server_client.py | 2 +- Control/Jetson/main.py | 157 ++++++++++++++--------- 2 files changed, 98 insertions(+), 61 deletions(-) diff --git a/Control/Jetson/dronekit_server_client.py b/Control/Jetson/dronekit_server_client.py index 2e8ae70..3d03f58 100644 --- a/Control/Jetson/dronekit_server_client.py +++ b/Control/Jetson/dronekit_server_client.py @@ -156,7 +156,7 @@ class DroneKitComms: self.nudge_fork_horiz(go_left=True, amount=0.9) self.nudge_fork_horiz(go_left=True, amount=0.9) time.sleep(2) - self.nudge_fork_horiz(go_left=False, amount=1.0) + self.nudge_fork_horiz(go_left=False, amount=1.1) def approach_pallet(self): diff --git a/Control/Jetson/main.py b/Control/Jetson/main.py index 23a29a0..b6cd248 100755 --- a/Control/Jetson/main.py +++ b/Control/Jetson/main.py @@ -21,6 +21,16 @@ from pallet_hole_alignment_c import HoleAligner # time.sleep(5) # subprocess.run(["route", "-n"]) +DO_GPS_THERE = False +DO_VISION_APPROACH = True +DO_FORK_ALIGN = True +DO_PICKUP_CARGO = True +DO_REVERSE_TURN = False +DO_GPS_RETURN = False +DO_DROPOFF_CARGO = False +DO_RETURN_TO_NEUTRAL = False +DO_LOOP = True + JOB_QUEUE = Queue(maxsize=10) KILL_SIGNAL = False @@ -133,77 +143,104 @@ def queue_executor_thread(): FORKLIFT.set_state("READY_FOR_NEXT_TASK") executor_logger.info("Ready.") - while not KILL_SIGNAL: if not JOB_QUEUE.empty(): current_job = JOB_QUEUE.get() executor_logger.info(f"Executing new job: {current_job}") #execute job..... - FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS") - gps_navigation_succcess = __go_to_gps_waypoints(FORKLIFT, current_job['pickup_waypoints']) - if not gps_navigation_succcess: - executor_logger.critical("Go to GPS was not successful, check logs !") - FORKLIFT.set_state("OTHER_ERROR") - return False - - FORKLIFT.set_state("NAVIGATING_TO_POINT_VISION") - vision_to_cargo_succcess = __go_to_cargo_vision(VISION_CARGO_APPROACH) - if not vision_to_cargo_succcess: - executor_logger.critical("Go to cargo with vision was not successful, check logs !") - FORKLIFT.set_state("OTHER_ERROR") - return False - - FORKLIFT.zero_fork_vert_pickup() - - FORKLIFT.set_state("MANIPULATING_CARGO") - align_forks_sucess = __align_forks_with_holes(HOLE_ALIGNMENT_SYSTEM) - if not align_forks_sucess: - executor_logger.critical("Fork hole alignment was not successful, check logs !") - FORKLIFT.set_state("OTHER_ERROR") - return False - - - FORKLIFT.set_state("MANIPULATING_CARGO") - cargo_pickupe = __pick_up_cargo(FORKLIFT) - if not cargo_pickupe: - executor_logger.critical("Problem with cargo pickup, check logs !") - FORKLIFT.set_state("OTHER_ERROR") - return False - - - reverse_turn = reverse_turn(FORKLIFT) - if not reverse_turn: - executor_logger.critical("Problem with reverse turn, check logs !") - FORKLIFT.set_state("OTHER_ERROR") - return False - - - FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS") - gps_dropoff_succcess = __go_to_gps_waypoints(FORKLIFT, current_job['dropoff_waypoints']) - if not gps_dropoff_succcess: - executor_logger.critical("Go to GPS was not successful, check logs !") - FORKLIFT.set_state("OTHER_ERROR") - return False - - FORKLIFT.set_state("MANIPULATING_CARGO") - cargo_down_success = __put_cargo_down_and_reverse_turn(FORKLIFT) - if not cargo_down_success: - executor_logger.critical("Problem with cargo put down, check logs !") - FORKLIFT.set_state("OTHER_ERROR") - return False - - if current_job['on_finish']['goto'] == True: + if DO_GPS_THERE: FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS") - gps_return_success = __go_to_gps_waypoints(FORKLIFT, current_job['on_finish']['waypoints']) - if not gps_return_success: + gps_navigation_succcess = __go_to_gps_waypoints(FORKLIFT, current_job['pickup_waypoints']) + if not gps_navigation_succcess: executor_logger.critical("Go to GPS was not successful, check logs !") FORKLIFT.set_state("OTHER_ERROR") return False + else: + executor_logger.warning("DO_GPS_THERE flag is false, skipping gps navigation to cargo") - FORKLIFT.set_state("READY_FOR_NEXT_TASK") - logging.info("Task complete... Sleeping for 5 and Waiting for next task") - time.sleep(5) + if DO_VISION_APPROACH: + FORKLIFT.set_state("NAVIGATING_TO_POINT_VISION") + vision_to_cargo_succcess = __go_to_cargo_vision(VISION_CARGO_APPROACH) + if not vision_to_cargo_succcess: + executor_logger.critical("Go to cargo with vision was not successful, check logs !") + FORKLIFT.set_state("OTHER_ERROR") + return False + else: + executor_logger.warning("DO_VISION_APPROACH flag is false, skipping vision navigation to cargo") + + if DO_FORK_ALIGN: + FORKLIFT.set_state("MANIPULATING_CARGO") + FORKLIFT.zero_fork_vert_pickup() + align_forks_sucess = __align_forks_with_holes(HOLE_ALIGNMENT_SYSTEM) + if not align_forks_sucess: + executor_logger.critical("Fork hole alignment was not successful, check logs !") + FORKLIFT.set_state("OTHER_ERROR") + return False + else: + executor_logger.warning("DO_FORK_ALIGN flag is false, skipping fork alignment") + + if DO_PICKUP_CARGO: + FORKLIFT.set_state("MANIPULATING_CARGO") + cargo_pickupe = __pick_up_cargo(FORKLIFT) + if not cargo_pickupe: + executor_logger.critical("Problem with cargo pickup, check logs !") + FORKLIFT.set_state("OTHER_ERROR") + return False + else: + executor_logger.warning("DO_PICKUP_CARGO flag is false, skipping cargo pickup") + + if DO_REVERSE_TURN: + reverse_turn = reverse_turn(FORKLIFT) + if not reverse_turn: + executor_logger.critical("Problem with reverse turn, check logs !") + FORKLIFT.set_state("OTHER_ERROR") + return False + else: + executor_logger.warning("DO_REVERSE_TURN flag is false, skipping reverse turn") + + + if DO_GPS_RETURN: + FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS") + gps_dropoff_succcess = __go_to_gps_waypoints(FORKLIFT, current_job['dropoff_waypoints']) + if not gps_dropoff_succcess: + executor_logger.critical("Go to GPS was not successful, check logs !") + FORKLIFT.set_state("OTHER_ERROR") + return False + else: + executor_logger.warning("DO_GPS_RETURN flag is false, skipping navigationg to dropoff with gps") + + + if DO_DROPOFF_CARGO: + FORKLIFT.set_state("MANIPULATING_CARGO") + cargo_down_success = __put_cargo_down_and_reverse_turn(FORKLIFT) + if not cargo_down_success: + executor_logger.critical("Problem with cargo put down, check logs !") + FORKLIFT.set_state("OTHER_ERROR") + return False + else: + executor_logger.warning("DO_DROPOFF_CARGO flag is false, skipping dropping off cargo") + + + + if current_job['on_finish']['goto'] == True: + if DO_RETURN_TO_NEUTRAL: + FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS") + gps_return_success = __go_to_gps_waypoints(FORKLIFT, current_job['on_finish']['waypoints']) + if not gps_return_success: + executor_logger.critical("Go to GPS was not successful, check logs !") + FORKLIFT.set_state("OTHER_ERROR") + return False + else: + executor_logger.warning("DO_RETURN_TO_NEUTRAL flag is false, skipping returning to neutral point over GPS") + + if DO_LOOP: + FORKLIFT.set_state("READY_FOR_NEXT_TASK") + logging.info("Task complete... Sleeping for 5 and Waiting for next task") + time.sleep(5) + else: + executor_logger.warning("DO_LOOP flag is false, exiting...") + exit(0) executor_logger.info("Kill signal activated, exiting")