Here is the current code for the robot arm (robot_agent.py) and the actuator (sem_actuator_agent.py). They are primarily AI generated to fix blocking but I'm still running into the same issue.

I couldn't find the thread but you wanted me to switch over to mosquitto, right?

# robot_agent.py running on the actual arm

import json, time, os, sys, traceback, threading, queue
import paho.mqtt.client as mqtt
from pymycobot.mycobot import MyCobot

# -------------------- MQTT --------------------
BROKER = os.getenv("MQTT_HOST", "INSERT IP")  # SEM_IP
PORT   = int(os.getenv("MQTT_PORT", "1883"))
USER   = os.getenv("MQTT_USER", "labuser")
PASS   = os.getenv("MQTT_PASS", "INSERT PASSWORD")

TOP_CMD    = "sem/robot/cmd"
TOP_STATUS = "sem/robot/status"

# -------------------- Robot I/O --------------------
SERIAL_PORT = os.getenv("MC_PORT", "/dev/ttyAMA0")
BAUD        = int(os.getenv("MC_BAUD", "1000000")) 

# -------------------- Speed Profile --------------------
JNT_SPEED  = int(os.getenv("JNT_SPEED",  "55"))
XYZ_SPEED  = int(os.getenv("XYZ_SPEED",  "65"))
GRIP_SPEED = int(os.getenv("GRIP_SPEED", "80"))

# -------------------- Waypoints --------------------
HOME = [0, 0, 0, 0, 0, 0]

PICK_ABOVE = [254.3,  29.8, 180.3,  62.37,  51.25, 163.06]
PICK_DOWN  = [259.0,  36.7, 148.9,  65.13,  45.78, 167.40]

SEM_ABOVE  = [-115.1, -114.2, 284.8, -141.24, -65.73, -91.65]
SEM_DOWN   = [-115.0, -113.9, 256.4, -135.05, -63.33, -97.65]

EVAC_ABOVE = [-162.6, -199.3, 217.7,  138.95,  79.59, -20.58]
EVAC_PRESS = [-192.9, -172.1, 218.9,   98.51,  -0.86, -72.85]
EVAC_PRESS_MS = 150

# -------------------- Gripper semantics --------------------
# set_gripper_state(flag, speed): 0=open, 1=close (swap if inverted)
GRIP_OPEN_FLAG  = int(os.getenv("GRIP_OPEN_FLAG", "0"))
GRIP_CLOSE_FLAG = int(os.getenv("GRIP_CLOSE_FLAG", "1"))
GRIP_SETTLE_S   = float(os.getenv("GRIP_SETTLE_S", "0.15"))

# -------------------- Globals --------------------
_cmd_q: "queue.Queue[dict]" = queue.Queue(maxsize=8)
_stop = threading.Event()

def log(*a): print(*a, flush=True)

def publish_status(c: mqtt.Client, state: str, **extra):
    msg = {"state": state, "ts": time.time()}
    msg.update(extra)
    c.publish(TOP_STATUS, json.dumps(msg), qos=1)
    log("PUB", msg)

# -------------------- Motion helpers --------------------
def open_gripper(mc: MyCobot):
    mc.set_gripper_state(GRIP_OPEN_FLAG, GRIP_SPEED)
    time.sleep(GRIP_SETTLE_S)

def close_gripper(mc: MyCobot):
    mc.set_gripper_state(GRIP_CLOSE_FLAG, GRIP_SPEED)
    time.sleep(GRIP_SETTLE_S)

def go_angles(mc: MyCobot, angles, speed=JNT_SPEED, timeout=7.0):
    try:
        mc.sync_send_angles(angles, int(speed), float(timeout))
    except Exception:
        mc.send_angles(angles, int(speed))
        time.sleep(0.8)

def go_coords(mc: MyCobot, coords, speed=XYZ_SPEED, mode=0, timeout=7.0):
    try:
        mc.sync_send_coords(coords, int(speed), int(mode), float(timeout))
    except Exception:
        mc.send_coords(coords, int(speed), int(mode))
        time.sleep(0.8)

# -------------------- Tasks --------------------
def home(mc: MyCobot):
    go_angles(mc, HOME, JNT_SPEED)

def grab_sample(mc: MyCobot, mqttc: mqtt.Client):
    publish_status(mqttc, "BUSY", op="GRAB")
    go_coords(mc, PICK_ABOVE, XYZ_SPEED)
    open_gripper(mc)
    go_coords(mc, PICK_DOWN, XYZ_SPEED)
    close_gripper(mc)
    go_coords(mc, PICK_ABOVE, XYZ_SPEED)
    publish_status(mqttc, "DONE", op="GRAB")

