max/new_thea
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max 2021-09-21 13:23:54 +01:00
parent f3ecc67c8d
commit 77966ca9c7
2 changed files with 98 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Control/Jetson/dronekit_server_client.py
View File

@ -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):

157
Control/Jetson/main.py
View File

@ -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")