From 98f41cfa28c7075e806844cfdd7da43860d94945 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 10:11:55 +0100 Subject: [PATCH 01/66] Asebamedulla inlcuded in python --- visualswarm/app.py | 3 ++- visualswarm/contrib/control.py | 4 ++-- visualswarm/control/motorinterface.py | 10 ++++++++++ 3 files changed, 14 insertions(+), 3 deletions(-) create mode 100644 visualswarm/control/motorinterface.py diff --git a/visualswarm/app.py b/visualswarm/app.py index 0491d389..bab8322a 100644 --- a/visualswarm/app.py +++ b/visualswarm/app.py @@ -12,7 +12,7 @@ from visualswarm.vision import vacquire, vprocess from visualswarm.contrib import logparams, vision from visualswarm.behavior import behavior -from visualswarm.control import motoroutput +from visualswarm.control import motorinterface, motoroutput import dbus.mainloop.glib @@ -155,4 +155,5 @@ def start_application(with_control=False): def start_application_with_control(): + motorinterface.asebamedulla_init() start_application(with_control=True) diff --git a/visualswarm/contrib/control.py b/visualswarm/contrib/control.py index d2aa45c2..36e51ca2 100644 --- a/visualswarm/contrib/control.py +++ b/visualswarm/contrib/control.py @@ -2,5 +2,5 @@ @author: mezdahun @description: Motor Control related parameters """ - -ENABLE_MOTOR_CONTROL = True +# Serial port on which the Thymio is available +THYMIO_DEVICE_PORT = "/dev/ttyACM0" diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py new file mode 100644 index 00000000..7a3e6c63 --- /dev/null +++ b/visualswarm/control/motorinterface.py @@ -0,0 +1,10 @@ +import subprocess +from visualswarm.contrib import control + +def asebamedulla_init(): + """Establishing initial connection with the Thymio robot on a predefined interface + Args: None + Vars: visualswarm.control.THYMIO_DEVICE_PORT: serial port on which the robot is available for the Pi + Returns: None + """ + subprocess.run(['asebamedulla', f'"ser:device={control.THYMIO_DEVICE_PORT}"'] \ No newline at end of file From 14eae77ed5581cf3345a8a0f61ff1432d71c4969 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 10:15:05 +0100 Subject: [PATCH 02/66] fix bug in motorinterface --- visualswarm/control/motorinterface.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 7a3e6c63..f6657816 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -1,10 +1,11 @@ import subprocess from visualswarm.contrib import control + def asebamedulla_init(): """Establishing initial connection with the Thymio robot on a predefined interface Args: None Vars: visualswarm.control.THYMIO_DEVICE_PORT: serial port on which the robot is available for the Pi Returns: None """ - subprocess.run(['asebamedulla', f'"ser:device={control.THYMIO_DEVICE_PORT}"'] \ No newline at end of file + subprocess.run(['asebamedulla', f'"ser:device={control.THYMIO_DEVICE_PORT}"']) From 6d3d3b797640b3ebe507d2f4c427fbf50ba98bac Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 10:15:54 +0100 Subject: [PATCH 03/66] changed asebamedulla syntax --- visualswarm/control/motorinterface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index f6657816..6f06c498 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -8,4 +8,4 @@ def asebamedulla_init(): Vars: visualswarm.control.THYMIO_DEVICE_PORT: serial port on which the robot is available for the Pi Returns: None """ - subprocess.run(['asebamedulla', f'"ser:device={control.THYMIO_DEVICE_PORT}"']) + subprocess.run(['asebamedulla', f'ser:device={control.THYMIO_DEVICE_PORT}']) From 216b59cc236842aac74809151b1dde6b7ee274c0 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 10:51:46 +0100 Subject: [PATCH 04/66] Added start, end, health for asebamedulla --- visualswarm/control/motorinterface.py | 29 +++++++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 6f06c498..79eef5d8 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -1,11 +1,36 @@ -import subprocess +import os +import dbus +import dbus.mainloop.glib from visualswarm.contrib import control +def asebamedulla_health(): + """Checking health of the established connection by requesting robot health""" + dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) + bus = dbus.SessionBus() + + # Create Aseba network + network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), + dbus_interface='ch.epfl.mobots.AsebaNetwork') + + # Check Thymio's health + is_robot_healthy = network.GetVariable("thymio-II", "prox.horizontal", reply_handler=lambda v: True, + error_handler=lambda e: False) + + return is_robot_healthy + + def asebamedulla_init(): """Establishing initial connection with the Thymio robot on a predefined interface Args: None Vars: visualswarm.control.THYMIO_DEVICE_PORT: serial port on which the robot is available for the Pi Returns: None """ - subprocess.run(['asebamedulla', f'ser:device={control.THYMIO_DEVICE_PORT}']) + os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &) && sleep 1") + if not asebamedulla_health(): + raise Exception('Connection can not be established with robot!') + + +def asebamedulla_end(): + """Killing all established asebamedulla processes""" + os.system("pkill -f asebamedulla") From b22e7325262c030c3605a32cfa312b81b9ae6df4 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 10:55:37 +0100 Subject: [PATCH 05/66] debug --- visualswarm/control/motorinterface.py | 1 + 1 file changed, 1 insertion(+) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 79eef5d8..2e7f5db0 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -17,6 +17,7 @@ def asebamedulla_health(): is_robot_healthy = network.GetVariable("thymio-II", "prox.horizontal", reply_handler=lambda v: True, error_handler=lambda e: False) + print(is_robot_healthy) return is_robot_healthy From 5a8a01ee5cd68959f48e484acf7336447a8b76f9 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:00:07 +0100 Subject: [PATCH 06/66] global error handling --- visualswarm/control/motorinterface.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 2e7f5db0..9690edc8 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -3,9 +3,22 @@ import dbus.mainloop.glib from visualswarm.contrib import control +is_robot_healthy = False + +def healthy_response(v): + global is_robot_healthy + is_robot_healthy = True + + +def unhealthy_response(e): + global is_robot_healthy + is_robot_healthy = False + def asebamedulla_health(): """Checking health of the established connection by requesting robot health""" + global is_robot_healthy + dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) bus = dbus.SessionBus() @@ -14,8 +27,8 @@ def asebamedulla_health(): dbus_interface='ch.epfl.mobots.AsebaNetwork') # Check Thymio's health - is_robot_healthy = network.GetVariable("thymio-II", "prox.horizontal", reply_handler=lambda v: True, - error_handler=lambda e: False) + network.GetVariable("thymio-II", "prox.horizontal", reply_handler=healthy_response, + error_handler=unhealthy_response) print(is_robot_healthy) return is_robot_healthy From 11577632252fde6395a7658ec92de29077d7b8d2 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:01:07 +0100 Subject: [PATCH 07/66] debug --- visualswarm/control/motorinterface.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 9690edc8..e3bffd1c 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -5,13 +5,16 @@ is_robot_healthy = False + def healthy_response(v): global is_robot_healthy + print("called healthy callback") is_robot_healthy = True def unhealthy_response(e): global is_robot_healthy + print("called unhealthy callback") is_robot_healthy = False From ed31f37ebabec605c6262549d30146e5502ae4e3 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:17:11 +0100 Subject: [PATCH 08/66] changed GetVariable syntax --- visualswarm/control/motorinterface.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index e3bffd1c..156cdcd9 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -30,11 +30,13 @@ def asebamedulla_health(): dbus_interface='ch.epfl.mobots.AsebaNetwork') # Check Thymio's health - network.GetVariable("thymio-II", "prox.horizontal", reply_handler=healthy_response, - error_handler=unhealthy_response) + test_var = None + test_var = network.GetVariable("thymio-II", "acc", timeout=5) - print(is_robot_healthy) - return is_robot_healthy + if test_var is not None: + return True + else: + return False def asebamedulla_init(): From cf5f12bd951d500df47c992eface1fd9605c5c71 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:19:14 +0100 Subject: [PATCH 09/66] adding some sleep --- visualswarm/control/motorinterface.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 156cdcd9..9f5b4f82 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -1,6 +1,7 @@ import os import dbus import dbus.mainloop.glib +from time import sleep from visualswarm.contrib import control is_robot_healthy = False @@ -45,9 +46,12 @@ def asebamedulla_init(): Vars: visualswarm.control.THYMIO_DEVICE_PORT: serial port on which the robot is available for the Pi Returns: None """ - os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &) && sleep 1") + os.system(f"asebamedulla ser:device={control.THYMIO_DEVICE_PORT}") + sleep(5) if not asebamedulla_health(): raise Exception('Connection can not be established with robot!') + else: + print("Connection via asebamedulla is healthy!") def asebamedulla_end(): From 6c00372da0cc093b8855aa421b2be1cdce8b17ce Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:23:01 +0100 Subject: [PATCH 10/66] slightly changed implementation --- visualswarm/control/motorinterface.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 9f5b4f82..10ca9e9c 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -21,18 +21,22 @@ def unhealthy_response(e): def asebamedulla_health(): """Checking health of the established connection by requesting robot health""" - global is_robot_healthy - + # init the dbus main loop dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) + + # get stub of the aseba network bus = dbus.SessionBus() + asebaNetworkObject = bus.get_object('ch.epfl.mobots.Aseba', '/') - # Create Aseba network - network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), - dbus_interface='ch.epfl.mobots.AsebaNetwork') + # prepare interface + asebaNetwork = dbus.Interface( + asebaNetworkObject, + dbus_interface='ch.epfl.mobots.AsebaNetwork' + ) # Check Thymio's health test_var = None - test_var = network.GetVariable("thymio-II", "acc", timeout=5) + test_var = asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) if test_var is not None: return True @@ -48,6 +52,7 @@ def asebamedulla_init(): """ os.system(f"asebamedulla ser:device={control.THYMIO_DEVICE_PORT}") sleep(5) + print('checking for health') if not asebamedulla_health(): raise Exception('Connection can not be established with robot!') else: From ff4ac2a86daac76eb873642b7e97a4de2d27d12d Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:28:54 +0100 Subject: [PATCH 11/66] changed blocking process --- visualswarm/control/motorinterface.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 10ca9e9c..81e15285 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -1,7 +1,7 @@ import os import dbus import dbus.mainloop.glib -from time import sleep +import time from visualswarm.contrib import control is_robot_healthy = False @@ -50,8 +50,8 @@ def asebamedulla_init(): Vars: visualswarm.control.THYMIO_DEVICE_PORT: serial port on which the robot is available for the Pi Returns: None """ - os.system(f"asebamedulla ser:device={control.THYMIO_DEVICE_PORT}") - sleep(5) + os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") + time.sleep(2.5) print('checking for health') if not asebamedulla_health(): raise Exception('Connection can not be established with robot!') From 92a2993b59dc16839fc847875e40a0aac6d06f9b Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:29:55 +0100 Subject: [PATCH 12/66] increase sleep --- visualswarm/control/motorinterface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 81e15285..079f1d0c 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -51,7 +51,7 @@ def asebamedulla_init(): Returns: None """ os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") - time.sleep(2.5) + time.sleep(4.5) print('checking for health') if not asebamedulla_health(): raise Exception('Connection can not be established with robot!') From ec7f74d10c0bb53efbb7facba7fc452fb1650c1e Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:31:59 +0100 Subject: [PATCH 13/66] handling DBusException --- visualswarm/control/motorinterface.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 079f1d0c..90f16b98 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -1,6 +1,7 @@ import os import dbus import dbus.mainloop.glib +from dbus.exceptions import DBusException import time from visualswarm.contrib import control @@ -36,7 +37,10 @@ def asebamedulla_health(): # Check Thymio's health test_var = None - test_var = asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) + try: + test_var = asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) + except DBusException: + return False if test_var is not None: return True @@ -51,7 +55,7 @@ def asebamedulla_init(): Returns: None """ os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") - time.sleep(4.5) + time.sleep(5) print('checking for health') if not asebamedulla_health(): raise Exception('Connection can not be established with robot!') From 51d3c9fa34a7c8c65654f70353e3988e40b25215 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:34:59 +0100 Subject: [PATCH 14/66] handling os,system exception --- visualswarm/control/motorinterface.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 90f16b98..0065a169 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -39,13 +39,10 @@ def asebamedulla_health(): test_var = None try: test_var = asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) + return True except DBusException: return False - if test_var is not None: - return True - else: - return False def asebamedulla_init(): @@ -54,7 +51,8 @@ def asebamedulla_init(): Vars: visualswarm.control.THYMIO_DEVICE_PORT: serial port on which the robot is available for the Pi Returns: None """ - os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") + info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") + print(info) time.sleep(5) print('checking for health') if not asebamedulla_health(): From 11f1e5d0d5050debb96e05f05d00467ea704a68e Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:38:57 +0100 Subject: [PATCH 15/66] remove unused var --- visualswarm/control/motorinterface.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 0065a169..5861bcc7 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -36,15 +36,13 @@ def asebamedulla_health(): ) # Check Thymio's health - test_var = None try: - test_var = asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) + asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) return True except DBusException: return False - def asebamedulla_init(): """Establishing initial connection with the Thymio robot on a predefined interface Args: None @@ -52,7 +50,6 @@ def asebamedulla_init(): Returns: None """ info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") - print(info) time.sleep(5) print('checking for health') if not asebamedulla_health(): From 4284f7e3470ff2f5bc6e36fb50ce2053a8db826b Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:39:49 +0100 Subject: [PATCH 16/66] remove unused var --- visualswarm/control/motorinterface.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 5861bcc7..8e20ad03 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -37,7 +37,8 @@ def asebamedulla_health(): # Check Thymio's health try: - asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) + test_var = asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) + print(test_var) return True except DBusException: return False From ce37c3e46931faa2c72d5becd92c7f901fb30b02 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 11:41:40 +0100 Subject: [PATCH 17/66] remove unused var --- visualswarm/control/motorinterface.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 8e20ad03..5861bcc7 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -37,8 +37,7 @@ def asebamedulla_health(): # Check Thymio's health try: - test_var = asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) - print(test_var) + asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) return True except DBusException: return False From a448e6d812bbd02da387954e0d41a84f21bb6c05 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:01:03 +0100 Subject: [PATCH 18/66] Added logging --- visualswarm/control/motorinterface.py | 52 +++++++++++++-------------- 1 file changed, 24 insertions(+), 28 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 5861bcc7..fd1d2b19 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -1,42 +1,35 @@ import os import dbus import dbus.mainloop.glib +import logging + from dbus.exceptions import DBusException import time -from visualswarm.contrib import control - -is_robot_healthy = False - - -def healthy_response(v): - global is_robot_healthy - print("called healthy callback") - is_robot_healthy = True - +from visualswarm.contrib import control, logparams -def unhealthy_response(e): - global is_robot_healthy - print("called unhealthy callback") - is_robot_healthy = False +# using main logger +logger = logging.getLogger('visualswarm.app') +bcolors = logparams.BColors def asebamedulla_health(): """Checking health of the established connection by requesting robot health""" - # init the dbus main loop - dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) + logger.info(f'{bcolors.OKBLUE}HEALTHCHECK{bcolors.ENDC} asebamedulla connection') + # Check Thymio's health + try: + # init the dbus main loop + dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) - # get stub of the aseba network - bus = dbus.SessionBus() - asebaNetworkObject = bus.get_object('ch.epfl.mobots.Aseba', '/') + # get stub of the aseba network + bus = dbus.SessionBus() + asebaNetworkObject = bus.get_object('ch.epfl.mobots.Aseba', '/') - # prepare interface - asebaNetwork = dbus.Interface( - asebaNetworkObject, - dbus_interface='ch.epfl.mobots.AsebaNetwork' - ) + # prepare interface + asebaNetwork = dbus.Interface( + asebaNetworkObject, + dbus_interface='ch.epfl.mobots.AsebaNetwork' + ) - # Check Thymio's health - try: asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) return True except DBusException: @@ -49,15 +42,18 @@ def asebamedulla_init(): Vars: visualswarm.control.THYMIO_DEVICE_PORT: serial port on which the robot is available for the Pi Returns: None """ + logger.info(f'{bcolors.OKBLUE}CONNECT{bcolors.ENDC} via asebamedulla on {control.THYMIO_DEVICE_PORT}') info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") time.sleep(5) - print('checking for health') if not asebamedulla_health(): + logger.error(f'{bcolors.FAIL}CONNECTION FAILED{bcolors.ENDC} via asebamedulla') raise Exception('Connection can not be established with robot!') else: - print("Connection via asebamedulla is healthy!") + logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') def asebamedulla_end(): """Killing all established asebamedulla processes""" + logger.info(f'{bcolors.OKBLUE}CLOSE{bcolors.ENDC} connection via asebamedulla') os.system("pkill -f asebamedulla") + logger.info(f'{bcolors.OKGREEN}✓ CLOSE CONNECTION SUCCESSFUL{bcolors.ENDC}via asebamedulla') From d02f8f780b9b22ffaf84f8085e10f1844750da62 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:04:08 +0100 Subject: [PATCH 19/66] Added logging --- visualswarm/control/motorinterface.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index fd1d2b19..b148de8e 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -46,8 +46,9 @@ def asebamedulla_init(): info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") time.sleep(5) if not asebamedulla_health(): - logger.error(f'{bcolors.FAIL}CONNECTION FAILED{bcolors.ENDC} via asebamedulla') - raise Exception('Connection can not be established with robot!') + logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') + asebamedulla_end() + raise Exception('Connection could not be established with robot!') else: logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') From 568bc3d682060b2ce25481d4b70004e3fdd7547c Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:05:46 +0100 Subject: [PATCH 20/66] Added asebamedulla to app --- visualswarm/app.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/visualswarm/app.py b/visualswarm/app.py index bab8322a..26282154 100644 --- a/visualswarm/app.py +++ b/visualswarm/app.py @@ -39,6 +39,10 @@ def start_application(with_control=False): logger.info(f'{bcolors.OKGREEN}START vision stream{bcolors.ENDC} ') + # connect to Thymio + if with_control: + motorinterface.asebamedulla_init() + # Creating Queues raw_vision_stream = Queue() high_level_vision_stream = Queue() @@ -150,6 +154,7 @@ def start_application(with_control=False): dbus_interface='ch.epfl.mobots.AsebaNetwork') network.SetVariable("thymio-II", "motor.left.target", [0]) network.SetVariable("thymio-II", "motor.right.target", [0]) + motorinterface.asebamedulla_end() logger.info(f'{bcolors.OKGREEN}EXITED Gracefully. Bye bye!{bcolors.ENDC}') From df414d17929bd80f52f6a6973346b1d4bf2bd164 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:22:07 +0100 Subject: [PATCH 21/66] remove ENABLE_MOTOR_CONTROL --- tests/test_behavior.py | 44 ++++++++++++++---------------- visualswarm/app.py | 19 +++++-------- visualswarm/behavior/behavior.py | 5 ++-- visualswarm/control/motoroutput.py | 35 ++++++++++++++---------- 4 files changed, 51 insertions(+), 52 deletions(-) diff --git a/tests/test_behavior.py b/tests/test_behavior.py index 7a0dc61e..d78274c1 100644 --- a/tests/test_behavior.py +++ b/tests/test_behavior.py @@ -34,32 +34,30 @@ def test_VPF_to_behavior(self): control_stream = mock.MagicMock() control_stream.put.return_value = None - with mock.patch('visualswarm.contrib.control.ENABLE_MOTOR_CONTROL', False): - # Case 1: no save control params - with mock.patch('visualswarm.contrib.monitoring.SAVE_CONTROL_PARAMS', False): - behavior.VPF_to_behavior(VPF_stream, control_stream) - fake_create_client.assert_called_once() - fake_control_params.assert_called_once() + # Case 1: no save control params + with mock.patch('visualswarm.contrib.monitoring.SAVE_CONTROL_PARAMS', False): + behavior.VPF_to_behavior(VPF_stream, control_stream, False) + fake_create_client.assert_called_once() + fake_control_params.assert_called_once() - # resetting mocks - fake_create_client.reset_mock() - fake_control_params.reset_mock() + # resetting mocks + fake_create_client.reset_mock() + fake_control_params.reset_mock() - # Mocking calculations - fake_ifclient = mock.MagicMock() - fake_ifclient.write_points.return_value = None - fake_create_client.return_value = fake_ifclient + # Mocking calculations + fake_ifclient = mock.MagicMock() + fake_ifclient.write_points.return_value = None + fake_create_client.return_value = fake_ifclient - fake_control_params.return_value = (1, 1) + fake_control_params.return_value = (1, 1) - # Case 2: save control params to ifdb - with mock.patch('visualswarm.contrib.monitoring.SAVE_CONTROL_PARAMS', True): - behavior.VPF_to_behavior(VPF_stream, control_stream) - fake_create_client.assert_called_once() - fake_control_params.assert_called_once() - fake_ifclient.write_points.assert_called_once() + # Case 2: save control params to ifdb + with mock.patch('visualswarm.contrib.monitoring.SAVE_CONTROL_PARAMS', True): + behavior.VPF_to_behavior(VPF_stream, control_stream, False) + fake_create_client.assert_called_once() + fake_control_params.assert_called_once() + fake_ifclient.write_points.assert_called_once() # Case 3: motor output turned off - with mock.patch('visualswarm.contrib.control.ENABLE_MOTOR_CONTROL', True): - behavior.VPF_to_behavior(VPF_stream, control_stream) - control_stream.put.assert_called_once() + behavior.VPF_to_behavior(VPF_stream, control_stream, True) + control_stream.put.assert_called_once() diff --git a/visualswarm/app.py b/visualswarm/app.py index 26282154..d5edb593 100644 --- a/visualswarm/app.py +++ b/visualswarm/app.py @@ -74,9 +74,8 @@ def start_application(with_control=False): visualswarm.contrib.vision.NUM_SEGMENTATION_PROCS)] visualizer = Process(target=vprocess.visualizer, args=(visualization_stream, target_config_stream,)) VPF_extractor = Process(target=vprocess.VPF_extraction, args=(high_level_vision_stream, VPF_stream,)) - behavior_proc = Process(target=behavior.VPF_to_behavior, args=(VPF_stream, control_stream,)) - if with_control: - motor_control = Process(target=motoroutput.control_thymio, args=(control_stream,)) + behavior_proc = Process(target=behavior.VPF_to_behavior, args=(VPF_stream, control_stream, with_control)) + motor_control = Process(target=motoroutput.control_thymio, args=(control_stream, with_control)) system_monitor_proc = Process(target=system_monitor.system_monitor) try: @@ -89,8 +88,7 @@ def start_application(with_control=False): visualizer.start() VPF_extractor.start() behavior_proc.start() - if with_control: - motor_control.start() + motor_control.start() system_monitor_proc.start() # Wait for subprocesses in main process to terminate @@ -100,8 +98,7 @@ def start_application(with_control=False): raw_vision.join() VPF_extractor.join() behavior_proc.join() - if with_control: - motor_control.join() + motor_control.join() system_monitor_proc.join() except KeyboardInterrupt: @@ -111,10 +108,9 @@ def start_application(with_control=False): system_monitor_proc.terminate() system_monitor_proc.join() logger.info(f'{bcolors.WARNING}TERMINATED{bcolors.ENDC} system monitor process and joined!') - if with_control: - motor_control.terminate() - motor_control.join() - logger.info(f'{bcolors.WARNING}TERMINATED{bcolors.ENDC} motor control process and joined!') + motor_control.terminate() + motor_control.join() + logger.info(f'{bcolors.WARNING}TERMINATED{bcolors.ENDC} motor control process and joined!') behavior_proc.terminate() behavior_proc.join() logger.info(f'{bcolors.WARNING}TERMINATED{bcolors.ENDC} control parameter calculations!') @@ -160,5 +156,4 @@ def start_application(with_control=False): def start_application_with_control(): - motorinterface.asebamedulla_init() start_application(with_control=True) diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index e2ba84e3..583e3c6c 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -17,7 +17,7 @@ logger = logging.getLogger('visualswarm.app') -def VPF_to_behavior(VPF_stream, control_stream): +def VPF_to_behavior(VPF_stream, control_stream, with_control=False): """ Process to extract final visual projection field from high level visual input. Args: @@ -25,6 +25,7 @@ def VPF_to_behavior(VPF_stream, control_stream): control_stream (multiprocessing Queue): stream to push calculated control parameters Returns: -shall not return- + :param with_control: """ measurement_name = "control_parameters" ifclient = ifdb.create_ifclient() @@ -64,7 +65,7 @@ def VPF_to_behavior(VPF_stream, control_stream): ifclient.write_points(body, time_precision='ms') - if control.ENABLE_MOTOR_CONTROL: + if with_control: control_stream.put((v, dpsi)) # To test infinite loops diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index d9457fa5..eda49a95 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -36,21 +36,26 @@ def test_motor_control(network): return True -def control_thymio(control_stream): - dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) - bus = dbus.SessionBus() - - # Create Aseba network - network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), - dbus_interface='ch.epfl.mobots.AsebaNetwork') - while True: - (v, psi) = control_stream.get() - - v_left = v * (1 + psi) / 2 * 100 - v_right = v * (1 - psi) / 2 * 100 - - network.SetVariable("thymio-II", "motor.left.target", [v_left]) - network.SetVariable("thymio-II", "motor.right.target", [v_right]) +def control_thymio(control_stream, with_control=False): + if not with_control: + # simply consuming the input stream so that we don't fill up memory + while True: + (v, psi) = control_stream.get() + else: + dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) + bus = dbus.SessionBus() + + # Create Aseba network + network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), + dbus_interface='ch.epfl.mobots.AsebaNetwork') + while True: + (v, psi) = control_stream.get() + + v_left = v * (1 + psi) / 2 * 100 + v_right = v * (1 - psi) / 2 * 100 + + network.SetVariable("thymio-II", "motor.left.target", [v_left]) + network.SetVariable("thymio-II", "motor.right.target", [v_right]) def handle_GetVariable_reply(r): From 6f733e14bdcd1851e2a59ee76948d9750f6b5e0a Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:26:10 +0100 Subject: [PATCH 22/66] trying to resolve update issue --- visualswarm/control/motorinterface.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index b148de8e..2f73b247 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -45,12 +45,12 @@ def asebamedulla_init(): logger.info(f'{bcolors.OKBLUE}CONNECT{bcolors.ENDC} via asebamedulla on {control.THYMIO_DEVICE_PORT}') info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") time.sleep(5) - if not asebamedulla_health(): - logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') - asebamedulla_end() - raise Exception('Connection could not be established with robot!') - else: - logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') + # if not asebamedulla_health(): + # logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') + # asebamedulla_end() + # raise Exception('Connection could not be established with robot!') + # else: + # logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') def asebamedulla_end(): From 09f57b9ed1b6c7fb0887af1d4e452bc738c603c3 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:29:05 +0100 Subject: [PATCH 23/66] trying to resolve update issue --- visualswarm/app.py | 4 ++-- visualswarm/control/motorinterface.py | 14 ++++++++------ visualswarm/control/motoroutput.py | 14 +++++++------- 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/visualswarm/app.py b/visualswarm/app.py index d5edb593..b5e6c283 100644 --- a/visualswarm/app.py +++ b/visualswarm/app.py @@ -41,7 +41,7 @@ def start_application(with_control=False): # connect to Thymio if with_control: - motorinterface.asebamedulla_init() + network = motorinterface.asebamedulla_init() # Creating Queues raw_vision_stream = Queue() @@ -75,7 +75,7 @@ def start_application(with_control=False): visualizer = Process(target=vprocess.visualizer, args=(visualization_stream, target_config_stream,)) VPF_extractor = Process(target=vprocess.VPF_extraction, args=(high_level_vision_stream, VPF_stream,)) behavior_proc = Process(target=behavior.VPF_to_behavior, args=(VPF_stream, control_stream, with_control)) - motor_control = Process(target=motoroutput.control_thymio, args=(control_stream, with_control)) + motor_control = Process(target=motoroutput.control_thymio, args=(control_stream, with_control, network)) system_monitor_proc = Process(target=system_monitor.system_monitor) try: diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 2f73b247..854f30d0 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -45,12 +45,14 @@ def asebamedulla_init(): logger.info(f'{bcolors.OKBLUE}CONNECT{bcolors.ENDC} via asebamedulla on {control.THYMIO_DEVICE_PORT}') info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") time.sleep(5) - # if not asebamedulla_health(): - # logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') - # asebamedulla_end() - # raise Exception('Connection could not be established with robot!') - # else: - # logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') + network, connection_health = asebamedulla_health() + if not connection_health: + logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') + asebamedulla_end() + raise Exception('Connection could not be established with robot!') + else: + logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') + return network def asebamedulla_end(): diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index eda49a95..3875729b 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -36,18 +36,18 @@ def test_motor_control(network): return True -def control_thymio(control_stream, with_control=False): +def control_thymio(control_stream, with_control=False, network=None): if not with_control: # simply consuming the input stream so that we don't fill up memory while True: (v, psi) = control_stream.get() else: - dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) - bus = dbus.SessionBus() - - # Create Aseba network - network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), - dbus_interface='ch.epfl.mobots.AsebaNetwork') + # dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) + # bus = dbus.SessionBus() + # + # # Create Aseba network + # network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), + # dbus_interface='ch.epfl.mobots.AsebaNetwork') while True: (v, psi) = control_stream.get() From 43edf8b7e5991d18e5ccc05fefddad85525731f8 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:30:08 +0100 Subject: [PATCH 24/66] trying to resolve update issue --- visualswarm/control/motorinterface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 854f30d0..fccc720e 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -31,9 +31,9 @@ def asebamedulla_health(): ) asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) - return True + return asebaNetwork, True except DBusException: - return False + return None, False def asebamedulla_init(): From 4a8239c010b706c7a9445e4afc74c98ecac3ca4f Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:32:08 +0100 Subject: [PATCH 25/66] trying to resolve update issue --- visualswarm/control/motorinterface.py | 16 ++++++++-------- visualswarm/control/motoroutput.py | 14 +++++++------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index fccc720e..f47451df 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -45,14 +45,14 @@ def asebamedulla_init(): logger.info(f'{bcolors.OKBLUE}CONNECT{bcolors.ENDC} via asebamedulla on {control.THYMIO_DEVICE_PORT}') info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") time.sleep(5) - network, connection_health = asebamedulla_health() - if not connection_health: - logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') - asebamedulla_end() - raise Exception('Connection could not be established with robot!') - else: - logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') - return network + # network, connection_health = asebamedulla_health() + # if not connection_health: + # logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') + # asebamedulla_end() + # raise Exception('Connection could not be established with robot!') + # else: + # logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') + # return network def asebamedulla_end(): diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 3875729b..eda49a95 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -36,18 +36,18 @@ def test_motor_control(network): return True -def control_thymio(control_stream, with_control=False, network=None): +def control_thymio(control_stream, with_control=False): if not with_control: # simply consuming the input stream so that we don't fill up memory while True: (v, psi) = control_stream.get() else: - # dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) - # bus = dbus.SessionBus() - # - # # Create Aseba network - # network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), - # dbus_interface='ch.epfl.mobots.AsebaNetwork') + dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) + bus = dbus.SessionBus() + + # Create Aseba network + network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), + dbus_interface='ch.epfl.mobots.AsebaNetwork') while True: (v, psi) = control_stream.get() From b8fdcf04ee19fb717fa26745c500d65590ff96c1 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:33:03 +0100 Subject: [PATCH 26/66] trying to resolve update issue --- visualswarm/app.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/app.py b/visualswarm/app.py index b5e6c283..d5edb593 100644 --- a/visualswarm/app.py +++ b/visualswarm/app.py @@ -41,7 +41,7 @@ def start_application(with_control=False): # connect to Thymio if with_control: - network = motorinterface.asebamedulla_init() + motorinterface.asebamedulla_init() # Creating Queues raw_vision_stream = Queue() @@ -75,7 +75,7 @@ def start_application(with_control=False): visualizer = Process(target=vprocess.visualizer, args=(visualization_stream, target_config_stream,)) VPF_extractor = Process(target=vprocess.VPF_extraction, args=(high_level_vision_stream, VPF_stream,)) behavior_proc = Process(target=behavior.VPF_to_behavior, args=(VPF_stream, control_stream, with_control)) - motor_control = Process(target=motoroutput.control_thymio, args=(control_stream, with_control, network)) + motor_control = Process(target=motoroutput.control_thymio, args=(control_stream, with_control)) system_monitor_proc = Process(target=system_monitor.system_monitor) try: From c69625ea7fe9cfacc4887efbcc32be0e2a1eeecb Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:38:05 +0100 Subject: [PATCH 27/66] trying to resolve update issue --- visualswarm/control/motorinterface.py | 21 ++++----------------- visualswarm/control/motoroutput.py | 16 ++++++++++------ 2 files changed, 14 insertions(+), 23 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index f47451df..f028b744 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -12,28 +12,15 @@ bcolors = logparams.BColors -def asebamedulla_health(): +def asebamedulla_health(network): """Checking health of the established connection by requesting robot health""" logger.info(f'{bcolors.OKBLUE}HEALTHCHECK{bcolors.ENDC} asebamedulla connection') # Check Thymio's health try: - # init the dbus main loop - dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) - - # get stub of the aseba network - bus = dbus.SessionBus() - asebaNetworkObject = bus.get_object('ch.epfl.mobots.Aseba', '/') - - # prepare interface - asebaNetwork = dbus.Interface( - asebaNetworkObject, - dbus_interface='ch.epfl.mobots.AsebaNetwork' - ) - - asebaNetwork.GetVariable("thymio-II", "acc", timeout=5) - return asebaNetwork, True + network.GetVariable("thymio-II", "acc", timeout=5) + return True except DBusException: - return None, False + return False def asebamedulla_init(): diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index eda49a95..a0783981 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -2,6 +2,7 @@ import dbus.mainloop.glib from gi.repository import GLib +from visualswarm.control import motorinterface import tempfile import random @@ -48,14 +49,17 @@ def control_thymio(control_stream, with_control=False): # Create Aseba network network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), dbus_interface='ch.epfl.mobots.AsebaNetwork') - while True: - (v, psi) = control_stream.get() + if motorinterface.asebamedulla_health(network): + while True: + (v, psi) = control_stream.get() - v_left = v * (1 + psi) / 2 * 100 - v_right = v * (1 - psi) / 2 * 100 + v_left = v * (1 + psi) / 2 * 100 + v_right = v * (1 - psi) / 2 * 100 - network.SetVariable("thymio-II", "motor.left.target", [v_left]) - network.SetVariable("thymio-II", "motor.right.target", [v_right]) + network.SetVariable("thymio-II", "motor.left.target", [v_left]) + network.SetVariable("thymio-II", "motor.right.target", [v_right]) + else: + raise Exception('asebamedulla connection not healthy!') def handle_GetVariable_reply(r): From cd8d364b63e2dec109085c6975c6c027208e595e Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 12:48:14 +0100 Subject: [PATCH 28/66] added logging to control proc --- visualswarm/control/motorinterface.py | 2 +- visualswarm/control/motoroutput.py | 38 ++++++++++++--------------- 2 files changed, 18 insertions(+), 22 deletions(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index f028b744..c07cffbd 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -32,7 +32,7 @@ def asebamedulla_init(): logger.info(f'{bcolors.OKBLUE}CONNECT{bcolors.ENDC} via asebamedulla on {control.THYMIO_DEVICE_PORT}') info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") time.sleep(5) - # network, connection_health = asebamedulla_health() + # if not connection_health: # logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') # asebamedulla_end() diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index a0783981..23e4586f 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -1,8 +1,10 @@ import dbus import dbus.mainloop.glib +import logging from gi.repository import GLib from visualswarm.control import motorinterface +from visualswarm.contrib import logparams import tempfile import random @@ -11,6 +13,19 @@ # to get and store Thymio sensor values proxSensorsVal = [0, 0, 0, 0, 0] +# using main logger +logger = logging.getLogger('visualswarm.app') +bcolors = logparams.BColors + + +def handle_GetVariable_reply(r): + global proxSensorsVal + proxSensorsVal = r + + +def handle_GetVariable_error(e): + raise Exception(str(e)) + def test_motor_control(network): # get the values of the sensors @@ -50,6 +65,7 @@ def control_thymio(control_stream, with_control=False): network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), dbus_interface='ch.epfl.mobots.AsebaNetwork') if motorinterface.asebamedulla_health(network): + logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') while True: (v, psi) = control_stream.get() @@ -59,25 +75,5 @@ def control_thymio(control_stream, with_control=False): network.SetVariable("thymio-II", "motor.left.target", [v_left]) network.SetVariable("thymio-II", "motor.right.target", [v_right]) else: + logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') raise Exception('asebamedulla connection not healthy!') - - -def handle_GetVariable_reply(r): - global proxSensorsVal - proxSensorsVal = r - - -def handle_GetVariable_error(e): - raise Exception(str(e)) - - -def execute_control_thymio(control_stream, network, loop): - # print in the terminal the name of each Aseba Node - # gobject.threads_init() - # print(network.GetNodesList()) - - # # GObject loop - # loop = GLib.MainLoop() - # call the callback of test_motor_control in every iteration - GLib.timeout_add(100, control_thymio, control_stream, network) # every 0.1 sec - loop.run() From aa57b64178cf84d134a1f597dd5a394f47af9fa7 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 13:03:18 +0100 Subject: [PATCH 29/66] Cleanup --- visualswarm/app.py | 4 ++-- visualswarm/behavior/behavior.py | 2 +- visualswarm/control/motorinterface.py | 16 +++------------- visualswarm/control/motoroutput.py | 1 - 4 files changed, 6 insertions(+), 17 deletions(-) diff --git a/visualswarm/app.py b/visualswarm/app.py index d5edb593..29c8a96c 100644 --- a/visualswarm/app.py +++ b/visualswarm/app.py @@ -40,8 +40,8 @@ def start_application(with_control=False): logger.info(f'{bcolors.OKGREEN}START vision stream{bcolors.ENDC} ') # connect to Thymio - if with_control: - motorinterface.asebamedulla_init() + # if with_control: + # motorinterface.asebamedulla_init() # Creating Queues raw_vision_stream = Queue() diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index 583e3c6c..dd425e7a 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -9,7 +9,7 @@ import visualswarm.contrib.vision from visualswarm.monitoring import ifdb -from visualswarm.contrib import monitoring, control +from visualswarm.contrib import monitoring from visualswarm.behavior import statevarcomp from visualswarm import env diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index c07cffbd..13d91214 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -1,6 +1,4 @@ import os -import dbus -import dbus.mainloop.glib import logging from dbus.exceptions import DBusException @@ -30,20 +28,12 @@ def asebamedulla_init(): Returns: None """ logger.info(f'{bcolors.OKBLUE}CONNECT{bcolors.ENDC} via asebamedulla on {control.THYMIO_DEVICE_PORT}') - info = os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") + os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") # nosec time.sleep(5) - # if not connection_health: - # logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') - # asebamedulla_end() - # raise Exception('Connection could not be established with robot!') - # else: - # logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') - # return network - def asebamedulla_end(): """Killing all established asebamedulla processes""" logger.info(f'{bcolors.OKBLUE}CLOSE{bcolors.ENDC} connection via asebamedulla') - os.system("pkill -f asebamedulla") - logger.info(f'{bcolors.OKGREEN}✓ CLOSE CONNECTION SUCCESSFUL{bcolors.ENDC}via asebamedulla') + os.system("pkill -f asebamedulla") # nosec + logger.info(f'{bcolors.OKGREEN}✓ CLOSE CONNECTION SUCCESSFUL{bcolors.ENDC} via asebamedulla') diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 23e4586f..aaf1028a 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -2,7 +2,6 @@ import dbus.mainloop.glib import logging -from gi.repository import GLib from visualswarm.control import motorinterface from visualswarm.contrib import logparams From 36e2146eae4d6510a0b0e777890c2a8056512af5 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 13:09:40 +0100 Subject: [PATCH 30/66] close connection when error occurs --- visualswarm/control/motoroutput.py | 1 + 1 file changed, 1 insertion(+) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index aaf1028a..79a68bc2 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -75,4 +75,5 @@ def control_thymio(control_stream, with_control=False): network.SetVariable("thymio-II", "motor.right.target", [v_right]) else: logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') + motorinterface.asebamedulla_end() raise Exception('asebamedulla connection not healthy!') From 07c79e3a53c0d93f09f218789a2e6778c9204ce5 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 13:12:57 +0100 Subject: [PATCH 31/66] oops --- visualswarm/app.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/app.py b/visualswarm/app.py index 29c8a96c..d5edb593 100644 --- a/visualswarm/app.py +++ b/visualswarm/app.py @@ -40,8 +40,8 @@ def start_application(with_control=False): logger.info(f'{bcolors.OKGREEN}START vision stream{bcolors.ENDC} ') # connect to Thymio - # if with_control: - # motorinterface.asebamedulla_init() + if with_control: + motorinterface.asebamedulla_init() # Creating Queues raw_vision_stream = Queue() From d0a81c91df35feca3ed647c6f507aab9e174e103 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Fri, 12 Mar 2021 16:07:47 +0100 Subject: [PATCH 32/66] switch to wired connection --- visualswarm/control/motorinterface.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index 13d91214..b5c5ed0b 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -28,7 +28,8 @@ def asebamedulla_init(): Returns: None """ logger.info(f'{bcolors.OKBLUE}CONNECT{bcolors.ENDC} via asebamedulla on {control.THYMIO_DEVICE_PORT}') - os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") # nosec + # os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") # nosec + os.system(f"(asebamedulla ser:name=Thymio-II &)") # nosec time.sleep(5) From 13835708f992404924264efdd067211808521ffa Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:36:30 +0100 Subject: [PATCH 33/66] Calculating theoretical maximum for state variables --- tests/test_behavior.py | 4 ++-- visualswarm/behavior/behavior.py | 14 +++++++++++--- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/tests/test_behavior.py b/tests/test_behavior.py index d78274c1..3ca67dcc 100644 --- a/tests/test_behavior.py +++ b/tests/test_behavior.py @@ -38,7 +38,7 @@ def test_VPF_to_behavior(self): with mock.patch('visualswarm.contrib.monitoring.SAVE_CONTROL_PARAMS', False): behavior.VPF_to_behavior(VPF_stream, control_stream, False) fake_create_client.assert_called_once() - fake_control_params.assert_called_once() + self.assertEqual(fake_control_params.call_count, 2) # resetting mocks fake_create_client.reset_mock() @@ -55,7 +55,7 @@ def test_VPF_to_behavior(self): with mock.patch('visualswarm.contrib.monitoring.SAVE_CONTROL_PARAMS', True): behavior.VPF_to_behavior(VPF_stream, control_stream, False) fake_create_client.assert_called_once() - fake_control_params.assert_called_once() + self.assertEqual(fake_control_params.call_count, 2) fake_ifclient.write_points.assert_called_once() # Case 3: motor output turned off diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index dd425e7a..e34c03cd 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -4,6 +4,7 @@ """ import datetime import logging +from math import floor import numpy as np @@ -33,11 +34,18 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): v = 0 psi = 0 + (projection_field, capture_timestamp) = VPF_stream.get() + phi = np.linspace(visualswarm.contrib.vision.PHI_START, visualswarm.contrib.vision.PHI_END, + len(projection_field)) + + # calculating theoretically max value for velocity change for normalization + max_VPF = np.zeros(len(projection_field)) + max_VPF[floor(len(projection_field) / 2)] = 1 + dv_max, dpsi_max = statevarcomp.compute_state_variables(v, phi, max_VPF) + print(dv_max, dpsi_max) + while True: (projection_field, capture_timestamp) = VPF_stream.get() - if phi is None: - phi = np.linspace(visualswarm.contrib.vision.PHI_START, visualswarm.contrib.vision.PHI_END, - len(projection_field)) dv, dpsi = statevarcomp.compute_state_variables(v, phi, projection_field) v += dv From 22b6e766b2f942d719e7c4cab10c895247fc073f Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:36:42 +0100 Subject: [PATCH 34/66] Cleanup --- visualswarm/control/motorinterface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualswarm/control/motorinterface.py b/visualswarm/control/motorinterface.py index b5c5ed0b..afb9b32d 100644 --- a/visualswarm/control/motorinterface.py +++ b/visualswarm/control/motorinterface.py @@ -29,7 +29,7 @@ def asebamedulla_init(): """ logger.info(f'{bcolors.OKBLUE}CONNECT{bcolors.ENDC} via asebamedulla on {control.THYMIO_DEVICE_PORT}') # os.system(f"(asebamedulla ser:device={control.THYMIO_DEVICE_PORT} &)") # nosec - os.system(f"(asebamedulla ser:name=Thymio-II &)") # nosec + os.system("(asebamedulla ser:name=Thymio-II &)") # nosec time.sleep(5) From 37777744d0e1f7435d7422ee0bcee07e30afc108 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:37:15 +0100 Subject: [PATCH 35/66] Mocking asebamedulla fn so that tests do not hang --- tests/test_app.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/tests/test_app.py b/tests/test_app.py index fe15d14e..8890b58e 100644 --- a/tests/test_app.py +++ b/tests/test_app.py @@ -15,7 +15,8 @@ def test_health(self): @mock.patch('visualswarm.app.Process') @mock.patch('visualswarm.app.Queue') - def test_start_application(self, mockQueue, mockProcess): + @mock.patch('visualswarm.control.motorinterface.asebamedulla_init', return_value=None) + def test_start_application(self, mock_asebamedulla_init, mockQueue, mockProcess): if FAKE_STATUS: with mock.patch('visualswarm.env.INFLUX_FRESH_DB_UPON_START', False): # Case 1 with interactive visualization @@ -29,10 +30,12 @@ def test_start_application(self, mockQueue, mockProcess): self.assertEqual(mp.start.call_count, num_processes) self.assertEqual(mp.join.call_count, num_processes) self.assertEqual(mockQueue.call_count, num_queues) + mock_asebamedulla_init.assert_called_once() # Case 1/B visualization was desired in env mockProcess.reset_mock() mockQueue.reset_mock() + mock_asebamedulla_init.reset_mock() with mock.patch('visualswarm.contrib.vision.SHOW_VISION_STREAMS', True): num_queues = 7 mp = mockProcess.return_value @@ -40,11 +43,13 @@ def test_start_application(self, mockQueue, mockProcess): self.assertEqual(mp.start.call_count, num_processes) self.assertEqual(mp.join.call_count, num_processes) self.assertEqual(mockQueue.call_count, num_queues) + mock_asebamedulla_init.assert_called_once() with mock.patch('visualswarm.contrib.vision.FIND_COLOR_INTERACTIVE', False): # Case 2 with no visualization at all mockProcess.reset_mock() mockQueue.reset_mock() + mock_asebamedulla_init.reset_mock() with mock.patch('visualswarm.contrib.vision.SHOW_VISION_STREAMS', False): num_queues = 4 mp = mockProcess.return_value @@ -52,6 +57,7 @@ def test_start_application(self, mockQueue, mockProcess): self.assertEqual(mp.start.call_count, num_processes) self.assertEqual(mp.join.call_count, num_processes) self.assertEqual(mockQueue.call_count, num_queues) + mock_asebamedulla_init.assert_called_once() # Case 2/B visualization but not interactive mockProcess.reset_mock() @@ -63,8 +69,10 @@ def test_start_application(self, mockQueue, mockProcess): self.assertEqual(mp.start.call_count, num_processes) self.assertEqual(mp.join.call_count, num_processes) self.assertEqual(mockQueue.call_count, num_queues) + mock_asebamedulla_init.assert_called_once() # Case 3 starting with DB wipe + mock_asebamedulla_init.reset_mock() with mock.patch('visualswarm.contrib.vision.FIND_COLOR_INTERACTIVE', False): with mock.patch('visualswarm.env.INFLUX_FRESH_DB_UPON_START', True): with mock.patch('visualswarm.monitoring.ifdb.create_ifclient') as fake_create_client: @@ -76,6 +84,7 @@ def test_start_application(self, mockQueue, mockProcess): fake_create_client.assert_called_once() fake_ifclient.drop_database.assert_called_once() fake_ifclient.create_database.assert_called_once() + mock_asebamedulla_init.assert_called_once() @mock.patch('visualswarm.app.start_application', return_value=None) def test_start_application_with_control(self, mock_start): From 5a252b81a47e20c8495ce136b0dff4ca62915b50 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:37:24 +0100 Subject: [PATCH 36/66] Mocking asebamedulla fn so that tests do not hang --- tests/test_app.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/test_app.py b/tests/test_app.py index 8890b58e..d8e92984 100644 --- a/tests/test_app.py +++ b/tests/test_app.py @@ -62,6 +62,7 @@ def test_start_application(self, mock_asebamedulla_init, mockQueue, mockProcess) # Case 2/B visualization but not interactive mockProcess.reset_mock() mockQueue.reset_mock() + mock_asebamedulla_init.reset_mock() with mock.patch('visualswarm.contrib.vision.SHOW_VISION_STREAMS', True): num_queues = 5 mp = mockProcess.return_value From 4d658eb31634c7c1b7fb8f46ac9d67f5ef0a3989 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:46:39 +0100 Subject: [PATCH 37/66] log --- visualswarm/behavior/behavior.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index e34c03cd..4bc271ea 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -42,7 +42,7 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): max_VPF = np.zeros(len(projection_field)) max_VPF[floor(len(projection_field) / 2)] = 1 dv_max, dpsi_max = statevarcomp.compute_state_variables(v, phi, max_VPF) - print(dv_max, dpsi_max) + logger.info(f"Theoretical MAX: {dv_max}, {dpsi_max}") while True: (projection_field, capture_timestamp) = VPF_stream.get() From caed4bdb3f62cebfb9c4549b482d008216190aeb Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:48:23 +0100 Subject: [PATCH 38/66] log --- visualswarm/behavior/behavior.py | 1 + 1 file changed, 1 insertion(+) diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index 4bc271ea..f7ef35cc 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -48,6 +48,7 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): (projection_field, capture_timestamp) = VPF_stream.get() dv, dpsi = statevarcomp.compute_state_variables(v, phi, projection_field) + logger.info(dv) v += dv psi += dpsi psi = psi % (2 * np.pi) From 50ebec63adcd0488b6a58a6f2ba736e9b52b4e7b Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:49:54 +0100 Subject: [PATCH 39/66] log --- visualswarm/behavior/behavior.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index f7ef35cc..aab26440 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -48,7 +48,7 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): (projection_field, capture_timestamp) = VPF_stream.get() dv, dpsi = statevarcomp.compute_state_variables(v, phi, projection_field) - logger.info(dv) + logger.info(dv/dv_max) v += dv psi += dpsi psi = psi % (2 * np.pi) From fad6dfd2d4614b59ed30b347c4e24241828ae85e Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:59:08 +0100 Subject: [PATCH 40/66] new motor control --- visualswarm/behavior/behavior.py | 5 ++--- visualswarm/control/motoroutput.py | 21 ++++++++++++++++++--- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index aab26440..c002ba58 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -42,13 +42,12 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): max_VPF = np.zeros(len(projection_field)) max_VPF[floor(len(projection_field) / 2)] = 1 dv_max, dpsi_max = statevarcomp.compute_state_variables(v, phi, max_VPF) - logger.info(f"Theoretical MAX: {dv_max}, {dpsi_max}") while True: (projection_field, capture_timestamp) = VPF_stream.get() dv, dpsi = statevarcomp.compute_state_variables(v, phi, projection_field) - logger.info(dv/dv_max) + dv_norm = dv/dv_max v += dv psi += dpsi psi = psi % (2 * np.pi) @@ -75,7 +74,7 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): ifclient.write_points(body, time_precision='ms') if with_control: - control_stream.put((v, dpsi)) + control_stream.put((dv_norm, dpsi)) # To test infinite loops if env.EXIT_CONDITION: diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 79a68bc2..e1f62841 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -66,13 +66,28 @@ def control_thymio(control_stream, with_control=False): if motorinterface.asebamedulla_health(network): logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') while True: - (v, psi) = control_stream.get() + v_max_motor = 500 + (dv_norm, dpsi) = control_stream.get() - v_left = v * (1 + psi) / 2 * 100 - v_right = v * (1 - psi) / 2 * 100 + v_left_current = network.GetVariable("thymio-II", "motor.left.target") + v_right_current = network.SetVariable("thymio-II", "motor.right.target") + + # v_left = v * (1 + dpsi) / 2 * 100 + # v_right = v * (1 - dpsi) / 2 * 100 + + v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 + v_left = v_left_current + v_left_change + if v_left >= v_max_motor: + v_left = v_max_motor + + v_right_change = dv_norm * v_max_motor * (1 - dpsi) / 2 + v_right = v_right_current + v_right_change + if v_right >= v_max_motor: + v_right = v_max_motor network.SetVariable("thymio-II", "motor.left.target", [v_left]) network.SetVariable("thymio-II", "motor.right.target", [v_right]) + logger.info(f"left: {v_left} \t right: {v_right}") else: logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') motorinterface.asebamedulla_end() From 65feeedc27d7f2db6c8e379609f4d6e43789486f Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 08:59:59 +0100 Subject: [PATCH 41/66] Set Get bug --- visualswarm/control/motoroutput.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index e1f62841..08ad6871 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -70,7 +70,7 @@ def control_thymio(control_stream, with_control=False): (dv_norm, dpsi) = control_stream.get() v_left_current = network.GetVariable("thymio-II", "motor.left.target") - v_right_current = network.SetVariable("thymio-II", "motor.right.target") + v_right_current = network.GetVariable("thymio-II", "motor.right.target") # v_left = v * (1 + dpsi) / 2 * 100 # v_right = v * (1 - dpsi) / 2 * 100 From 9eabf7b5f5bcb68f6b13c53c5c31fc027683ad95 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 09:04:17 +0100 Subject: [PATCH 42/66] velocity signs --- visualswarm/control/motoroutput.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 08ad6871..9794acdd 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -17,6 +17,9 @@ bcolors = logparams.BColors +def sign(x): return (x > 0) - (x < 0) + + def handle_GetVariable_reply(r): global proxSensorsVal proxSensorsVal = r @@ -77,13 +80,13 @@ def control_thymio(control_stream, with_control=False): v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 v_left = v_left_current + v_left_change - if v_left >= v_max_motor: - v_left = v_max_motor + if abs(v_left) >= v_max_motor: + v_left = sign(v_left) * v_max_motor v_right_change = dv_norm * v_max_motor * (1 - dpsi) / 2 v_right = v_right_current + v_right_change - if v_right >= v_max_motor: - v_right = v_max_motor + if abs(v_right) >= v_max_motor: + v_right = sign(v_right) * v_max_motor network.SetVariable("thymio-II", "motor.left.target", [v_left]) network.SetVariable("thymio-II", "motor.right.target", [v_right]) From 7cc4dccd63fc1204f553517ea07d72a30c49ce03 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 10:43:46 +0100 Subject: [PATCH 43/66] numpy sign --- visualswarm/control/motoroutput.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 9794acdd..29c8ecd4 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -1,6 +1,7 @@ import dbus import dbus.mainloop.glib import logging +from numpy import sign from visualswarm.control import motorinterface from visualswarm.contrib import logparams @@ -17,9 +18,6 @@ bcolors = logparams.BColors -def sign(x): return (x > 0) - (x < 0) - - def handle_GetVariable_reply(r): global proxSensorsVal proxSensorsVal = r From fa82c6456424d2d5dd710b3da32939409cdd56be Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 10:51:47 +0100 Subject: [PATCH 44/66] go back to old solution --- visualswarm/behavior/behavior.py | 2 +- visualswarm/control/motoroutput.py | 26 +++++++++++++------------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index c002ba58..c2efa2a3 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -74,7 +74,7 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): ifclient.write_points(body, time_precision='ms') if with_control: - control_stream.put((dv_norm, dpsi)) + control_stream.put((v, dpsi)) # To test infinite loops if env.EXIT_CONDITION: diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 29c8ecd4..f0b8ffca 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -68,23 +68,23 @@ def control_thymio(control_stream, with_control=False): logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') while True: v_max_motor = 500 - (dv_norm, dpsi) = control_stream.get() + (v, dpsi) = control_stream.get() v_left_current = network.GetVariable("thymio-II", "motor.left.target") v_right_current = network.GetVariable("thymio-II", "motor.right.target") - # v_left = v * (1 + dpsi) / 2 * 100 - # v_right = v * (1 - dpsi) / 2 * 100 - - v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 - v_left = v_left_current + v_left_change - if abs(v_left) >= v_max_motor: - v_left = sign(v_left) * v_max_motor - - v_right_change = dv_norm * v_max_motor * (1 - dpsi) / 2 - v_right = v_right_current + v_right_change - if abs(v_right) >= v_max_motor: - v_right = sign(v_right) * v_max_motor + v_left = v * (1 + dpsi) / 2 * 100 + v_right = v * (1 - dpsi) / 2 * 100 + + # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 + # v_left = v_left_current + v_left_change + # if abs(v_left) >= v_max_motor: + # v_left = sign(v_left) * v_max_motor + # + # v_right_change = dv_norm * v_max_motor * (1 - dpsi) / 2 + # v_right = v_right_current + v_right_change + # if abs(v_right) >= v_max_motor: + # v_right = sign(v_right) * v_max_motor network.SetVariable("thymio-II", "motor.left.target", [v_left]) network.SetVariable("thymio-II", "motor.right.target", [v_right]) From a9311b30d0a6e511d6fe937eeac2d7ae7e7c4342 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:11:16 +0100 Subject: [PATCH 45/66] go back to old solution --- visualswarm/control/motoroutput.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index f0b8ffca..2638ddd3 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -73,8 +73,8 @@ def control_thymio(control_stream, with_control=False): v_left_current = network.GetVariable("thymio-II", "motor.left.target") v_right_current = network.GetVariable("thymio-II", "motor.right.target") - v_left = v * (1 + dpsi) / 2 * 100 - v_right = v * (1 - dpsi) / 2 * 100 + v_left = v * (1 + dpsi) / 2 + v_right = v * (1 - dpsi) / 2 # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 # v_left = v_left_current + v_left_change From fc2a821310c6be885775339f0b054f59d4a60c2b Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:16:12 +0100 Subject: [PATCH 46/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 2638ddd3..a2f81639 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -76,6 +76,9 @@ def control_thymio(control_stream, with_control=False): v_left = v * (1 + dpsi) / 2 v_right = v * (1 - dpsi) / 2 + v_left_perc = v_left / (v_left + v_right) + v_right_perc = v_right/ (v_left + v_right) + # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 # v_left = v_left_current + v_left_change # if abs(v_left) >= v_max_motor: @@ -86,8 +89,8 @@ def control_thymio(control_stream, with_control=False): # if abs(v_right) >= v_max_motor: # v_right = sign(v_right) * v_max_motor - network.SetVariable("thymio-II", "motor.left.target", [v_left]) - network.SetVariable("thymio-II", "motor.right.target", [v_right]) + network.SetVariable("thymio-II", "motor.left.target", [v_left_perc*v_max_motor]) + network.SetVariable("thymio-II", "motor.right.target", [v_right_perc*v_max_motor]) logger.info(f"left: {v_left} \t right: {v_right}") else: logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') From 4f9fab257c53713e03c883adeeb407b0d984c6fd Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:18:40 +0100 Subject: [PATCH 47/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index a2f81639..9c12a169 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -77,7 +77,7 @@ def control_thymio(control_stream, with_control=False): v_right = v * (1 - dpsi) / 2 v_left_perc = v_left / (v_left + v_right) - v_right_perc = v_right/ (v_left + v_right) + v_right_perc = v_right / (v_left + v_right) # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 # v_left = v_left_current + v_left_change @@ -89,8 +89,8 @@ def control_thymio(control_stream, with_control=False): # if abs(v_right) >= v_max_motor: # v_right = sign(v_right) * v_max_motor - network.SetVariable("thymio-II", "motor.left.target", [v_left_perc*v_max_motor]) - network.SetVariable("thymio-II", "motor.right.target", [v_right_perc*v_max_motor]) + network.SetVariable("thymio-II", "motor.left.target", [int(v_left_perc*v_max_motor)]) + network.SetVariable("thymio-II", "motor.right.target", [int(v_right_perc*v_max_motor)]) logger.info(f"left: {v_left} \t right: {v_right}") else: logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') From 0b44ed1a06a64d026ea29fe8af09370dbd914a48 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:20:17 +0100 Subject: [PATCH 48/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 9c12a169..89473f63 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -79,6 +79,9 @@ def control_thymio(control_stream, with_control=False): v_left_perc = v_left / (v_left + v_right) v_right_perc = v_right / (v_left + v_right) + v_left = int(v_left_perc*v_max_motor) + v_right = int(v_right_perc*v_max_motor) + # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 # v_left = v_left_current + v_left_change # if abs(v_left) >= v_max_motor: @@ -89,8 +92,8 @@ def control_thymio(control_stream, with_control=False): # if abs(v_right) >= v_max_motor: # v_right = sign(v_right) * v_max_motor - network.SetVariable("thymio-II", "motor.left.target", [int(v_left_perc*v_max_motor)]) - network.SetVariable("thymio-II", "motor.right.target", [int(v_right_perc*v_max_motor)]) + network.SetVariable("thymio-II", "motor.left.target", [v_left]) + network.SetVariable("thymio-II", "motor.right.target", [v_right]) logger.info(f"left: {v_left} \t right: {v_right}") else: logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') From 7912761cc22212b8645f9e532654f67b616029c6 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:23:26 +0100 Subject: [PATCH 49/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 1 + 1 file changed, 1 insertion(+) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 89473f63..f87f0086 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -81,6 +81,7 @@ def control_thymio(control_stream, with_control=False): v_left = int(v_left_perc*v_max_motor) v_right = int(v_right_perc*v_max_motor) + logger.info(v_left) # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 # v_left = v_left_current + v_left_change From 179ccc2b9cb086fec93b1771e3faaa897f286be1 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:26:08 +0100 Subject: [PATCH 50/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index f87f0086..054fbb16 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -76,11 +76,15 @@ def control_thymio(control_stream, with_control=False): v_left = v * (1 + dpsi) / 2 v_right = v * (1 - dpsi) / 2 - v_left_perc = v_left / (v_left + v_right) - v_right_perc = v_right / (v_left + v_right) - - v_left = int(v_left_perc*v_max_motor) - v_right = int(v_right_perc*v_max_motor) + v_left_perc = v_left / (abs(v_left) + abs(v_right)) + v_right_perc = v_right / (abs(v_left) + abs(v_right)) + + try: + v_left = int(v_left_perc*v_max_motor) + v_right = int(v_right_perc*v_max_motor) + except ValueError: + v_left = 0 + v_right = 0 logger.info(v_left) # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 From 8ec09841b03fe8b55a22bbca87e09759692f1540 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:28:18 +0100 Subject: [PATCH 51/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 054fbb16..af641dce 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -73,19 +73,18 @@ def control_thymio(control_stream, with_control=False): v_left_current = network.GetVariable("thymio-II", "motor.left.target") v_right_current = network.GetVariable("thymio-II", "motor.right.target") - v_left = v * (1 + dpsi) / 2 - v_right = v * (1 - dpsi) / 2 - - v_left_perc = v_left / (abs(v_left) + abs(v_right)) - v_right_perc = v_right / (abs(v_left) + abs(v_right)) - - try: - v_left = int(v_left_perc*v_max_motor) - v_right = int(v_right_perc*v_max_motor) - except ValueError: - v_left = 0 - v_right = 0 - logger.info(v_left) + v_left = v * (1 + dpsi) / 2 * 10 + v_right = v * (1 - dpsi) / 2 * 10 + + # v_left_perc = v_left / (abs(v_left) + abs(v_right)) + # v_right_perc = v_right / (abs(v_left) + abs(v_right)) + # + # try: + # v_left = int(v_left_perc*v_max_motor) + # v_right = int(v_right_perc*v_max_motor) + # except ValueError: + # v_left = 0 + # v_right = 0 # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 # v_left = v_left_current + v_left_change From e943e0ae9bcad7a0dbc875f134e18433e5ee8c92 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:35:24 +0100 Subject: [PATCH 52/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index af641dce..f257bf7f 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -73,8 +73,8 @@ def control_thymio(control_stream, with_control=False): v_left_current = network.GetVariable("thymio-II", "motor.left.target") v_right_current = network.GetVariable("thymio-II", "motor.right.target") - v_left = v * (1 + dpsi) / 2 * 10 - v_right = v * (1 - dpsi) / 2 * 10 + v_left = v * (1 + dpsi) / 2 * 100 + v_right = v * (1 - dpsi) / 2 * 100 # v_left_perc = v_left / (abs(v_left) + abs(v_right)) # v_right_perc = v_right / (abs(v_left) + abs(v_right)) From 2177153b30f9854feebb1f088e0a3f5714efffcc Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 11:41:41 +0100 Subject: [PATCH 53/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index f257bf7f..50b27b8a 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -70,8 +70,8 @@ def control_thymio(control_stream, with_control=False): v_max_motor = 500 (v, dpsi) = control_stream.get() - v_left_current = network.GetVariable("thymio-II", "motor.left.target") - v_right_current = network.GetVariable("thymio-II", "motor.right.target") + # v_left_current = network.GetVariable("thymio-II", "motor.left.target") + # v_right_current = network.GetVariable("thymio-II", "motor.right.target") v_left = v * (1 + dpsi) / 2 * 100 v_right = v * (1 - dpsi) / 2 * 100 From 5eae1736ced4a1683e40ec15be86fb08d5ed202a Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:12:05 +0100 Subject: [PATCH 54/66] trying to normalize velocity --- visualswarm/control/motoroutput.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 50b27b8a..eea888c8 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -76,15 +76,15 @@ def control_thymio(control_stream, with_control=False): v_left = v * (1 + dpsi) / 2 * 100 v_right = v * (1 - dpsi) / 2 * 100 - # v_left_perc = v_left / (abs(v_left) + abs(v_right)) - # v_right_perc = v_right / (abs(v_left) + abs(v_right)) - # - # try: - # v_left = int(v_left_perc*v_max_motor) - # v_right = int(v_right_perc*v_max_motor) - # except ValueError: - # v_left = 0 - # v_right = 0 + v_left_perc = v_left / (abs(v_left) + abs(v_right)) + v_right_perc = v_right / (abs(v_left) + abs(v_right)) + + try: + v_left = int(v_left_perc*v_max_motor) + v_right = int(v_right_perc*v_max_motor) + except ValueError: + v_left = 0 + v_right = 0 # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 # v_left = v_left_current + v_left_change From a34b05c8800ed825cd122ac62bfdb797ca061f29 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:13:38 +0100 Subject: [PATCH 55/66] original heuristics approach works the best --- visualswarm/control/motoroutput.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index eea888c8..34e034b4 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -73,18 +73,18 @@ def control_thymio(control_stream, with_control=False): # v_left_current = network.GetVariable("thymio-II", "motor.left.target") # v_right_current = network.GetVariable("thymio-II", "motor.right.target") - v_left = v * (1 + dpsi) / 2 * 100 - v_right = v * (1 - dpsi) / 2 * 100 - - v_left_perc = v_left / (abs(v_left) + abs(v_right)) - v_right_perc = v_right / (abs(v_left) + abs(v_right)) - - try: - v_left = int(v_left_perc*v_max_motor) - v_right = int(v_right_perc*v_max_motor) - except ValueError: - v_left = 0 - v_right = 0 + v_left = v * (1 + dpsi) / 2 * 10 + v_right = v * (1 - dpsi) / 2 * 10 + + # v_left_perc = v_left / (abs(v_left) + abs(v_right)) + # v_right_perc = v_right / (abs(v_left) + abs(v_right)) + # + # try: + # v_left = int(v_left_perc*v_max_motor) + # v_right = int(v_right_perc*v_max_motor) + # except ValueError: + # v_left = 0 + # v_right = 0 # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 # v_left = v_left_current + v_left_change From bfa836a4e40f9948bedb147659cc2a9e74076e23 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:16:09 +0100 Subject: [PATCH 56/66] original heuristics approach works the best --- visualswarm/control/motoroutput.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 34e034b4..406cf434 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -73,8 +73,8 @@ def control_thymio(control_stream, with_control=False): # v_left_current = network.GetVariable("thymio-II", "motor.left.target") # v_right_current = network.GetVariable("thymio-II", "motor.right.target") - v_left = v * (1 + dpsi) / 2 * 10 - v_right = v * (1 - dpsi) / 2 * 10 + v_left = v * (1 + dpsi) / 2 * 50 + v_right = v * (1 - dpsi) / 2 * 50 # v_left_perc = v_left / (abs(v_left) + abs(v_right)) # v_right_perc = v_right / (abs(v_left) + abs(v_right)) From 2a2b9534144add3b9e4db9cc91e6782b56a52e9c Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:19:53 +0100 Subject: [PATCH 57/66] original heuristics approach works the best --- visualswarm/control/motoroutput.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 406cf434..50b27b8a 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -73,8 +73,8 @@ def control_thymio(control_stream, with_control=False): # v_left_current = network.GetVariable("thymio-II", "motor.left.target") # v_right_current = network.GetVariable("thymio-II", "motor.right.target") - v_left = v * (1 + dpsi) / 2 * 50 - v_right = v * (1 - dpsi) / 2 * 50 + v_left = v * (1 + dpsi) / 2 * 100 + v_right = v * (1 - dpsi) / 2 * 100 # v_left_perc = v_left / (abs(v_left) + abs(v_right)) # v_right_perc = v_right / (abs(v_left) + abs(v_right)) From e925960d28c5b7529955db5fca941586b7c8200c Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:26:10 +0100 Subject: [PATCH 58/66] original heuristics approach works the best --- visualswarm/contrib/behavior.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualswarm/contrib/behavior.py b/visualswarm/contrib/behavior.py index 3a34a8ee..008c248c 100644 --- a/visualswarm/contrib/behavior.py +++ b/visualswarm/contrib/behavior.py @@ -6,7 +6,7 @@ # Velocity Parameters GAM = 0.2 V0 = 0 -ALP0 = 0.5 +ALP0 = 0.1 ALP1 = 0.015 ALP2 = 0 From bc4ec630ad00153782c1e7bb1d499983a312fe25 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:27:18 +0100 Subject: [PATCH 59/66] original heuristics approach works the best --- visualswarm/contrib/behavior.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/visualswarm/contrib/behavior.py b/visualswarm/contrib/behavior.py index 008c248c..c1441525 100644 --- a/visualswarm/contrib/behavior.py +++ b/visualswarm/contrib/behavior.py @@ -6,12 +6,12 @@ # Velocity Parameters GAM = 0.2 V0 = 0 -ALP0 = 0.1 +ALP0 = 0.5 ALP1 = 0.015 ALP2 = 0 # Heading Vector Parameters -BET0 = 0.5 +BET0 = 0.1 BET1 = 0.015 BET2 = 0 From b22f6591f9f787886332c76408a90b66b472dad1 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:30:10 +0100 Subject: [PATCH 60/66] investigating parameters --- visualswarm/contrib/behavior.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/visualswarm/contrib/behavior.py b/visualswarm/contrib/behavior.py index c1441525..4a1a2d12 100644 --- a/visualswarm/contrib/behavior.py +++ b/visualswarm/contrib/behavior.py @@ -6,12 +6,12 @@ # Velocity Parameters GAM = 0.2 V0 = 0 -ALP0 = 0.5 -ALP1 = 0.015 +ALP0 = 0.5 # overall responsiveness of velocity change (acceleration "speed") +ALP1 = 0.015 # ~ 1 / equilibrium distance ALP2 = 0 # Heading Vector Parameters -BET0 = 0.1 +BET0 = 0.3 # overall responsiveness of heading change (turning "speed") BET1 = 0.015 BET2 = 0 From 446542d03fbcbf24a4788bc9bf853b158925fd38 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:33:51 +0100 Subject: [PATCH 61/66] Deleting unused parameters --- visualswarm/contrib/behavior.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/visualswarm/contrib/behavior.py b/visualswarm/contrib/behavior.py index 4a1a2d12..ddf66104 100644 --- a/visualswarm/contrib/behavior.py +++ b/visualswarm/contrib/behavior.py @@ -6,7 +6,7 @@ # Velocity Parameters GAM = 0.2 V0 = 0 -ALP0 = 0.5 # overall responsiveness of velocity change (acceleration "speed") +ALP0 = 0.5 # overall speed scale (limited by possible motor speed) ALP1 = 0.015 # ~ 1 / equilibrium distance ALP2 = 0 @@ -15,10 +15,3 @@ BET1 = 0.015 BET2 = 0 -# Normalizing Velocity -V_MAX_ALG = 6000 -V_MAX_PHYS = 400 - -# Normalizing dpsi -DPSI_MAX_ALG = 700 -DPSI_MAX_PHYS = 1 From 45e7f76e604f74a5526d7356ff4ca3ca61ae6e17 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:41:16 +0100 Subject: [PATCH 62/66] Cleaning up --- visualswarm/contrib/control.py | 3 +++ visualswarm/control/motoroutput.py | 39 +++++++++--------------------- 2 files changed, 14 insertions(+), 28 deletions(-) diff --git a/visualswarm/contrib/control.py b/visualswarm/contrib/control.py index 36e51ca2..690fae34 100644 --- a/visualswarm/contrib/control.py +++ b/visualswarm/contrib/control.py @@ -4,3 +4,6 @@ """ # Serial port on which the Thymio is available THYMIO_DEVICE_PORT = "/dev/ttyACM0" + +# Motor scale correction to put the motor scales into the right region +MOTOR_SCALE_CORRECTION = 100 \ No newline at end of file diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 50b27b8a..a5e37608 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -4,7 +4,7 @@ from numpy import sign from visualswarm.control import motorinterface -from visualswarm.contrib import logparams +from visualswarm.contrib import logparams, control import tempfile import random @@ -56,48 +56,31 @@ def control_thymio(control_stream, with_control=False): if not with_control: # simply consuming the input stream so that we don't fill up memory while True: - (v, psi) = control_stream.get() + (v, dpsi) = control_stream.get() else: + # Initializing DBus dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) bus = dbus.SessionBus() # Create Aseba network network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), dbus_interface='ch.epfl.mobots.AsebaNetwork') + if motorinterface.asebamedulla_health(network): logger.info(f'{bcolors.OKGREEN}✓ CONNECTION SUCCESSFUl{bcolors.ENDC} via asebamedulla') + while True: - v_max_motor = 500 + # fetching state variables (v, dpsi) = control_stream.get() - # v_left_current = network.GetVariable("thymio-II", "motor.left.target") - # v_right_current = network.GetVariable("thymio-II", "motor.right.target") - - v_left = v * (1 + dpsi) / 2 * 100 - v_right = v * (1 - dpsi) / 2 * 100 - - # v_left_perc = v_left / (abs(v_left) + abs(v_right)) - # v_right_perc = v_right / (abs(v_left) + abs(v_right)) - # - # try: - # v_left = int(v_left_perc*v_max_motor) - # v_right = int(v_right_perc*v_max_motor) - # except ValueError: - # v_left = 0 - # v_right = 0 - - # v_left_change = dv_norm * v_max_motor * (1 + dpsi) / 2 - # v_left = v_left_current + v_left_change - # if abs(v_left) >= v_max_motor: - # v_left = sign(v_left) * v_max_motor - # - # v_right_change = dv_norm * v_max_motor * (1 - dpsi) / 2 - # v_right = v_right_current + v_right_change - # if abs(v_right) >= v_max_motor: - # v_right = sign(v_right) * v_max_motor + # distributing v according dpsi to the differential system + v_left = v * (1 + dpsi) / 2 * control.MOTOR_SCALE_CORRECTION + v_right = v * (1 - dpsi) / 2 * control.MOTOR_SCALE_CORRECTION + # sending motor values to robot network.SetVariable("thymio-II", "motor.left.target", [v_left]) network.SetVariable("thymio-II", "motor.right.target", [v_right]) + logger.info(f"left: {v_left} \t right: {v_right}") else: logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') From bfc5788d6387852f1dd3ae1096ab04c6e3564de7 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:47:30 +0100 Subject: [PATCH 63/66] Cleaning up --- visualswarm/behavior/behavior.py | 1 - visualswarm/control/motoroutput.py | 66 ++++++++++++++---------------- 2 files changed, 30 insertions(+), 37 deletions(-) diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index c2efa2a3..157c5934 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -26,7 +26,6 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): control_stream (multiprocessing Queue): stream to push calculated control parameters Returns: -shall not return- - :param with_control: """ measurement_name = "control_parameters" ifclient = ifdb.create_ifclient() diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index a5e37608..bd6b6ff1 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -9,50 +9,44 @@ import tempfile import random -# Create a global variable or Queue for GetVariable values -# to get and store Thymio sensor values -proxSensorsVal = [0, 0, 0, 0, 0] - # using main logger logger = logging.getLogger('visualswarm.app') bcolors = logparams.BColors -def handle_GetVariable_reply(r): - global proxSensorsVal - proxSensorsVal = r - - -def handle_GetVariable_error(e): - raise Exception(str(e)) - - -def test_motor_control(network): - # get the values of the sensors - network.GetVariable("thymio-II", "prox.horizontal", reply_handler=handle_GetVariable_reply, - error_handler=handle_GetVariable_error) - - # print the proximity sensors value in the terminal - print(proxSensorsVal[0], proxSensorsVal[1], proxSensorsVal[2], proxSensorsVal[3], proxSensorsVal[4]) - - with tempfile.NamedTemporaryFile(suffix='.aesl', mode='w+t') as aesl: - aesl.write('\n\n') - node_id = 1 - name = 'thymio-II' - aesl.write(f'\n') - # add code to handle incoming events - R = random.randint(0, 32) # nosec - G = random.randint(0, 32) # nosec - B = random.randint(0, 32) # nosec - aesl.write(f'call leds.top({R},{G},{B})\n') - aesl.write('\n') - aesl.write('\n') - aesl.seek(0) - network.LoadScripts(aesl.name) - return True +# def test_motor_control(network): +# # get the values of the sensors +# proxSensorsVal = network.GetVariable("thymio-II", "prox.horizontal") +# +# # print the proximity sensors value in the terminal +# print(proxSensorsVal[0], proxSensorsVal[1], proxSensorsVal[2], proxSensorsVal[3], proxSensorsVal[4]) +# +# with tempfile.NamedTemporaryFile(suffix='.aesl', mode='w+t') as aesl: +# aesl.write('\n\n') +# node_id = 1 +# name = 'thymio-II' +# aesl.write(f'\n') +# # add code to handle incoming events +# R = random.randint(0, 32) # nosec +# G = random.randint(0, 32) # nosec +# B = random.randint(0, 32) # nosec +# aesl.write(f'call leds.top({R},{G},{B})\n') +# aesl.write('\n') +# aesl.write('\n') +# aesl.seek(0) +# network.LoadScripts(aesl.name) +# return True def control_thymio(control_stream, with_control=False): + """ + Process to translate state variables to motor velocities and send to Thymio2 robot via DBUS. + Args: + control_stream (multiprocessing Queue): stream to push calculated control parameters + with_control (boolean): sends motor command to robot if true. Only consumes input stream if false. + Returns: + -shall not return- + """ if not with_control: # simply consuming the input stream so that we don't fill up memory while True: From 2be2d3f06656d9eb230f5080439a53fd5851f818 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 15:51:00 +0100 Subject: [PATCH 64/66] Cleaning up --- visualswarm/behavior/behavior.py | 6 ++---- visualswarm/contrib/behavior.py | 1 - visualswarm/contrib/control.py | 2 +- visualswarm/control/motoroutput.py | 5 ++--- 4 files changed, 5 insertions(+), 9 deletions(-) diff --git a/visualswarm/behavior/behavior.py b/visualswarm/behavior/behavior.py index 157c5934..8fc2786b 100644 --- a/visualswarm/behavior/behavior.py +++ b/visualswarm/behavior/behavior.py @@ -31,7 +31,6 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): ifclient = ifdb.create_ifclient() phi = None v = 0 - psi = 0 (projection_field, capture_timestamp) = VPF_stream.get() phi = np.linspace(visualswarm.contrib.vision.PHI_START, visualswarm.contrib.vision.PHI_END, @@ -46,10 +45,9 @@ def VPF_to_behavior(VPF_stream, control_stream, with_control=False): (projection_field, capture_timestamp) = VPF_stream.get() dv, dpsi = statevarcomp.compute_state_variables(v, phi, projection_field) - dv_norm = dv/dv_max v += dv - psi += dpsi - psi = psi % (2 * np.pi) + # psi += dpsi + # psi = psi % (2 * np.pi) if monitoring.SAVE_CONTROL_PARAMS: diff --git a/visualswarm/contrib/behavior.py b/visualswarm/contrib/behavior.py index ddf66104..54421480 100644 --- a/visualswarm/contrib/behavior.py +++ b/visualswarm/contrib/behavior.py @@ -14,4 +14,3 @@ BET0 = 0.3 # overall responsiveness of heading change (turning "speed") BET1 = 0.015 BET2 = 0 - diff --git a/visualswarm/contrib/control.py b/visualswarm/contrib/control.py index 690fae34..e3742732 100644 --- a/visualswarm/contrib/control.py +++ b/visualswarm/contrib/control.py @@ -6,4 +6,4 @@ THYMIO_DEVICE_PORT = "/dev/ttyACM0" # Motor scale correction to put the motor scales into the right region -MOTOR_SCALE_CORRECTION = 100 \ No newline at end of file +MOTOR_SCALE_CORRECTION = 100 diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index bd6b6ff1..694f3dc8 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -1,13 +1,12 @@ import dbus import dbus.mainloop.glib import logging -from numpy import sign from visualswarm.control import motorinterface from visualswarm.contrib import logparams, control -import tempfile -import random +# import tempfile +# import random # using main logger logger = logging.getLogger('visualswarm.app') From b4b3412deddbe0e67c39aceb8eefa9cb93eb8e1e Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 16:05:37 +0100 Subject: [PATCH 65/66] Added tests to motorinterface (95%) --- tests/test_motorinterface.py | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 tests/test_motorinterface.py diff --git a/tests/test_motorinterface.py b/tests/test_motorinterface.py new file mode 100644 index 00000000..42180d7a --- /dev/null +++ b/tests/test_motorinterface.py @@ -0,0 +1,36 @@ +from unittest import TestCase, mock + +from dbus.exceptions import DBusException + +from visualswarm.control import motorinterface + + +class MotorInterfaceTest(TestCase): + + def DBusException_raise(self, *args, **kwargs): + raise DBusException + + def test_asebamedulla_health(self): + network = mock.MagicMock() + network.GetVariable.return_value = None + + # Case 1 : connection to thymio successful + motorinterface.asebamedulla_health(network) + network.GetVariable.assert_called_once_with("thymio-II", "acc", timeout=5) + + # Case 2 : DBusException is raised during communication + network.GetVariable.reset_mock() + network.GetVariable.side_effect = self.DBusException_raise + self.assertEqual(motorinterface.asebamedulla_health(network), False) + + @mock.patch('os.system', return_value=None) + @mock.patch('time.sleep', return_value=None) + def test_asebamedulla_init(self, mock_sleep, mock_os): + motorinterface.asebamedulla_init() + mock_os.assert_called_once_with("(asebamedulla ser:name=Thymio-II &)") + mock_sleep.assert_called_once_with(5) + + @mock.patch('os.system', return_value=None) + def test_asebamedulla_end(self, mock_os): + motorinterface.asebamedulla_end() + mock_os.assert_called_once_with("pkill -f asebamedulla") From 57c7b3367f4106ddaa8d3d00488007eef49d90b7 Mon Sep 17 00:00:00 2001 From: mezdahun Date: Thu, 18 Mar 2021 16:44:15 +0100 Subject: [PATCH 66/66] Added tests to motoroutput (100%) --- tests/test_motoroutput.py | 48 ++++++++++++++++++++++++++++++ visualswarm/control/motoroutput.py | 9 ++++++ 2 files changed, 57 insertions(+) create mode 100644 tests/test_motoroutput.py diff --git a/tests/test_motoroutput.py b/tests/test_motoroutput.py new file mode 100644 index 00000000..66eac665 --- /dev/null +++ b/tests/test_motoroutput.py @@ -0,0 +1,48 @@ +from unittest import TestCase, mock + +from visualswarm.control import motoroutput + + +class MotorInterfaceTest(TestCase): + + @mock.patch('visualswarm.env.EXIT_CONDITION', True) + @mock.patch('dbus.mainloop.glib.DBusGMainLoop', return_value=None) + @mock.patch('dbus.SessionBus') + @mock.patch('dbus.Interface') + def test_control_thymio(self, mock_network_init, mock_dbus_sessionbus, mock_dbus_init): + mock_network = mock.MagicMock(return_value=None) + mock_network.SetVariable.return_value = None + mock_network_init.return_value = mock_network + + mock_dbus_sessionbus.return_value.get_object.return_value = None + control_stream = mock.MagicMock() + control_stream.get.return_value = (1, 1) + + # Case 1 : healthy connection via asebamedulla + with mock.patch('visualswarm.control.motorinterface.asebamedulla_health') as mock_health: + mock_health.return_value = True + # Case 1/a : with no control + motoroutput.control_thymio(control_stream, with_control=False) + control_stream.get.assert_called_once() + + control_stream.get.reset_mock() + mock_health.reset_mock() + # Case 1/b : with control + motoroutput.control_thymio(control_stream, with_control=True) + mock_dbus_init.assert_called_once_with(set_as_default=True) + mock_dbus_sessionbus.assert_called_once() + mock_network_init.assert_called_once() + mock_health.assert_called_once() + control_stream.get.assert_called_once() + self.assertEqual(mock_network.SetVariable.call_count, 2) + + # Case 2 : unhealthy connection via asebamedulla + with mock.patch('visualswarm.control.motorinterface.asebamedulla_health') as mock_health: + with mock.patch('visualswarm.control.motorinterface.asebamedulla_end'): + mock_health.return_value = False + control_stream.reset_mock() + motoroutput.control_thymio(control_stream, with_control=False) + control_stream.get.assert_called_once() + + with self.assertRaises(Exception): + motoroutput.control_thymio(control_stream, with_control=True) diff --git a/visualswarm/control/motoroutput.py b/visualswarm/control/motoroutput.py index 694f3dc8..851c43fc 100644 --- a/visualswarm/control/motoroutput.py +++ b/visualswarm/control/motoroutput.py @@ -4,6 +4,7 @@ from visualswarm.control import motorinterface from visualswarm.contrib import logparams, control +from visualswarm import env # import tempfile # import random @@ -50,6 +51,10 @@ def control_thymio(control_stream, with_control=False): # simply consuming the input stream so that we don't fill up memory while True: (v, dpsi) = control_stream.get() + + # To test infinite loops + if env.EXIT_CONDITION: + break else: # Initializing DBus dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) @@ -75,6 +80,10 @@ def control_thymio(control_stream, with_control=False): network.SetVariable("thymio-II", "motor.right.target", [v_right]) logger.info(f"left: {v_left} \t right: {v_right}") + + # To test infinite loops + if env.EXIT_CONDITION: + break else: logger.error(f'{bcolors.FAIL}🗴 CONNECTION FAILED{bcolors.ENDC} via asebamedulla') motorinterface.asebamedulla_end()