def drop_to_sem(mc: MyCobot, mqttc: mqtt.Client):
    publish_status(mqttc, "BUSY", op="DROP_TO_SEM")
    go_coords(mc, SEM_ABOVE, XYZ_SPEED)
    go_coords(mc, SEM_DOWN,  XYZ_SPEED)
    open_gripper(mc)   # release
    go_coords(mc, SEM_ABOVE, XYZ_SPEED)
    publish_status(mqttc, "DONE", op="DROP_TO_SEM")

def press_evac(mc: MyCobot, mqttc: mqtt.Client):
    publish_status(mqttc, "BUSY", op="PRESS_EVAC")
    open_gripper(mc)
    go_coords(mc, EVAC_ABOVE, XYZ_SPEED)
    go_coords(mc, EVAC_PRESS, XYZ_SPEED)
    time.sleep(EVAC_PRESS_MS / 1000.0)
    go_coords(mc, EVAC_ABOVE, XYZ_SPEED)
    publish_status(mqttc, "DONE", op="PRESS_EVAC")

def full_load_sequence(mc: MyCobot, mqttc: mqtt.Client):
    publish_status(mqttc, "BUSY", op="LOAD")
    home(mc)
    grab_sample(mc, mqttc)     # actuator should OPEN on DONE/GRAB
    drop_to_sem(mc, mqttc)     # actuator should CLOSE on DONE/DROP_TO_SEM
    press_evac(mc, mqttc)
    home(mc)
    publish_status(mqttc, "DONE", op="LOAD")

# -------------------- Worker --------------------
def worker(mc: MyCobot, mqttc: mqtt.Client):
    while not _stop.is_set():
        try:
            cmd = _cmd_q.get(timeout=0.2)
        except queue.Empty:
            continue
        try:
            op = (cmd.get("cmd") or cmd.get("op") or "").upper()
            if   op == "LOAD":          full_load_sequence(mc, mqttc)
            elif op == "HOME":          publish_status(mqttc, "BUSY", op=op); home(mc); publish_status(mqttc, "DONE", op=op)
            elif op == "GRAB":          grab_sample(mc, mqttc)
            elif op == "DROP_TO_SEM":   drop_to_sem(mc, mqttc)
            elif op == "PRESS_EVAC":    press_evac(mc, mqttc)
            else:
                publish_status(mqttc, "ERROR", error=f"unknown cmd {op}")
        except Exception as e:
            traceback.print_exc()
            publish_status(mqttc, "ERROR", error=str(e))
        finally:
            _cmd_q.task_done()

# -------------------- MQTT callbacks (non-blocking) --------------------
def on_message(c, u, msg):
    try:
        payload = json.loads(msg.payload.decode())
    except Exception:
        payload = {}
    op = (payload.get("cmd") or payload.get("op") or "").upper()
    if not op:
        publish_status(c, "ERROR", error="empty cmd"); return
    # Enqueue only; do NOT move inside callback
    try:
        _cmd_q.put_nowait({"cmd": op})
        log("ENQ", op)
    except queue.Full:
        publish_status(c, "ERROR", error="cmd queue full")

# -------------------- Main --------------------
def main():
    try:
        mc = MyCobot(SERIAL_PORT, BAUD)
    except Exception as e:
        log("Could not open serial:", e); sys.exit(1)

    try:
        mc.set_fresh_mode(0)  # 0=queue/in-order
    except Exception:
        pass

    # Newer paho v2 API (optional; silences deprecation)
    from paho.mqtt.client import Client, CallbackAPIVersion, MQTTv311
    c = Client(client_id="robot-agent", protocol=MQTTv311,
               callback_api_version=CallbackAPIVersion.VERSION2)
    c.username_pw_set(USER, PASS)
    c.user_data_set({"mc": mc})
    c.on_message = on_message
    c.will_set(TOP_STATUS, json.dumps({"state": "DEAD", "ts": time.time()}), qos=1, retain=False)

    log(f"Connect MQTT {BROKER}:{PORT} as {USER}")
    c.connect(BROKER, PORT, keepalive=60)
    c.subscribe(TOP_CMD, qos=1)
    publish_status(c, "READY")

    # Spin up network and worker threads
    c.loop_start()
    t = threading.Thread(target=worker, args=(mc, c), daemon=True)
    t.start()

    # Optional initial home + gripper warm-up in the worker queue
    _cmd_q.put({"cmd": "HOME"})

    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        pass
    finally:
        _stop.set()
        c.loop_stop()
        try: t.join(timeout=1.0)
        except: pass

if __name__ == "__main__":
    main()

# sem_actuator_agent.py running on sem-brain (rpi)

