docker-compose.yml

@@ -1,36 +0,0 @@
-version: "3.9"
-services:
-  controller:
-    build: .
-    ports:
-      - "8888:8888"
-    environment:
-      - MASTER_IP=10.6.6.8
-      - SLAVE_IP=10.6.6.8
-      #BME lab Configuration
-      # Fred
-      #MASTER_IP=10.6.6.12
-      # Erik
-      #SLAVE_IP=10.6.6.10
-      # Master: RG2 Gripper
-      - MASTER_GRIPPER_WIDTH_OPENED=50
-      - MASTER_GRIPPER_WIDTH_CLOSED=28
-      # Slave: RG2FT Gripper
-      - SLAVE_GRIPPER_IP=10.6.6.11
-      - SLAVE_GRIPPER_WIDTH_OPENED=90
-      - SLAVE_GRIPPER_WIDTH_CLOSED=28
-      - MASTER_ENABLE=0
-      - SLAVE_ENABLE=0
-      - JOGGING_ENABLE=1
-      - EXECUTION=SLOW
-      #EXECUTION=FAST
-      #Used with moveL
-      - L_SPEED_SLOW=0.05
-      - L_SPEED_FAST=0.1
-      - L_ACC_SLOW=0.75
-      - L_ACC_FAST=0.75
-      #Used with moveJ
-      - J_SPEED_SLOW=1
-      - J_SPEED_FAST=1
-      - J_ACC_SLOW=4
-      - J_ACC_FAST=4

dual_ursim_control/config.py

@@ -1,30 +0,0 @@
-import os
-
-class Config:
-    MASTER_IP = os.environ.get("MASTER_IP")
-    SLAVE_IP = os.environ.get("SLAVE_IP")
-
-    MASTER_GRIPPER_WIDTH_OPENED = int(os.environ.get("MASTER_GRIPPER_WIDTH_OPENED"))
-    MASTER_GRIPPER_WIDTH_CLOSED = int(os.environ.get("MASTER_GRIPPER_WIDTH_CLOSED"))
-
-    SLAVE_GRIPPER_IP = os.environ.get("SLAVE_GRIPPER_IP")
-    SLAVE_GRIPPER_WIDTH_OPENED = int(os.environ.get("SLAVE_GRIPPER_WIDTH_OPENED"))
-    SLAVE_GRIPPER_WIDTH_CLOSED = int(os.environ.get("SLAVE_GRIPPER_WIDTH_CLOSED"))
-
-    MASTER_ENABLE = os.environ.get("MASTER_ENABLE", 'False').lower() in ('true', '1', 't')
-    SLAVE_ENABLE = os.environ.get("SLAVE_ENABLE", 'False').lower() in ('true', '1', 't')
-
-    JOGGING_ENABLE = os.environ.get("JOGGING_ENABLE", 'False').lower() in ('true', '1', 't')
-
-    # SLOW / FAST
-    EXECUTION = os.environ.get("EXECUTION")
-
-    L_SPEED_SLOW = float(os.environ.get("L_SPEED_SLOW"))
-    L_SPEED_FAST = float(os.environ.get("L_SPEED_FAST"))
-    L_ACC_SLOW = float(os.environ.get("L_ACC_SLOW"))
-    L_ACC_FAST = float(os.environ.get("L_ACC_FAST"))
-
-    J_SPEED_SLOW = float(os.environ.get("J_SPEED_SLOW"))
-    J_SPEED_FAST = float(os.environ.get("J_SPEED_FAST"))
-    J_ACC_SLOW = float(os.environ.get("J_ACC_SLOW"))
-    J_ACC_FAST = float(os.environ.get("J_ACC_FAST"))

dual_ursim_control/main.py

@@ -15,12 +15,15 @@ import logging
 import logging.config
 from xmlrpc.server import SimpleXMLRPCServer
 from configparser import ConfigParser
-from config import Config
 
 # from pynput.keyboard import Listener, Key
 
 SCRIPT_DIR = os.path.dirname(__file__)
 
