1357 lines
50 KiB
Python
Executable File
1357 lines
50 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
from alive_progress import alive_bar
|
|
import get_specs
|
|
import traceback
|
|
#import logging
|
|
import yaml
|
|
from multiprocessing import Process, Manager, Pool, TimeoutError, active_children, log_to_stderr, Pipe, Queue
|
|
from multiprocessing.pool import Pool
|
|
import multiprocessing
|
|
from time import sleep
|
|
from util import fprint
|
|
from util import run_cmd
|
|
from util import win32
|
|
import sys
|
|
import ur5_control
|
|
from ur5_control import Rob
|
|
import os
|
|
import signal
|
|
import socket
|
|
from flask import Flask, render_template, request
|
|
import requests
|
|
from led_control import LEDSystem
|
|
import server
|
|
import asyncio
|
|
import json
|
|
import process_video
|
|
import search
|
|
from search import JukeboxSearch
|
|
#multiprocessing.set_start_method('spawn', True)
|
|
from pyModbusTCP.client import ModbusClient
|
|
from uptime import uptime
|
|
import fileserver
|
|
import paho.mqtt.client as mqtt
|
|
import pickle
|
|
import time
|
|
import subprocess
|
|
|
|
# set to false to run without real hardware for development
|
|
real = False
|
|
skip_scanning = True
|
|
|
|
mbconn = None
|
|
config = None
|
|
keeprunning = True
|
|
arm_ready = False
|
|
led_ready = False
|
|
camera_ready = False
|
|
sensor_ready = False
|
|
vm_ready = False
|
|
cable_search_ready = False
|
|
killme = None
|
|
#pool = None
|
|
serverproc = None
|
|
camera = None
|
|
ledsys = None
|
|
arm = None
|
|
to_server_queue = Queue()
|
|
from_server_queue = Queue()
|
|
mainloop_get = Queue()
|
|
mode = "Startup"
|
|
oldmode = "Startup"
|
|
counter = 0
|
|
jbs = None
|
|
scan_value = None
|
|
arm_state = None
|
|
cable_list = list()
|
|
parse_res = None
|
|
cable_list_state = list()
|
|
just_placed = -1
|
|
ring_animation = None
|
|
led_set_mode = None
|
|
sensors = [0,0,0,0]
|
|
websocket_process = None
|
|
arm_updates = None
|
|
animation_wait = False
|
|
arm_position = (0,0,0,0,0,0)
|
|
arm_position_process = None
|
|
start_animation = False
|
|
failcount = 0
|
|
timecount = 0
|
|
secondsclock = 0
|
|
unacked_publish = set()
|
|
mqttc = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)
|
|
counter_file = 'pick_count.txt'
|
|
cycle_start_time = time.time()
|
|
arm_distance = 0
|
|
arm_distance_old = 0
|
|
arm_distance_total = 0
|
|
kill_ssh = False
|
|
mqttc.user_data_set(unacked_publish)
|
|
spot = -1
|
|
placed = 0
|
|
|
|
def arm_start_callback(res):
|
|
fprint("Arm action complete.")
|
|
global arm_ready
|
|
arm_ready = True
|
|
|
|
|
|
def led_start_callback(res):
|
|
global led_ready
|
|
led_ready = True
|
|
global ledsys
|
|
ledsys = res
|
|
|
|
def camera_start_callback(res):
|
|
global camera_ready
|
|
camera_ready = True
|
|
global scan_value
|
|
scan_value = res
|
|
|
|
def sensor_start_callback(res):
|
|
global sensor_ready
|
|
sensor_ready = True
|
|
|
|
def vm_start_callback(res):
|
|
global vm_ready
|
|
vm_ready = True
|
|
|
|
def cable_search_callback(res):
|
|
global cable_search_ready
|
|
cable_search_ready = True
|
|
global parse_res
|
|
parse_res = res
|
|
|
|
def wait_for(val, name):
|
|
#global val
|
|
if val is False:
|
|
fprint("waiting for " + name + " to complete...")
|
|
while val is False:
|
|
sleep(0.1)
|
|
|
|
def send_data(type, call, data, client_id="*"):
|
|
out = dict()
|
|
out["type"] = type
|
|
out["call"] = call
|
|
out["data"] = data
|
|
to_server_queue.put((client_id, json.dumps(out)))
|
|
|
|
def initialize_counter():
|
|
global counter
|
|
try:
|
|
with open(counter_file, 'rb') as file:
|
|
counter = int(file.readline().decode())
|
|
fprint("Read count at " + str(counter))
|
|
if not isinstance(counter, int):
|
|
raise ValueError("Counter is not an integer")
|
|
|
|
except (FileNotFoundError, ValueError) as e:
|
|
counter = 101
|
|
fprint("Error. Resetting count." + str(e))
|
|
fprint(counter)
|
|
save_counter()
|
|
|
|
fprint(counter)
|
|
|
|
# Save the counter to the file
|
|
def save_counter():
|
|
global counter
|
|
fprint(counter)
|
|
with open(counter_file, 'wb') as file:
|
|
file.write(str(counter).encode())
|
|
|
|
# Increment the counter
|
|
def increment_counter():
|
|
global counter
|
|
fprint(counter)
|
|
fprint("Setting count to " + str(counter + 1))
|
|
counter = counter + 1
|
|
save_counter()
|
|
|
|
def check_server():
|
|
#print("HI")
|
|
global cable_list
|
|
global to_server_queue
|
|
global from_server_queue
|
|
global jbs
|
|
if True:
|
|
# Handeling Server Requests Loop, will run forever
|
|
|
|
if not from_server_queue.empty():
|
|
client_id, message = from_server_queue.get()
|
|
fprint(f"Message from client {client_id}: {message}")
|
|
|
|
# Message handler
|
|
try:
|
|
decoded = json.loads(message)
|
|
except:
|
|
fprint("Non-JSON message recieved")
|
|
return
|
|
|
|
if "type" not in decoded:
|
|
fprint("Missing \"type\" field.")
|
|
return
|
|
if "call" not in decoded:
|
|
fprint("Missing \"call\" field.")
|
|
return
|
|
if "data" not in decoded:
|
|
fprint("Missing \"data\" field.")
|
|
return
|
|
# if we get here, we have a "valid" data packet
|
|
data = decoded["data"]
|
|
call = decoded["call"]
|
|
try:
|
|
match decoded["type"]:
|
|
case "log":
|
|
fprint("log message")
|
|
if call == "send":
|
|
fprint("webapp: " + str(data), sendqueue=to_server_queue)
|
|
elif call == "request":
|
|
pass
|
|
case "cable_map":
|
|
fprint("cable_map message")
|
|
if call == "send":
|
|
pass
|
|
elif call == "request":
|
|
tmp = list()
|
|
for idx in range(len(cable_list)):
|
|
if cable_list[idx] is not False:
|
|
cabledata = jbs.get_position(str(idx))
|
|
fs = cabledata["fullspecs"]
|
|
tmp1 = {"part_number": cable_list[idx], "position": idx, "name": cable_list[idx], "brand": cabledata["brand"] }
|
|
if "Product Overview" in fs and "Product Category" in fs["Product Overview"]:
|
|
tmp1["category"] = fs["Product Overview"]["Product Category"]
|
|
if "Product Overview" in fs and "Suitable Applications" in fs["Product Overview"]:
|
|
if len(fs["Product Overview"]["Suitable Applications"]) == 0 or not isinstance(fs["Product Overview"]["Suitable Applications"], str):
|
|
for key, value in fs["Product Overview"].items():
|
|
#print(key,value)
|
|
if len(value) > 5 and isinstance(value, str):
|
|
tmp1["application"] = value
|
|
elif len(key) > 15 and not isinstance(value, str):
|
|
tmp1["application"] = key
|
|
else:
|
|
tmp1["application"] = fs["Product Overview"]["Suitable Applications"]
|
|
elif "Product Overview" in fs and "Suitable Applications:" in fs["Product Overview"]:
|
|
if len(fs["Product Overview"]["Suitable Applications:"]) == 0 or not isinstance(fs["Product Overview"]["Suitable Applications:"], str):
|
|
for key, value in fs["Product Overview"].items():
|
|
#print(key,value)
|
|
if len(value) > 5 and isinstance(value, str):
|
|
tmp1["application"] = value
|
|
elif len(key) > 15 and not isinstance(value, str):
|
|
tmp1["application"] = key
|
|
else:
|
|
tmp1["application"] = fs["Product Overview"]["Suitable Applications:"]
|
|
|
|
if "image" in cabledata:
|
|
tmp1["image"] = cabledata["image"]
|
|
if "datasheet" in cabledata:
|
|
tmp1["datasheet"] = cabledata["datasheet"]
|
|
if "description" in cabledata:
|
|
tmp1["description"] = cabledata["description"]
|
|
if "short_description" in cabledata:
|
|
tmp1["short_description"] = cabledata["short_description"]
|
|
if "application" in cabledata:
|
|
tmp1["application"] = cabledata["application"]
|
|
if "category" in cabledata:
|
|
tmp1["category"] = cabledata["category"]
|
|
|
|
tmp.append(tmp1)
|
|
out = {"map": tmp}
|
|
fprint(out)
|
|
send_data(decoded["type"], "send", out, client_id)
|
|
|
|
case "ping":
|
|
fprint("Pong!!!")
|
|
|
|
case "mode":
|
|
if call == "request":
|
|
send_data("mode", "send", "{\"mode\": \"" + mode + "\" }")
|
|
# Lucas' notes
|
|
# Add a ping pong :) response/handler
|
|
# Add a get cable response/handler
|
|
# this will tell the robot arm to move
|
|
# Call for turning off everything
|
|
# TODO Helper for converting Python Dictionaries to JSON
|
|
# make function: pythonData --> { { "type": "...", "call": "...", "data": pythonData } }
|
|
|
|
# to send: to_server_queue.put(("*", "JSON STRING HERE")) # replace * with UUID of client to send to one specific location
|
|
|
|
case "cable_details":
|
|
fprint("cable_details message")
|
|
if call == "send":
|
|
pass
|
|
elif call == "request":
|
|
dataout = dict()
|
|
dataout["cables"] = list()
|
|
print(data)
|
|
if "part_number" in data:
|
|
for part in data["part_number"]:
|
|
#print(part)
|
|
#print(jbs.get_partnum(part))
|
|
dataout["cables"].append(jbs.get_partnum(part)["fullspecs"])
|
|
if "position" in data:
|
|
for pos in data["position"]:
|
|
#print(pos)
|
|
#print(jbs.get_position(str(pos)))
|
|
dataout["cables"].append(jbs.get_position(str(pos))["fullspecs"])
|
|
send_data(decoded["type"], "send", dataout, client_id)
|
|
|
|
case "cable_search":
|
|
fprint("cable_search message")
|
|
if call == "send":
|
|
pass
|
|
elif call == "request":
|
|
results = jbs.search(data["string"])["hits"]
|
|
dataout = dict()
|
|
dataout["map"] = list()
|
|
for cabledata in results:
|
|
|
|
fs = cabledata["fullspecs"]
|
|
tmp1 = {"part_number": cabledata["partnum"], "position": cabledata["position"], "name": cabledata["partnum"], "brand": cabledata["brand"] }
|
|
if "Product Overview" in fs and "Product Category" in fs["Product Overview"]:
|
|
tmp1["category"] = fs["Product Overview"]["Product Category"]
|
|
if "Product Overview" in fs and "Suitable Applications" in fs["Product Overview"]:
|
|
if len(fs["Product Overview"]["Suitable Applications"]) == 0 or not isinstance(fs["Product Overview"]["Suitable Applications"], str):
|
|
for key, value in fs["Product Overview"].items():
|
|
#print(key,value)
|
|
if len(value) > 5 and isinstance(value, str):
|
|
tmp1["application"] = value
|
|
elif len(key) > 15 and not isinstance(value, str):
|
|
tmp1["application"] = key
|
|
else:
|
|
tmp1["application"] = fs["Product Overview"]["Suitable Applications"]
|
|
elif "Product Overview" in fs and "Suitable Applications:" in fs["Product Overview"]:
|
|
if len(fs["Product Overview"]["Suitable Applications:"]) == 0 or not isinstance(fs["Product Overview"]["Suitable Applications:"], str):
|
|
for key, value in fs["Product Overview"].items():
|
|
#print(key,value)
|
|
if len(value) > 5 and isinstance(value, str):
|
|
tmp1["application"] = value
|
|
elif len(key) > 15 and not isinstance(value, str):
|
|
tmp1["application"] = key
|
|
else:
|
|
tmp1["application"] = fs["Product Overview"]["Suitable Applications:"]
|
|
if "image" in cabledata:
|
|
tmp1["image"] = cabledata["image"]
|
|
if "datasheet" in cabledata:
|
|
tmp1["datasheet"] = cabledata["datasheet"]
|
|
if "description" in cabledata:
|
|
tmp1["description"] = cabledata["description"]
|
|
if "short_description" in cabledata:
|
|
tmp1["short_description"] = cabledata["short_description"]
|
|
if "application" in cabledata:
|
|
tmp1["application"] = cabledata["application"]
|
|
if "category" in cabledata:
|
|
tmp1["category"] = cabledata["category"]
|
|
|
|
dataout["map"].append(tmp1)
|
|
|
|
# after loop
|
|
send_data(decoded["type"], "send", dataout, client_id)
|
|
|
|
case "keyboard":
|
|
fprint("keyboard message")
|
|
if call == "send":
|
|
pass
|
|
elif call == "request":
|
|
if data["enabled"] == True:
|
|
# todo : send this to client
|
|
p = Process(target=run_cmd, args=("./keyboard-up.ps1",))
|
|
p.start()
|
|
elif data["enabled"] == False:
|
|
p = Process(target=run_cmd, args=("./keyboard-down.ps1",))
|
|
p.start()
|
|
case "machine_settings":
|
|
fprint("machine_settings message")
|
|
if call == "send":
|
|
pass
|
|
elif call == "request":
|
|
pass
|
|
|
|
case "cable_get":
|
|
fprint("cable_get message")
|
|
if call == "send":
|
|
global mainloop_get
|
|
|
|
if "part_number" in data:
|
|
for cableidx in range(len(cable_list)):
|
|
cable = cable_list[cableidx]
|
|
if cable == data["part_number"]:
|
|
fprint("Adding cable to dispense queue")
|
|
mainloop_get.put(("pickup", cableidx))
|
|
elif "position" in data:
|
|
fprint("Adding cable to dispense queue")
|
|
mainloop_get.put(("pickup", data["position"]))
|
|
elif "tray" in data:
|
|
fprint("Adding tray return to dispense queue")
|
|
mainloop_get.put(("returnCheck", 0))
|
|
#mainloop_get.put(("return", data["tray"]))
|
|
else:
|
|
fprint("Invalid data.")
|
|
elif call == 'request':
|
|
if "position" in data:
|
|
mainloop_get.put(("show", data["position"]))
|
|
|
|
case "shutdown":
|
|
mainloop_get.put(("shutdown", 0))
|
|
case _:
|
|
fprint("Unknown/unimplemented data type: " + decoded["type"])
|
|
except Exception as e:
|
|
fprint(traceback.format_exc())
|
|
fprint(e)
|
|
|
|
|
|
|
|
|
|
#sleep(0.001) # Sleep to prevent tight loop
|
|
|
|
|
|
|
|
def start_client_socket():
|
|
app = Flask(__name__)
|
|
|
|
@app.route('/control_client', methods=['POST'])
|
|
def message_from_server():
|
|
# Handle message from server
|
|
data = request.json
|
|
fprint(f"Message from server: {data.get('message')}")
|
|
return "Message received", 200
|
|
|
|
app.run(host='0.0.0.0', port=6000)
|
|
|
|
|
|
def check_server_online(serverip, clientip):
|
|
def send_ip_to_server(server_url, client_ip):
|
|
try:
|
|
response = requests.post(server_url, json={'ip': client_ip}, timeout=1)
|
|
fprint(f"Server response: {response.text}")
|
|
return True
|
|
except requests.exceptions.RequestException as e:
|
|
fprint(f"Error sending IP to server: {e}")
|
|
return False
|
|
|
|
server_url = 'http://' + serverip + ':5000/report_ip'
|
|
while not send_ip_to_server(server_url, clientip):
|
|
sleep(1)
|
|
|
|
fprint("Successfully connected to server.")
|
|
return True
|
|
|
|
|
|
def setup_server(pool, manager):
|
|
# linux server setup
|
|
global config
|
|
global counter
|
|
global sensor_ready
|
|
global camera_ready
|
|
global led_ready
|
|
global arm_ready
|
|
global serverproc
|
|
global camera
|
|
global arm
|
|
global jbs
|
|
global to_server_queue
|
|
global from_server_queue
|
|
global websocket_process
|
|
global arm_updates
|
|
global arm_position_process
|
|
global ledsys
|
|
arm_updates = manager.Queue()
|
|
arm = Rob(config)
|
|
if real:
|
|
pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error)
|
|
else:
|
|
arm_ready = True
|
|
|
|
if real:
|
|
ledsys = LEDSystem()
|
|
#pool.apply_async(ledsys.init, callback=led_start_callback)
|
|
#pool.apply_async(sensor_control.init, callback=sensor_start_callback)
|
|
jbs = JukeboxSearch()
|
|
|
|
|
|
|
|
if led_ready is False:
|
|
fprint("waiting for " + "LED controller initialization" + " to complete...", sendqueue=to_server_queue)
|
|
if real:
|
|
ledsys.init()
|
|
led_ready = True
|
|
fprint("LED controllers initialized.", sendqueue=to_server_queue)
|
|
|
|
if sensor_ready is False:
|
|
fprint("waiting for " + "Sensor Initialization" + " to complete...", sendqueue=to_server_queue)
|
|
global mbconn
|
|
if real:
|
|
mbconn = ModbusClient(host="192.168.1.20", port=502, auto_open=False, auto_close=False)
|
|
get_sensors()
|
|
fprint("Sensors initialized.", sendqueue=to_server_queue)
|
|
|
|
if camera_ready is False:
|
|
fprint("waiting for " + "Camera initilization" + " to complete...", sendqueue=to_server_queue)
|
|
camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], int(config["cameras"]["banner"]["port"]))
|
|
|
|
fprint("Camera initialized.", sendqueue=to_server_queue)
|
|
|
|
#arm_ready = True
|
|
if arm_ready is False:
|
|
fprint("waiting for " + "UR5 powerup" + " to complete...", sendqueue=to_server_queue)
|
|
while arm_ready is False:
|
|
sleep(0.1)
|
|
|
|
if real:
|
|
ur5_control.init_arm(arm)
|
|
fprint("Arm initialized.", sendqueue=to_server_queue)
|
|
|
|
fprint("Creating arm position watcher...")
|
|
arm_position_process = pool.apply_async(ur5_control.get_position_thread, (arm,arm_updates))
|
|
|
|
fprint("Starting websocket server...", sendqueue=to_server_queue)
|
|
websocket_process = server.start_websocket_server(to_server_queue, from_server_queue)
|
|
|
|
fprint("Starting file server...", sendqueue=to_server_queue)
|
|
image_server_process = Process(target=fileserver.run_server, args=(config["cables"]["port"], config["cables"]["directory"]))
|
|
image_server_process.start()
|
|
|
|
if config["mqtt"]["enabled"]:
|
|
global mqttc
|
|
global unacked_publish
|
|
mqttc.on_publish = on_publish
|
|
|
|
mqttc.user_data_set(unacked_publish)
|
|
mqttc.connect(config["mqtt"]["server"])
|
|
mqttc.loop_start()
|
|
|
|
|
|
|
|
return True
|
|
|
|
def mqtt_send(msg, name):
|
|
global config
|
|
if config["mqtt"]["enabled"]:
|
|
global mqttc
|
|
global unacked_publish
|
|
msg_info = mqttc.publish("jukebox/" + name, msg, qos=1)
|
|
unacked_publish.add(msg_info.mid)
|
|
while len(unacked_publish):
|
|
sleep(0.01)
|
|
|
|
# Due to race-condition described above, the following way to wait for all publish is safer
|
|
msg_info.wait_for_publish()
|
|
|
|
def handle_error(error):
|
|
print(error, flush=True)
|
|
|
|
def get_sensors():
|
|
global mbconn
|
|
global sensors
|
|
#print("Reading sensors")
|
|
if not real:
|
|
return -1
|
|
if not mbconn.is_open:
|
|
mbconn.open()
|
|
"""
|
|
port 1: 256
|
|
port 2: 272
|
|
port 3: 288
|
|
port 4: 304
|
|
port 5: 320
|
|
port 6: 336
|
|
port 7: 352
|
|
port 8: 368
|
|
|
|
"""
|
|
if real:
|
|
sens = [320, 336, 352, 368]
|
|
for idx in range(len(sens)):
|
|
reg = sens[idx]
|
|
val = mbconn.read_holding_registers(reg)
|
|
#fprint("Sensor " + str(idx) + " = " + str(val))
|
|
if val is not None:
|
|
val = val[0]
|
|
if val == 1: # skip negative values
|
|
sensors[idx] += 1
|
|
elif val == 0:
|
|
sensors[idx] -= 1
|
|
|
|
|
|
|
|
|
|
else:
|
|
sensors = [-10, -10, -10, -10]
|
|
|
|
#fprint("Values: " + str(sensors))
|
|
#mbconn.close()
|
|
for x in range(len(sensors)):
|
|
if sensors[x] > 10:
|
|
sensors[x] = 10
|
|
|
|
if sensors[x] < -10:
|
|
sensors[x] = -10
|
|
|
|
return -1
|
|
|
|
def get_open_spot(sensordata):
|
|
for x in range(len(sensors)):
|
|
sens = sensors[x]
|
|
if sens <= -5:
|
|
print("Open spot: " + str(x))
|
|
return x
|
|
|
|
# if we get here, every spot is full
|
|
fprint("No spots empty")
|
|
return False
|
|
|
|
def get_full_spot(sensordata):
|
|
for x in range(len(sensors)):
|
|
sens = sensors[x]
|
|
if sens >= 3:
|
|
print("Full spot: " + str(x))
|
|
return x
|
|
|
|
# if we get here, every spot is empty
|
|
fprint("No spots full")
|
|
return False
|
|
|
|
def get_spot(sensordata, idx):
|
|
if sensordata[idx] >= 3:
|
|
return True
|
|
elif sensordata[idx] <= -5:
|
|
return False
|
|
else:
|
|
return False
|
|
|
|
def on_publish(client, userdata, mid, reason_code, properties):
|
|
# reason_code and properties will only be present in MQTTv5. It's always unset in MQTTv3
|
|
try:
|
|
userdata.remove(mid)
|
|
except KeyError:
|
|
print("on_publish() is called with a mid not present in unacked_publish")
|
|
print("This is due to an unavoidable race-condition:")
|
|
print("* publish() return the mid of the message sent.")
|
|
print("* mid from publish() is added to unacked_publish by the main thread")
|
|
print("* on_publish() is called by the loop_start thread")
|
|
print("While unlikely (because on_publish() will be called after a network round-trip),")
|
|
print(" this is a race-condition that COULD happen")
|
|
print("")
|
|
print("The best solution to avoid race-condition is using the msg_info from publish()")
|
|
print("We could also try using a list of acknowledged mid rather than removing from pending list,")
|
|
print("but remember that mid could be re-used !")
|
|
|
|
|
|
def mainloop_server(pool, manager):
|
|
# NON-blocking loop
|
|
global real
|
|
global ring_animation
|
|
global led_set_mode
|
|
global just_placed
|
|
global config
|
|
global counter
|
|
global killme
|
|
global mode
|
|
global jbs
|
|
global arm
|
|
global ledsys
|
|
global camera
|
|
global arm_ready
|
|
global arm_state
|
|
global camera_ready
|
|
global cable_search_ready
|
|
global cable_list
|
|
global mainloop_get
|
|
global cable_list_state
|
|
global scan_value
|
|
global sensors
|
|
global oldmode
|
|
global arm_updates
|
|
global animation_wait
|
|
global arm_position
|
|
global arm_position_process
|
|
global start_animation
|
|
global failcount
|
|
global timecount
|
|
global secondsclock
|
|
global cycle_start_time
|
|
global arm_distance
|
|
global arm_distance_old
|
|
global arm_distance_total
|
|
global placed
|
|
global spot
|
|
|
|
if mode != oldmode:
|
|
print(" ***** Running mode:", mode, "***** ")
|
|
send_data("mode", "send", "{\"mode\": \"" + mode + "\" }")
|
|
oldmode = mode
|
|
mqtt_send(mode, "mode")
|
|
if mode == "Startup": # very first loop
|
|
pass
|
|
initialize_counter()
|
|
|
|
if killme.value > 0:
|
|
killall()
|
|
|
|
# check for messages
|
|
check_server()
|
|
|
|
|
|
# do every loop!
|
|
checkpoint = None
|
|
val = None
|
|
if not arm_updates.empty():
|
|
val = arm_updates.get()
|
|
|
|
if isinstance(val, tuple):
|
|
arm_position = val
|
|
if arm_distance_old == 0:
|
|
arm_distance_old = val[0] + val[1] + val[2]
|
|
arm_distance = 0
|
|
else:
|
|
arm_distance += val[0] + val[1] + val[2] - arm_distance_old
|
|
arm_distance_old = val[0] + val[1] + val[2]
|
|
else:
|
|
print("Arm queue message " + str(val))
|
|
checkpoint = val
|
|
print(ring_animation, animation_wait, ledsys.mode, arm_position)
|
|
|
|
|
|
if start_animation and real:
|
|
# animation start requested
|
|
# may not be immediate
|
|
if ring_animation is not None:
|
|
if animation_wait:
|
|
# wait for checkpoint
|
|
if checkpoint is not None:
|
|
fprint("Starting checkpointed animation " + str(led_set_mode) + " for ring " + str(ring_animation))
|
|
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
|
led_set_mode = None
|
|
animation_wait = False
|
|
start_animation = False
|
|
|
|
else:
|
|
# still waiting
|
|
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
|
|
|
else:
|
|
# no waiting, just start
|
|
fprint("Starting immediate animation " + str(led_set_mode) + " for ring " + str(ring_animation))
|
|
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
|
led_set_mode = None
|
|
animation_wait = False
|
|
start_animation = False
|
|
else:
|
|
# no ring animation specified
|
|
pass
|
|
|
|
else:
|
|
# no new animation
|
|
if ring_animation is not None and real:
|
|
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
|
|
|
elif real:
|
|
ledsys.mainloop(None, -1, arm_position=arm_position)
|
|
|
|
# every 1 second
|
|
if secondsclock >= config["core"]["loopspeed"] / 2:
|
|
secondsclock = 1
|
|
arm_distance_total += abs(arm_distance)
|
|
if abs(arm_distance) < 0.001:
|
|
arm_distance = 0.0
|
|
|
|
mqtt_send("{\"value\": " + str(abs(arm_distance)) + " }", "arm_speed")
|
|
mqtt_send("{\"value\": " + str(abs(arm_distance_total)) + " }", "arm_distance")
|
|
arm_distance_old = 0 # reset counter
|
|
get_sensors()
|
|
|
|
|
|
|
|
else:
|
|
secondsclock += 1
|
|
# if start_animation is False and ring_animation is not None and ledsys.mode != "Idle" and real:
|
|
# ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
|
|
|
# elif start_animation is True and ring_animation is not None and real:
|
|
# if animation_wait:
|
|
# if checkpoint is not None: # got to checkpoint from UR5
|
|
# fprint("Starting checkpointed animation " + str(led_set_mode) + " for ring " + str(ring_animation))
|
|
# ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
|
# led_set_mode = None
|
|
# animation_wait = False
|
|
# start_animation = False
|
|
# else:
|
|
# fprint("Starting immediate animation " + str(led_set_mode) + " for ring " + str(ring_animation))
|
|
# ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
|
# led_set_mode = None
|
|
# start_animation = False
|
|
# else:
|
|
# ledsys.mainloop(None, 49, arm_position=arm_position)
|
|
# pass
|
|
# #fprint("Not triggering LED loop: no ring animation")
|
|
|
|
if mode == "Startup":
|
|
if not real or skip_scanning:
|
|
counter = 54
|
|
|
|
if counter < 54:
|
|
# scanning cables
|
|
if arm_state is None:
|
|
#pool.apply_async(arm_start_callback, ("",))
|
|
arm_ready = False
|
|
pool.apply_async(ur5_control.holder_to_camera, (arm,arm_updates,counter), callback=arm_start_callback, error_callback=handle_error)
|
|
fprint("Getting cable index " + str(counter) + " and scanning...")
|
|
arm_state = "GET"
|
|
ring_animation = counter
|
|
animation_wait = True
|
|
start_animation = True
|
|
led_set_mode = "GrabA"
|
|
#ur5_control.to_camera(arm, counter)
|
|
#arm_ready = True
|
|
|
|
elif arm_ready and arm_state == "GET":
|
|
fprint("Looking for QR code...")
|
|
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
|
|
arm_ready = False
|
|
|
|
elif camera_ready:
|
|
ring_animation = counter
|
|
animation_wait = True
|
|
start_animation = True
|
|
led_set_mode = "GrabC"
|
|
fprint("Adding cable to list...")
|
|
global scan_value
|
|
if scan_value is False:
|
|
cable_list.append(scan_value)
|
|
elif scan_value.upper().find("BLDN.APP/") > -1:
|
|
scan_value = scan_value[scan_value.upper().find("BLDN.APP/")+9:]
|
|
else:
|
|
cable_list.append(scan_value)
|
|
fprint(scan_value)
|
|
pool.apply_async(ur5_control.camera_to_holder, (arm,arm_updates,counter), callback=arm_start_callback, error_callback=handle_error)
|
|
#ur5_control.return_camera(arm, counter)
|
|
#arm_ready = True
|
|
arm_state = "RETURN"
|
|
camera_ready = False
|
|
|
|
elif arm_ready and arm_state == "RETURN":
|
|
counter += 1
|
|
arm_state = None
|
|
else:
|
|
# just wait til arm/camera is ready
|
|
pass
|
|
else:
|
|
# scanned everything
|
|
ring_animation = None
|
|
led_set_mode == "Idle"
|
|
start_animation = True
|
|
tmp = [
|
|
# Actual cables in Jukebox
|
|
"BLTF-1LF-006-RS5",
|
|
"BLTF-SD9-006-RI5",
|
|
"BLTT-SLG-024-HTN",
|
|
"BLFISX012W0",
|
|
"BLFI4X012W0",
|
|
"BLSPE101",
|
|
"BLSPE102",
|
|
"BL7922A",
|
|
"BL7958A",
|
|
"BLIOP6U",
|
|
"BL10GXW13",
|
|
"BL10GXW53",
|
|
"BL29501F",
|
|
"BL29512",
|
|
"BL3106A",
|
|
"BL9841",
|
|
"BL3105A",
|
|
"BL3092A",
|
|
"BL8760",
|
|
"BL6300UE",
|
|
"BL6300FE",
|
|
"BLRA500P",
|
|
"AW86104CY",
|
|
"AW3050",
|
|
"AW6714",
|
|
"AW1172C",
|
|
"AWFIT-221-1_4"
|
|
]
|
|
while len(tmp) < 54:
|
|
tmp.append(False) # must have 54 entries
|
|
|
|
if not real or skip_scanning:
|
|
cable_list = tmp # comment out for real demo
|
|
|
|
for idx in range(len(cable_list)):
|
|
cable_list_state.append(True)
|
|
|
|
pool.apply_async(get_specs.get_multi, (cable_list, 0.3, config["cables"]["directory"], config["cables"]["port"]), callback=cable_search_callback, error_callback=handle_error)
|
|
mode = "Parsing"
|
|
fprint("All cables scanned. Finding & parsing datasheets...")
|
|
if mode == "Parsing":
|
|
# waiting for search & parse to complete
|
|
#cable_search_ready = True
|
|
if cable_search_ready is False:
|
|
pass
|
|
else:
|
|
# done
|
|
global parse_res
|
|
success, partnums = parse_res
|
|
for idx in range(len(partnums)):
|
|
if partnums[idx] is not False:
|
|
cable_list[idx] = partnums[idx][0].replace("/", "_")
|
|
else:
|
|
cable_list[idx] = False
|
|
|
|
print(partnums)
|
|
if success:
|
|
# easy mode
|
|
fprint("All cables inventoried and parsed.")
|
|
fprint("Adding to database...")
|
|
for idx in range(len(cable_list)):
|
|
partnum = cable_list[idx]
|
|
if partnum is not False:
|
|
with open("cables/" + partnum + "/search.json", "rb") as f:
|
|
searchdata = json.load(f)
|
|
searchdata["position"] = idx
|
|
with open("cables/" + partnum + "/specs.json", "rb") as f:
|
|
specs = json.load(f)
|
|
searchdata["fullspecs"] = specs
|
|
searchdata["fullspecs"]["position"] = idx
|
|
jbs.add_document(searchdata)
|
|
#sleep(0.5)
|
|
#print(jbs.get_position("1"))
|
|
|
|
fprint("All cables added to database.")
|
|
mode = "Idle"
|
|
|
|
else:
|
|
# TODO: manual input
|
|
pass
|
|
|
|
|
|
|
|
|
|
if mode == "Pickup":
|
|
# complete
|
|
if arm_ready == True:
|
|
mode = "Idle"
|
|
if not real:
|
|
sleep(9)
|
|
#global sensors
|
|
if placed > 5:
|
|
# success
|
|
mqtt_send("{\"value\": " + str(1) + " }", "pick_count_success")
|
|
mqtt_send("{\"value\": " + str(int(time.time() * 1000) - cycle_start_time) + " }", "cycle_time")
|
|
else:
|
|
# getting cable and bringing to tray
|
|
# led animation
|
|
if get_spot(sensors, spot):
|
|
placed += 1
|
|
if ledsys.mode == "Idle" and led_set_mode != "GrabAA":
|
|
animation_wait = True
|
|
start_animation = True
|
|
led_set_mode = "GrabAB"
|
|
pass
|
|
|
|
if mode == "ReturnC":
|
|
# complete
|
|
if arm_ready == True:
|
|
mode = "Scan"
|
|
arm_ready = False
|
|
camera_ready = False
|
|
if real:
|
|
animation_wait = False
|
|
start_animation = True
|
|
ring_animation = 49
|
|
led_set_mode = "Camera"
|
|
timecount = 0
|
|
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
|
|
else:
|
|
camera_ready = True
|
|
scan_value = "10GXS13"
|
|
|
|
else:
|
|
# getting cable from and bringing to camera
|
|
# led animation
|
|
pass
|
|
|
|
if mode == "Scan":
|
|
if camera_ready == True:
|
|
if scan_value is False:
|
|
# unable to scan ???? not good
|
|
if failcount > 30:
|
|
mode = "Idle"
|
|
fprint("Giving up scanning cable.")
|
|
failcount = 0
|
|
timecount = 0
|
|
arm_ready = True
|
|
else:
|
|
fprint("Unable to scan cable. Gonna retry.")
|
|
camera_ready = False
|
|
#mode = "Idle"
|
|
failcount += 1
|
|
timecount = 0
|
|
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
|
|
|
|
elif scan_value.upper().find("BLDN.APP/") > -1:
|
|
scan_value = scan_value[scan_value.upper().find("BLDN.APP/")+9:].strip("\r\n\t ")
|
|
|
|
|
|
if scan_value[0:2] == "BL" or scan_value[0:2] == "AW":
|
|
scan_value = scan_value[2:]
|
|
#print(cable_list)
|
|
fprint("Got cable: " + repr(scan_value))
|
|
|
|
for idx in range(len(cable_list)):
|
|
cable = cable_list[idx]
|
|
|
|
if cable is not False and cable.find(scan_value) == 0 and len(scan_value) == len(cable):
|
|
if cable_list_state[idx] == False:
|
|
cable_list_state[idx] = True # mark cable as returned
|
|
arm_ready = False
|
|
if real:
|
|
animation_wait = True
|
|
ring_animation = idx
|
|
led_set_mode = "GrabC"
|
|
start_animation = True
|
|
fprint("Returning to spot " + str(idx))
|
|
pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
|
|
else:
|
|
arm_ready = True
|
|
mode = "Return"
|
|
break
|
|
else:
|
|
print(cable_list)
|
|
print("Length:", len(cable_list))
|
|
for idx in range(len(cable_list)-1,-1,-1):
|
|
cable = cable_list[idx]
|
|
if cable is False:
|
|
if real:
|
|
animation_wait = True
|
|
ring_animation = idx
|
|
led_set_mode = "GrabC"
|
|
start_animation = True
|
|
cable_list_state[idx] = True
|
|
fprint("Returning to unused spot " + str(idx))
|
|
pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
|
|
else:
|
|
arm_ready = True
|
|
|
|
|
|
if mode == "Scan":
|
|
fprint("Unable to match cable.")
|
|
mode = "Idle"
|
|
else: # camera not ready
|
|
timecount += 1
|
|
if timecount > 180:
|
|
print("Camera timeout reached.")
|
|
timecount = 0
|
|
camera_ready = True
|
|
arm_ready = True
|
|
mode = "Idle"
|
|
|
|
|
|
if mode == "Return":
|
|
if arm_ready == True:
|
|
mode = "Idle"
|
|
#arm_ready = False
|
|
# movement finished
|
|
|
|
else:
|
|
# cable going from camera to holder
|
|
# led animation
|
|
pass
|
|
|
|
if mode == "IdleWait":
|
|
if arm_ready is True:
|
|
mode = "Idle"
|
|
|
|
if mode == "Idle":
|
|
# do nothing
|
|
if arm_ready is False:
|
|
mode = "IdleWait"
|
|
|
|
else:
|
|
global mainloop_get
|
|
|
|
|
|
if not mainloop_get.empty():
|
|
|
|
action, get_cable = mainloop_get.get()
|
|
if action == "show":
|
|
animation_wait = False
|
|
ring_animation = get_cable
|
|
start_animation = True
|
|
led_set_mode = "Show"
|
|
fprint("Showing cable at position " + str(get_cable))
|
|
elif action == "shutdown":
|
|
fprint("SHUTTING DOWN!!")
|
|
pool.apply_async(ur5_control.move_to_packup, (arm,), callback=arm_start_callback, error_callback=handle_error)
|
|
sleep(30)
|
|
killme.set(1)
|
|
elif action == "returnCheck":
|
|
print("Checking sensors..")
|
|
if real:
|
|
newtube = get_full_spot(sensors)
|
|
else:
|
|
newtube = -1
|
|
if newtube is not False and newtube >= 0:
|
|
# need to return a cable
|
|
mainloop_get.put(("return", newtube))
|
|
mainloop_get.put(("returnCheck", 0))
|
|
else:
|
|
fprint("Movement requested. Keep clear of the machine!")
|
|
|
|
placed = 0
|
|
arm_distance_total = 0
|
|
#mqtt_send("{\"value\": " + str(time.time() * 1000) + " }", "cycle_start")
|
|
cycle_start_time = int(time.time() * 1000)
|
|
increment_counter()
|
|
mqtt_send("{\"value\": " + str(1) + " }", "pick_count_total")
|
|
if get_cable > -1:
|
|
|
|
|
|
if action == "pickup":
|
|
spot = get_open_spot(sensors)
|
|
|
|
if spot is not False:
|
|
arm_ready = False
|
|
if real:
|
|
animation_wait = False
|
|
ring_animation = get_cable
|
|
start_animation = True
|
|
led_set_mode = "GrabAA"
|
|
pool.apply_async(ur5_control.holder_to_tray, (arm, arm_updates, get_cable, spot), callback=arm_start_callback, error_callback=handle_error)
|
|
else:
|
|
arm_ready = True
|
|
fprint("Getting cable at position " + str(get_cable))
|
|
mode = "Pickup"
|
|
cable_list_state[get_cable] = False # mark as removed
|
|
|
|
|
|
if action == "return":
|
|
arm_ready = False
|
|
fprint("Returning cable from tray position " + str(get_cable))
|
|
if real:
|
|
failcount = 0
|
|
pool.apply_async(ur5_control.tray_to_camera, (arm, arm_updates, get_cable), callback=arm_start_callback, error_callback=handle_error)
|
|
else:
|
|
arm_ready = True
|
|
mode = "ReturnC"
|
|
else:
|
|
# LED idle anim
|
|
pass
|
|
|
|
|
|
def ping(host):
|
|
#Returns True if host (str) responds to a ping request.
|
|
|
|
# Option for the number of packets as a function of
|
|
if win32:
|
|
param1 = '-n'
|
|
param2 = '-w'
|
|
param3 = '250'
|
|
else:
|
|
param1 = '-c'
|
|
param2 = '-W'
|
|
param3 = '0.25'
|
|
|
|
# Building the command. Ex: "ping -c 1 google.com"
|
|
command = ['ping', param1, '1', param2, param3, host]
|
|
|
|
return subprocess.call(command, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT) == 0
|
|
|
|
def run_loading_app():
|
|
|
|
app = Flask(__name__)
|
|
@app.route('/')
|
|
def index():
|
|
return render_template('index.html')
|
|
|
|
app.run(debug=True, use_reloader=False, port=7000)
|
|
|
|
def setup_client(pool):
|
|
global config
|
|
global vm_ready
|
|
global serverproc
|
|
if config["core"]["server"] == "Hyper-V":
|
|
run_cmd("Restart-VM -Name Jukebox*") # any and all VMs starting with "Jukebox"
|
|
sleep(2)
|
|
fprint("Waiting for VM to start...")
|
|
while not ping("192.168.1.25"):
|
|
sleep(0.25)
|
|
|
|
fprint("VM online.")
|
|
sleep(2)
|
|
# Windows client setup
|
|
fprint("Running full jukebox control system...")
|
|
jb = subprocess.Popen("ssh root@192.168.1.25 -t -- /root/jukebox-software/run.sh".split(' '), stdout=subprocess.PIPE, stderr=subprocess.STDOUT, bufsize=1, universal_newlines=True, encoding='utf-8')
|
|
while True:
|
|
if jb.poll() is not None:
|
|
break
|
|
line = jb.stdout.readline() # Alternatively proc.stdout.read(1024)
|
|
if len(line) == 0: # program end
|
|
break
|
|
print(line, end='')
|
|
if line.find("Running mode: Idle") > 0:
|
|
# Jukebox started
|
|
break # continue with program
|
|
|
|
if jb.poll() is None:
|
|
|
|
fprint("Opening browser...")
|
|
|
|
firefox = webdriver.Firefox()
|
|
firefox.fullscreen_window()
|
|
|
|
|
|
# firefox.get('http://localhost:7000')
|
|
|
|
|
|
firefox.get('http://192.168.1.25:3000')
|
|
global kill_ssh
|
|
while True:
|
|
if jb.poll() is not None:
|
|
break
|
|
line = jb.stdout.readline() # Alternatively proc.stdout.read(1024)
|
|
if len(line) == 0: # program end
|
|
break
|
|
print(line, end='')
|
|
if kill_ssh is True:
|
|
break
|
|
|
|
|
|
firefox.close()
|
|
|
|
jb.terminate()
|
|
run_cmd("Stop-VM -Name Jukebox*")
|
|
sleep(2)
|
|
killall()
|
|
|
|
#firefox.execute_script('document.body.style.MozTransform = "scale(0.80)";')
|
|
#firefox.execute_script('document.body.style.MozTransformOrigin = "0 0";')
|
|
#firefox.execute_script("document.body.style.zoom='80%'")
|
|
# import time
|
|
# while True:
|
|
# time.sleep(2) # Wait for a given interval
|
|
# logs = firefox.get_log('browser')
|
|
# for entry in logs:
|
|
# if "WebSocket connection" in entry['message'] or "ERR_" in entry['message'] or "Failed to connect" in entry['message'] or "failed to connect" in entry['message']:
|
|
# print(f"Error detected in console: {entry['message']}")
|
|
# firefox.refresh() # Refresh the page on error
|
|
# # else:
|
|
# # break # Exit the loop or continue depending on your logic
|
|
|
|
#return True
|
|
|
|
|
|
def mainloop_client(pool):
|
|
sleep(0.1)
|
|
killall()
|
|
# listen for & act on commands from VM, if needed
|
|
# mainly just shut down, possibly connect to wifi or something
|
|
|
|
"""class Logger(object):
|
|
def __init__(self, filename="output.log"):
|
|
self.log = open(filename, "a")
|
|
self.terminal = sys.stdout
|
|
|
|
def write(self, message):
|
|
self.log.write(message)
|
|
#close(filename)
|
|
#self.log = open(filename, "a")
|
|
try:
|
|
self.terminal.write(message)
|
|
except:
|
|
sleep(0)
|
|
|
|
def flush(self):
|
|
print("",end="")"""
|
|
|
|
def killall():
|
|
procs = active_children()
|
|
for proc in procs:
|
|
proc.kill()
|
|
fprint("All child processes killed")
|
|
os.kill(os.getpid(), 9) # dirty kill of self
|
|
|
|
|
|
def killall_signal(a, b):
|
|
global config
|
|
#if config["core"]["server"] == "Hyper-V":
|
|
#run_cmd("Stop-VM -Name Jukebox*") # any and all VMs starting with "Jukebox"
|
|
if config["core"]["mode"] == "winclient":
|
|
global kill_ssh
|
|
kill_ssh = True
|
|
else:
|
|
killall()
|
|
|
|
def error(msg, *args):
|
|
return multiprocessing.get_logger().error(msg, *args)
|
|
|
|
class LogExceptions(object):
|
|
def __init__(self, callable):
|
|
self.__callable = callable
|
|
|
|
def __call__(self, *args, **kwargs):
|
|
try:
|
|
result = self.__callable(*args, **kwargs)
|
|
|
|
except Exception as e:
|
|
# Here we add some debugging help. If multiprocessing's
|
|
# debugging is on, it will arrange to log the traceback
|
|
error(traceback.format_exc())
|
|
# Re-raise the original exception so the Pool worker can
|
|
# clean up
|
|
raise
|
|
|
|
# It was fine, give a normal answer
|
|
return result
|
|
|
|
class LoggingPool(Pool):
|
|
def apply_async(self, func, args=(), kwds={}, callback=None, error_callback=None):
|
|
return Pool.apply_async(self, LogExceptions(func), args, kwds, callback, error_callback)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
#sys.stdout = Logger(filename="output.log")
|
|
#sys.stderr = Logger(filename="output.log")
|
|
#log_to_stderr(logging.DEBUG)
|
|
|
|
fprint("Starting Jukebox control system...")
|
|
with open('config.yml', 'r') as fileread:
|
|
#global config
|
|
config = yaml.safe_load(fileread)
|
|
fprint("Config loaded.")
|
|
with Manager() as manager:
|
|
fprint("Spawning threads...")
|
|
pool = LoggingPool(processes=10)
|
|
counter = 0
|
|
killme = manager.Value('d', 0)
|
|
signal.signal(signal.SIGINT, killall_signal)
|
|
if config["core"]["mode"] == "winclient":
|
|
fprint("Starting in client mode.")
|
|
from selenium import webdriver
|
|
if setup_client(pool):
|
|
fprint("Entering main loop...")
|
|
while(keeprunning):
|
|
mainloop_client(pool)
|
|
|
|
elif config["core"]["mode"] == "linuxserver":
|
|
fprint("Starting in server mode.")
|
|
if setup_server(pool, manager):
|
|
fprint("Entering main loop...")
|
|
start = 0
|
|
speed = config["core"]["loopspeed"]
|
|
while(keeprunning):
|
|
start = uptime()
|
|
mainloop_server(pool, manager)
|
|
#sleep(0.01)
|
|
# limit to certain "framerate"
|
|
#print(start, start + 1.0/speed, uptime())
|
|
while start + 1.0/speed > uptime():
|
|
sleep(0.001)
|
|
else:
|
|
fprint("Mode unspecified - quitting")
|
|
|
|
|
|
|
|
|