import json, os, time, sys, signal, traceback, queue, threading
import paho.mqtt.client as mqtt
import RPi.GPIO as GPIO

# ----- CONFIG -----
BROKER = os.getenv("MQTT_HOST", "INSERT IP") 
PORT   = int(os.getenv("MQTT_PORT", "1883"))
USER   = os.getenv("MQTT_USER", "labuser")
PASS   = os.getenv("MQTT_PASS", "INSERT PASSWORD")

TOP_CMD    = "sem/actuator/cmd"
TOP_STATUS = "sem/actuator/status"
TOP_ROBOT  = "sem/robot/status"

GPIO.setmode(GPIO.BCM)
PWM_PIN        = int(os.getenv("ACT_PWM_GPIO", "18"))
PWM_HZ         = int(os.getenv("ACT_PWM_HZ",  "50"))
PULSE_OPEN_US  = int(os.getenv("ACT_PULSE_OPEN_US",  "2000"))
PULSE_CLOSE_US = int(os.getenv("ACT_PULSE_CLOSE_US", "1000"))
NEUTRAL_US     = int(os.getenv("ACT_PULSE_NEUTRAL_US","1500"))
MOVE_TIMEOUT_S = float(os.getenv("ACT_MOVE_TIMEOUT_S", "5.0"))
POST_CLOSE_PAUSE_S = float(os.getenv("ACT_POST_CLOSE_PAUSE_S", "0.5"))
HOLD_MODE      = os.getenv("ACT_HOLD_MODE", "off").lower()  # off|open|close|neutral

OPEN_SW_PIN  = int(os.getenv("ACT_OPEN_SW",  "-1"))
CLOSE_SW_PIN = int(os.getenv("ACT_CLOSE_SW", "-1"))

DEBUG = True

# ----- STATE -----
_pwm = None
state = "UNKNOWN"
action_q: "queue.Queue[str]" = queue.Queue(maxsize=8)
stop_ev = threading.Event()
lock = threading.Lock()  # protects 'state'

def dbg(*a):
    if DEBUG:
        print("[ACT]", *a, flush=True)

def publish(c: mqtt.Client, s: str, **extra):
    global state
    with lock:
        state = s
    msg = {"state": s, "ts": time.time()}
    msg.update(extra)
    c.publish(TOP_STATUS, json.dumps(msg), qos=1, retain=False)
    dbg("PUB", msg)

def _us_to_duty(pulse_us: int) -> float:
    period_us = 1_000_000.0 / PWM_HZ
    duty = 100.0 * (pulse_us / period_us)
    return max(0.0, min(100.0, duty))

def _start_pwm():
    global _pwm
    GPIO.setup(PWM_PIN, GPIO.OUT)
    _pwm = GPIO.PWM(PWM_PIN, PWM_HZ)
    _pwm.start(0.0)

def _set_pulse_us(pulse_us: int):
    _pwm.ChangeFrequency(PWM_HZ)
    _pwm.ChangeDutyCycle(_us_to_duty(pulse_us))

def _stop_pulses():
    _pwm.ChangeDutyCycle(0.0)