+config_object = ConfigParser()
+config_object.read(os.path.join(SCRIPT_DIR, 'config.ini'))
+config = config_object['DEFAULT']
+
 # TODO: export these to json?
 # Config file name
 # CONFIG_FILE = 'path.xlsx'
@@ -255,18 +258,54 @@ def read_path_waypoints_from_file():
     return path_master, path_slave
 
 
+# def on_press(key):
+#     try:
+#         # logging.debug("Key pressed: {0}".format(key))
+#
+#         if hasattr(key, 'char'):
+#             if key.char == 'f':
+#                 jogging_wait['Master'] = True
+#
+#             elif key.char == 'e':
+#                 jogging_wait['Slave'] = True
+#
+#     except AttributeError:
+#         logging.debug("Special key pressed: {0}".format(key))
+#
+#
+# def on_release(key):
+#     try:
+#         # logging.debug("Key released: {0}".format(key))
+#
+#         if hasattr(key, 'char'):
+#             if key.char == 'f':
+#                 jogging_wait['Master'] = False
+#
+#             elif key.char == 'e':
+#                 jogging_wait['Slave'] = False
+#
+#     except AttributeError:
+#         logging.debug("Special key pressed: {0}".format(key))
+
+
 def master_jogging_wait():
-    if Config.JOGGING_ENABLE:
+    if config.getboolean('JOGGING_ENABLE'):
         logging.debug("Master-Fred: Press 'f' to continue")
 
+        # while not jogging_wait['Master']:
+        #     pass
+        # jogging_wait['Master'] = False
         jogging_event['Master'].clear()
         jogging_event['Master'].wait()
 
 
 def slave_jogging_wait():
-    if Config.JOGGING_ENABLE:
+    if config.getboolean('JOGGING_ENABLE'):
         logging.debug("Slave-Erik: Press 'e' to continue")
 
+        # while not jogging_wait['Slave']:
+        #     pass
+        # jogging_wait['Slave'] = False
         jogging_event['Slave'].clear()
         jogging_event['Slave'].wait()
 
@@ -279,7 +318,7 @@ def master_operate_gripper(target_width):
         return
 
     port = 30001
-    host = Config.MASTER_IP
+    host = config['MASTER_IP']
     tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
     tcp_socket.connect((host, port))
 
@@ -447,10 +486,10 @@ def master_gripper_toggle(state='Open'):
     # This function accepts either 'Open' or 'Close'
 
     if state == 'Open':
-        master_operate_gripper(Config.MASTER_GRIPPER_WIDTH_OPENED)
+        master_operate_gripper(config.getint('MASTER_GRIPPER_WIDTH_OPENED'))
 
     elif state == 'Close':
-        master_operate_gripper(Config.MASTER_GRIPPER_WIDTH_CLOSED)
+        master_operate_gripper(config.getint('MASTER_GRIPPER_WIDTH_CLOSED'))
 
     else:
         logging.info('Invalid gripper option given')
@@ -459,15 +498,15 @@ def master_gripper_toggle(state='Open'):
 
 def master_connect():
     try:
-        robot = rtde_control.RTDEControlInterface(Config.MASTER_IP)
+        robot = rtde_control.RTDEControlInterface(config['MASTER_IP'])
 
     except:
-        text = f"Cannot connect to robot at: {Config.MASTER_IP}"
+        text = f"Cannot connect to robot at: {config['MASTER_IP']}"
 
         logging.info(text)
         exit(text)
 
-    logging.info(f"Connected to robot at: {Config.MASTER_IP}")
+    logging.info(f"Connected to robot at: {config['MASTER_IP']}")
 
     # robot.setPayload(1, (0, 0, 0.01))
     return robot
@@ -475,15 +514,15 @@ def master_connect():
 
 def slave_connect():
     try:
-        robot = rtde_control.RTDEControlInterface(Config.SLAVE_IP)
+        robot = rtde_control.RTDEControlInterface(config['SLAVE_IP'])
 
     except:
