mettabridge.py

import sys
sys.path.append('./plugins/')
sys.path.append('./channels/')
from copy import deepcopy
from exploration import *
from narsplugin import narsplugin_init, call_nars, call_nars_tuple
from guiplugin import guiplugin_init, call_gui
from hyperon.ext import register_atoms
from hyperon import *
import time
import io

NAV_STATE_FAIL = "FAIL"
NAV_STATE_BUSY = "BUSY"
NAV_STATE_SUCCESS = "SUCCESS"
NAV_STATE = NAV_STATE_SUCCESS #"busy", "success" and "fail" (initially set to success)
def NAV_STATE_SET(NAV_STATE_ARG):
    global NAV_STATE
    NAV_STATE = NAV_STATE_ARG
def NAV_STATE_GET():
    global NAV_STATE
    return NAV_STATE
def ARM_STATE_SET(ARM_STATE_ARG):
    global ARM_STATE
    ARM_STATE = ARM_STATE_ARG
def ARM_STATE_GET():
    global ARM_STATE
    return ARM_STATE

class PatternOperation(OperationObject):
    def __init__(self, name, op, unwrap=False, rec=False):
        super().__init__(name, op, unwrap)
        self.rec = rec
    def execute(self, *args, res_typ=AtomType.UNDEFINED):
        return super().execute(*args, res_typ=res_typ)

def wrapnpop(func):
    def wrapper(*args):
        a = [str("'"+arg) if arg is SymbolAtom else str(arg) for arg in args]
        res = func(*a)
        return [res]
    return wrapper

MeTTaROS2Command = ""
def call_ros(*a):
    global runner, MeTTaROS2Command
    tokenizer = runner.tokenizer()
    cmd = str(a[0])
    parser = SExprParser(f"(nartech.ros.command {cmd})")
    MeTTaROS2Command = cmd
    return parser.parse(tokenizer)

objects = "()"
def call_ros_objects(*a):
    global runner, objects
    tokenizer = runner.tokenizer()
    parser = SExprParser(objects)
    return parser.parse(tokenizer)

def call_ros_arm(*a):
    global runner, ARM_STATE
    tokenizer = runner.tokenizer()
    parser = SExprParser(ARM_STATE)
    return parser.parse(tokenizer)

def call_ros_navigation(*a):
    global runner, NAV_STATE
    tokenizer = runner.tokenizer()
    parser = SExprParser(NAV_STATE)
    return parser.parse(tokenizer)

def space_init():
    global runner
    metta_code = None
    for arg in sys.argv:
        if arg.endswith(".metta"):
            with open(arg, "r") as f:
                metta_code = f.read()
    runner = MeTTa()
    runner.run("""
(= (nartech.ros.objects.filter $category $objects)
   (collapse (let* (($obj (superpose $objects))
                    ((detection $category (coordinates $x $y)) $obj))
                   $obj)))
    """)
    runner.register_atom("nartech.ros", G(PatternOperation('nartech.ros', wrapnpop(call_ros), unwrap=False)))
    runner.register_atom("nartech.ros.arm", G(PatternOperation('nartech.ros.arm', wrapnpop(call_ros_arm), unwrap=False)))
    runner.register_atom("nartech.ros.navigation", G(PatternOperation('nartech.ros.navigation', wrapnpop(call_ros_navigation), unwrap=False)))
    runner.register_atom("nartech.ros.objects", G(PatternOperation('nartech.ros.objects', wrapnpop(call_ros_objects), unwrap=False)))
    runner.register_atom("nartech.nars", G(PatternOperation('nartech.nars', wrapnpop(call_nars), unwrap=False)))
    runner.register_atom("nartech.nars.tuple", G(PatternOperation('nartech.nars.tuple', wrapnpop(call_nars_tuple), unwrap=False)))
    runner.register_atom("nartech.gui", G(PatternOperation('nartech.gui', wrapnpop(call_gui), unwrap=False)))
    narsplugin_init(runner)
    guiplugin_init(runner)
    if metta_code is not None:
        result = runner.run(metta_code)
        for x in result:
            print(x)  # Only prints the return value

currentTime = 0
start_time = time.time()

def space_tick(node = None):
    global currentTime, MeTTaROS2Command, runner, objects
    elapsedTime = round(time.time() - start_time, 2)
    if elapsedTime < 10 and node is not None:
        return
    cmd = MeTTaROS2Command
    MeTTaROS2Command = ""
    objects = "()"
    if node:
        if cmd == "right":
            node.start_navigation_by_moves("right")
        if cmd == "left":
            node.start_navigation_by_moves("left")
        if cmd == "up":
            node.start_navigation_by_moves("up")
        if cmd == "down":
            node.start_navigation_by_moves("down")
        if cmd == "drop":
            node.drop()
        if cmd.startswith("(go (coordinates "):
            x_y = cmd.split("(go (coordinates ")[1].split(")")[0]
            x = int(x_y.split(" ")[0])
            y = int(x_y.split(" ")[1])
            potential_label = cmd.split("(go (coordinates ")[1].split(")")[1].split(")")[0].strip()
            node.start_navigation_to_coordinate((x, y), potential_label)
        if cmd.startswith("(pick "):
            label = cmd.split("(pick ")[1].split(")")[0]
            node.pick(label)
        alldetections = deepcopy(node.semantic_slam.previous_detections)
        objects = "("
        if "{SELF}" in node.semantic_slam.previous_detections:
            (t, object_grid_x, object_grid_y, origin_x, origin_y, point_map, point_base_link, imagecoords_depth) = node.semantic_slam.previous_detections["{SELF}"]
            x_y_unknown = BFS_for_nearest_unknown_cell(node.semantic_slam.low_res_grid, node.semantic_slam.new_width, node.semantic_slam.new_height, object_grid_x, object_grid_y)
            if x_y_unknown:
                (x_unknown,y_unknown) =  x_y_unknown
                alldetections["unknown"] = (time.time(), x_unknown, y_unknown, origin_x, origin_y, None, None, None)
        print(alldetections)
        for category in alldetections:
            (t, object_grid_x, object_grid_y, origin_x, origin_y, point_map, point_base_link, imagecoords_depth) = alldetections[category]
            SEXP = f"(detection {category} (coordinates {object_grid_x} {object_grid_y}))"
            objects += SEXP
            if category == "{SELF}":
                SELF_position = (object_grid_x, object_grid_y)
        objects += ")"
    currentTime += 1
    print("STEP", str(currentTime) +":", runner.run(f"!(Step {currentTime} {elapsedTime})"))

space_init()
if __name__ == "__main__":
    while True:
        space_tick()
        time.sleep(0.01)