def _setup_endstops():
    if OPEN_SW_PIN >= 0:
        GPIO.setup(OPEN_SW_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    if CLOSE_SW_PIN >= 0:
        GPIO.setup(CLOSE_SW_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)

def _reached_open() -> bool:
    return OPEN_SW_PIN >= 0 and GPIO.input(OPEN_SW_PIN) == GPIO.LOW

def _reached_close() -> bool:
    return CLOSE_SW_PIN >= 0 and GPIO.input(CLOSE_SW_PIN) == GPIO.LOW

# ----- ACTIONS (blocking but run in worker thread) -----
def do_open(c):
    publish(c, "OPENING", reason="cmd")
    _set_pulse_us(PULSE_OPEN_US)
    t0 = time.time()
    while time.time() - t0 < MOVE_TIMEOUT_S and not stop_ev.is_set():
        if _reached_open():
            break
        time.sleep(0.02)
    if   HOLD_MODE == "open":    _set_pulse_us(PULSE_OPEN_US)
    elif HOLD_MODE == "neutral": _set_pulse_us(NEUTRAL_US)
    else:                        _stop_pulses()
    publish(c, "OPEN")

def do_close(c):
    publish(c, "CLOSING", reason="cmd")
    _set_pulse_us(PULSE_CLOSE_US)
    t0 = time.time()
    while time.time() - t0 < MOVE_TIMEOUT_S and not stop_ev.is_set():
        if _reached_close():
            break
        time.sleep(0.02)
    if   HOLD_MODE == "close":   _set_pulse_us(PULSE_CLOSE_US)
    elif HOLD_MODE == "neutral": _set_pulse_us(NEUTRAL_US)
    else:                        _stop_pulses()
    publish(c, "CLOSED")
    if POST_CLOSE_PAUSE_S > 0:
        time.sleep(POST_CLOSE_PAUSE_S)

def do_stop(c):
    _stop_pulses()
    if HOLD_MODE == "neutral":
        _set_pulse_us(NEUTRAL_US)
    publish(c, "STOPPED")

# ----- WORKER LOOP -----
def worker(c: mqtt.Client):
    while not stop_ev.is_set():
        try:
            action = action_q.get(timeout=0.2)
        except queue.Empty:
            continue
        try:
            if action == "OPEN":
                do_open(c)
            elif action == "CLOSE":
                do_close(c)
            elif action == "STOP":
                do_stop(c)
        except Exception:
            traceback.print_exc()
            publish(c, "ERROR", error=f"action {action} failed")
        finally:
            action_q.task_done()

# ----- MQTT CALLBACKS (non-blocking) -----
def enqueue(action: str):
    # Deduplicate: if already in desired terminal state, ignore
    with lock:
        cur = state
    if action == "OPEN" and cur == "OPEN":
        return
    if action == "CLOSE" and cur == "CLOSED":
        return
    # Drop if queue is full to avoid backlog storm
    try:
        action_q.put_nowait(action)
        dbg("ENQ", action)
    except queue.Full:
        dbg("WARN: action queue full; dropping", action)

def handle_robot_status(c, payload):
    st = str(payload.get("state") or payload.get("STATE") or "").strip().upper()
    op = str(payload.get("op") or payload.get("OP") or "").strip().upper()
    dbg("RX robot:", {"state": st, "op": op})
    if st != "DONE":
        return
    if op == "GRAB":
        enqueue("OPEN")                # open as soon as GRAB completes
    elif op == "DROP_TO_SEM":
        enqueue("CLOSE")               # close as soon as DROP completes
    else:
        # Ignore LOAD etc. so it remains closed for the rest.
        pass

def on_message(c, u, msg):
    try:
        data = json.loads(msg.payload.decode())
    except Exception:
        data = {}
    dbg("RX", msg.topic, data)
    if msg.topic == TOP_ROBOT:
        handle_robot_status(c, data); return

    cmd = (data.get("cmd") or "").upper()
    if cmd == "OPEN":
        enqueue("OPEN")
    elif cmd == "CLOSE":
        enqueue("CLOSE")
    elif cmd == "STOP":
        enqueue("STOP")
    elif cmd == "STATUS":
        with lock: s = state
        publish(c, s or "UNKNOWN")
    else:
        publish(c, "ERROR", error=f"unknown cmd: {cmd}")

def main():
    def cleanup_and_exit(_sig=None, _frm=None):
        stop_ev.set()
        try:
            _stop_pulses()
        except Exception:
            pass
        try:
            GPIO.cleanup()
        except Exception:
            pass
        sys.exit(0)

    signal.signal(signal.SIGINT,  cleanup_and_exit)
    signal.signal(signal.SIGTERM, cleanup_and_exit)

    _start_pwm()
    _setup_endstops()

    from paho.mqtt.client import Client, CallbackAPIVersion, MQTTv311
    c = Client(client_id="sem-actuator-agent-gpio",
               protocol=MQTTv311,
               callback_api_version=CallbackAPIVersion.VERSION2)
    c.username_pw_set(USER, PASS)
    c.on_message = on_message
    c.will_set(TOP_STATUS, json.dumps({"state":"DEAD","ts":time.time()}), qos=1, retain=False)

    dbg(f"Connecting MQTT {BROKER}:{PORT} as {USER}")
    c.connect(BROKER, PORT, keepalive=60)
    c.subscribe([(TOP_CMD,1),(TOP_ROBOT,1)])
    publish(c, "READY")

    # Start worker and MQTT network loop (non-blocking)
    t = threading.Thread(target=worker, args=(c,), daemon=True)
    t.start()
    # Close at boot so we're in a known state
    enqueue("CLOSE")
    c.loop_start()

    # Keep main thread alive
    try:
        while not stop_ev.is_set():
            time.sleep(1.0)
    finally:
        c.loop_stop()

if __name__ == "__main__":
    try:
        main()
    except Exception:
        traceback.print_exc()
        os._exit(1)

Programming Homing, Sample Pickup, Sample Dropoff, and Button Press Path on MyCobot 280 Pi for SEM Automation #484

Description

Metadata

Metadata

Assignees

Labels

Type

Projects

Milestone

Relationships

Development

Issue actions