-        text = f"Cannot connect to robot at: {Config.SLAVE_IP}"
+        text = f"Cannot connect to robot at: {config['SLAVE_IP']}"
 
         logging.info(text)
         exit(text)
 
-    logging.info(f"Connected to robot at: {Config.SLAVE_IP}")
+    logging.info(f"Connected to robot at: {config['SLAVE_IP']}")
 
     # robot.setPayload(1, (0, 0, 0.01))
     return robot
@@ -505,13 +544,13 @@ def slave_gripper_toggle(state='Open'):
     logging.info(f'Slave RG2-FT gripper state: {state} ')
     # This function accepts either 'Open' or 'Close'
 
-    ip = Config.SLAVE_GRIPPER_IP
+    ip = config['SLAVE_GRIPPER_IP']
 
     if state == 'Open':
-        width = Config.SLAVE_GRIPPER_WIDTH_OPENED
+        width = config.getint('SLAVE_GRIPPER_WIDTH_OPENED')
 
     elif state == 'Close':
-        width = Config.SLAVE_GRIPPER_WIDTH_CLOSED
+        width = config.getint('SLAVE_GRIPPER_WIDTH_CLOSED')
     else:
         # log error
         logging.info('Invalid gripper option given')
@@ -528,7 +567,7 @@ def master_movel_time_based(pose, a=1, v=0.06, t=2):
     message = f"movel(p{pose}, a={a}, v={v}, t={t}, r=0)\n"
     logging.info(f'Time-based movel: {message}')
     s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-    s.connect((Config.MASTER_IP, 30001))
+    s.connect((config['MASTER_IP'], 30001))
 
     s.send(str.encode(message))
     s.close()
@@ -540,7 +579,7 @@ def slave_movel_time_based(pose, a=1, v=0.06, t=2):
     message = f"movel(p{pose}, a={a}, v={v}, t={t}, r=0)\n"
     logging.info(f'Time-based movel: {message}')
     s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-    s.connect((Config.SLAVE_IP, 30001))
+    s.connect((config['SlAVE_IP'], 30001))
 
     s.send(str.encode(message))
     s.close()
@@ -548,6 +587,32 @@ def slave_movel_time_based(pose, a=1, v=0.06, t=2):
     return
 
 
+# def move_marcell_sync_begin():
+#
+#     message = "movel(p[0.116, -0.394, 0.0, 0.608, -1.48, 0.621], a=1, v=0.06, t=5, r=0)\n"
+#     logging.debug(message)
+#     s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+#     s.connect((config['MASTER_IP'], 30001))
+#
+#     s.send(str.encode(message))
+#     s.close()
+#     time.sleep(5)
+#     return
+#
+# def move_marcell_snync_end():
+#     message = "movel(p[0.066, -0.444, 0.0, 0.572, -1.49, 0.585], a=1, v=0.06, t=5, r=0)\n"
+#
+#     logging.debug(message)
+#     s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+#     s.connect((config['MASTER_IP'], 30001))
+#
+#     s.send(str.encode(message))
+#     s.close()
+#     time.sleep(5)
+#
+#     return
+
+
 def master_thread(sync_event, path):
     logging.info(f'Starting master')
 
@@ -593,7 +658,7 @@ def master_thread(sync_event, path):
     robot.moveJ(path['CM4_J'], j_speed, j_acc)
     logging.info(f'CM4: M Sync Begin')
 
-    if Config.SLAVE_ENABLE:
+    if config.getboolean('SLAVE_ENABLE'):
         sync_event['Master'].set()
         logging.info('Signaling to slave')
 
@@ -619,7 +684,7 @@ def master_thread(sync_event, path):
     # robot = master_connect()
     # return
 
-    if Config.SLAVE_ENABLE:
+    if config.getboolean('SLAVE_ENABLE'):
         logging.debug('Waiting for slave')
 
         sync_event['Slave'].wait()
