314 lines
12 KiB
Python
Executable File
314 lines
12 KiB
Python
Executable File
#!/usr/bin/python3
|
|
import logging
|
|
import signal
|
|
import subprocess
|
|
import sys
|
|
import threading
|
|
import time
|
|
from queue import Queue
|
|
import os
|
|
import psutil
|
|
|
|
import flask
|
|
from flask import jsonify, request
|
|
|
|
from dronekit_server_client import DroneKitComms
|
|
from vision_cargo_approach import VisionCargoApproacher
|
|
from pallet_hole_alignment_c import HoleAligner
|
|
|
|
|
|
# def reinit_networking():
|
|
# logging.info('Resetting dhcpcd service, wait 5 seconds...')
|
|
# subprocess.run(["sudo", "service", "dhcpcd", "restart"])
|
|
# time.sleep(5)
|
|
# subprocess.run(["route", "-n"])
|
|
|
|
DO_ZERO = True
|
|
DO_GPS_THERE = True
|
|
DO_GOTO_TARGET_HEADING = True
|
|
DO_VISION_APPROACH = True
|
|
DO_FORK_ALIGN = True
|
|
DO_PICKUP_CARGO = True
|
|
DO_REVERSE_TURN = True
|
|
DO_GPS_RETURN = False
|
|
DO_DROPOFF_CARGO = True
|
|
DO_RETURN_TO_NEUTRAL = False
|
|
DO_LOOP = True
|
|
|
|
|
|
JOB_QUEUE = Queue(maxsize=10)
|
|
KILL_SIGNAL = False
|
|
|
|
def exit_handler(*_):
|
|
global KILL_SIGNAL
|
|
print('\nROOT: CTRL-C pressed, killing everything')
|
|
KILL_SIGNAL = True
|
|
sys.exit(0)
|
|
signal.signal(signal.SIGINT, exit_handler)
|
|
|
|
logging.basicConfig()
|
|
logging.root.setLevel(logging.NOTSET)
|
|
logging.basicConfig(level=logging.NOTSET)
|
|
|
|
executor_logger = logging.getLogger("LOGISTICS EXECUTOR")
|
|
executor_logger.setLevel(logging.DEBUG)
|
|
|
|
|
|
#--- <Pseudoprivate methods
|
|
|
|
def __go_to_gps_waypoints(vehicle_ctrl, waypoints: list):
|
|
if len(waypoints) > 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):
|
|
executor_logger.info(f"Starting GPS navigation to lat: {lat}, lon: {lon}")
|
|
timeout_time = 300 + time.time() # 5 minutes to get to target
|
|
vehicle_in_target = False
|
|
vehicle_ctrl.go_to_gps(lat=lat, lon=lon)
|
|
while vehicle_in_target == False and time.time() < timeout_time:
|
|
time.sleep(0.2) # Don't DDoS the Pi!
|
|
vehicle_in_target, distance = vehicle_ctrl.check_gps_in_radius((lat, lon))
|
|
if vehicle_in_target:
|
|
executor_logger.info(f'We are in radius ({distance})')
|
|
return True
|
|
else:
|
|
executor_logger.critical(f"5 minuted have elapsed and we are not in the target area ({distance}), aborting")
|
|
return False
|
|
|
|
def __go_to_cargo_vision(vision_system):
|
|
executor_logger.info(f"Starting vision to cargo navigation")
|
|
success = vision_system.navigate_to_cargo_vision()
|
|
return success
|
|
|
|
def __align_forks_with_holes(hole_alignment_system):
|
|
executor_logger.info(f"Starting pallet hole alignment")
|
|
success = hole_alignment_system.align_fork()
|
|
return success
|
|
|
|
def __pick_up_cargo(vehicle_ctrl):
|
|
vehicle_ctrl.approach_pallet()
|
|
time.sleep(1)
|
|
vehicle_ctrl.zero_fork_vert_movement()
|
|
return True
|
|
|
|
def __put_down_cargo(vehicle_ctrl):
|
|
vehicle_ctrl.zero_fork_vert_pickup()
|
|
time.sleep(1)
|
|
vehicle_ctrl.reverse_from_pallet()
|
|
return True
|
|
|
|
|
|
def __reverse_turn(vehicle_ctrl):
|
|
executor_logger.info(f"performing reverse turn")
|
|
vehicle_ctrl.nudge_drive(False, 5, 2000)
|
|
vehicle_ctrl.nudge_drive(False, 6, 2000)
|
|
vehicle_ctrl.nudge_drive(True, 5, 1000)
|
|
vehicle_ctrl.nudge_drive(True, 5, 1000)
|
|
vehicle_ctrl.nudge_drive(True, 6, 1000)
|
|
|
|
return True
|
|
|
|
#--- <Pseudoprivate methods
|
|
|
|
|
|
def queue_executor_thread():
|
|
executor_logger.info("Starting task executor...")
|
|
executor_logger.info("Reiniting networking...")
|
|
# reinit_networking() # Reinits dhcpcd to enable Pi communications ---We fixed this by tweaking /etc/network/interfaces
|
|
executor_logger.info("Init vehicle control...")
|
|
FORKLIFT = DroneKitComms()
|
|
executor_logger.info("Importing Vision system...")
|
|
VISION_CARGO_APPROACH = VisionCargoApproacher(FORKLIFT)
|
|
executor_logger.info("Importing Pallet hole aligner...")
|
|
HOLE_ALIGNMENT_SYSTEM = HoleAligner(FORKLIFT)
|
|
executor_logger.info("ARMING VEHICLE...")
|
|
FORKLIFT.set_arm()
|
|
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.....
|
|
|
|
if DO_ZERO:
|
|
executor_logger.info("Zeroing fork...")
|
|
FORKLIFT.zero_fork_vert_movement()
|
|
FORKLIFT.zero_fork_horiz()
|
|
else:
|
|
executor_logger.warning("DO_ZERO flag is false, skipping zeroing")
|
|
|
|
if DO_GPS_THERE:
|
|
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
|
|
else:
|
|
executor_logger.warning("DO_GPS_THERE flag is false, skipping gps navigation to cargo")
|
|
|
|
if DO_GOTO_TARGET_HEADING:
|
|
FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
|
|
heading_set_success = FORKLIFT.go_to_heading(current_job['pickup_heading'])
|
|
if not heading_set_success:
|
|
executor_logger.critical("Go to GPS HEADING was not successful, check logs !")
|
|
FORKLIFT.set_state("OTHER_ERROR")
|
|
return False
|
|
else:
|
|
executor_logger.warning("DO_GOTO_TARGET_HEADING flag is false, skipping GOTO target heading")
|
|
|
|
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()
|
|
executor_logger.info("Aligning forks with holes...")
|
|
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_success = __reverse_turn(FORKLIFT)
|
|
if not reverse_turn_success:
|
|
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_down_cargo(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 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 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")
|
|
|
|
|
|
job_executor = threading.Thread(target=queue_executor_thread)
|
|
job_executor.start()
|
|
|
|
|
|
app = flask.Flask("task_upload_server")
|
|
app.logger.setLevel("DEBUG")
|
|
|
|
DEFULT_JOB_TEMPLATE = {
|
|
"job_id": 0,
|
|
"cargo_id": 0,
|
|
"pickup_waypoints": [{"lat": 0, "lon": 0}, {"lat": 1, "lon": 1}, {"lat": 2, "lon": 2}],
|
|
"dropoff_waypoints": [{"lat": 2, "lon": 2}, {"lat": 3, "lon": 3}, {"lat": 4, "lon": 4}],
|
|
"on_finish": {"goto": True, "waypoints": [{"lat": 5, "lon": 5}, {"lat": 1, "lon": 1}, {"lat": 0, "lon": 0}]},
|
|
}
|
|
|
|
DEFAULT_JOB_TEMPLATE_KEYS = DEFULT_JOB_TEMPLATE.keys()
|
|
|
|
def verify_json_data(uploaded_data):
|
|
data_keys = uploaded_data.keys()
|
|
for item in DEFAULT_JOB_TEMPLATE_KEYS:
|
|
if item not in data_keys:
|
|
app.logger.error(f"Item {item} not in uploaded job uploaded_data keys")
|
|
app.logger.error(f"uploaded job dump: {uploaded_data}")
|
|
return False
|
|
return True
|
|
|
|
@app.route('/', methods=['GET'])
|
|
def home():
|
|
return "<h1>Logistics executionner is running...</h1>"
|
|
|
|
@app.route('/alive', methods=['POST'])
|
|
def alive():
|
|
app.logger.info(f"Returning alive")
|
|
return jsonify({'Success': True})
|
|
|
|
@app.route('/add_job', methods=['POST'])
|
|
def add_job():
|
|
if request.is_json:
|
|
job_request = request.get_json()
|
|
if not verify_json_data(job_request):
|
|
app.logger.error("Error processing job request: check logs...")
|
|
return jsonify({'Error': 'Error processing job request: check logs...'})
|
|
app.logger.info(f"Got valid job request: {job_request}")
|
|
JOB_QUEUE.put(job_request)
|
|
return jsonify({'Success': True})
|
|
else:
|
|
return jsonify({'Error': 'Not JSON'})
|
|
|
|
|
|
app.run(host='0.0.0.0', port=8080)
|