@@ -688,7 +753,7 @@ def slave_thread(sync_event, path):
 
     time.sleep(0.5)
 
-    if Config.MASTER_ENABLE:
+    if config.getboolean('MASTER_ENABLE'):
         logging.debug('Waiting for master')
 
         sync_event['Master'].wait()
@@ -701,7 +766,7 @@ def slave_thread(sync_event, path):
     # robot = slave_connect()
     logging.info(f'CS5: S Sync End')
 
-    if Config.MASTER_ENABLE:
+    if config.getboolean('MASTER_ENABLE'):
         sync_event['Slave'].set()
         logging.debug('Signaling to master')
 
@@ -735,7 +800,7 @@ def slave_thread(sync_event, path):
     robot.moveJ(path['CS3_J'], j_speed, j_acc)
     logging.info(f'CS3: S Start Oriented')
 
-    if Config.MASTER_ENABLE:
+    if config.getboolean('MASTER_ENABLE'):
         sync_event['Slave'].set()
         logging.debug('Signaling to master')
 
@@ -788,27 +853,28 @@ class ServerThread(threading.Thread):
 
 
 if __name__ == '__main__':
+
     logging.config.fileConfig(os.path.join(SCRIPT_DIR, 'log_config.ini'))
 
-    if Config.EXECUTION == 'SLOW':
+    if config['EXECUTION'] == 'SLOW':
-        j_speed = Config.J_SPEED_SLOW
+        j_speed = config.getfloat('J_SPEED_SLOW')
-        l_speed = Config.L_SPEED_SLOW
+        l_speed = config.getfloat('L_SPEED_SLOW')
 
-        j_acc = Config.J_ACC_SLOW
+        j_acc = config.getfloat('J_ACC_SLOW')
-        l_acc = Config.L_ACC_SLOW
+        l_acc = config.getfloat('L_ACC_SLOW')
 
         logging.info("Execution speed: slow")
 
-    elif Config.EXECUTION == 'FAST':
+    elif config['EXECUTION'] == 'FAST':
-        j_speed = Config.J_SPEED_FAST
+        j_speed = config.getfloat('J_SPEED_FAST')
-        l_speed = Config.L_SPEED_FAST
+        l_speed = config.getfloat('L_SPEED_FAST')
 
-        j_acc = Config.J_ACC_FAST
+        j_acc = config.getfloat('J_ACC_FAST')
-        l_acc = Config.L_ACC_FAST
+        l_acc = config.getfloat('L_ACC_FAST')
 
         logging.info("Execution speed: fast")
 
-    if Config.JOGGING_ENABLE:
+    if config.getboolean('JOGGING_ENABLE'):
         logging.info('Jogging enabled')
 
     # TODO: eliminate excel
@@ -828,18 +894,25 @@ if __name__ == '__main__':
     xmlrpcServer = ServerThread()
     xmlrpcServer.start()
 
-    if Config.MASTER_ENABLE:
+    # listener = Listener(on_press=on_press, on_release=on_release)
+    # listener.start()
+
+    if config.getboolean('MASTER_ENABLE'):
         master_thread = threading.Thread(name='MasterThread', target=master_thread, args=(sync_event, path_master))
         master_thread.start()
 
-    if Config.SLAVE_ENABLE:
+    if config.getboolean('SLAVE_ENABLE'):
         slave_thread = threading.Thread(name='SlaveThread', target=slave_thread, args=(sync_event, path_slave))
         slave_thread.start()
 
-    if Config.MASTER_ENABLE:
+    if config.getboolean('MASTER_ENABLE'):
         master_thread.join()
 
-    if Config.SLAVE_ENABLE:
+    if config.getboolean('SLAVE_ENABLE'):
         slave_thread.join()
 
+    # listener.stop()
+    # listener.join()
     logging.info("Done")
+
+# TODO: export from ursimulator: root@xu:/var/snap/docker/common/var-lib-docker/volumes/dockursim/_data/programs.UR3#