Merge pull request #48 from mezdahun/feature/enhance-motor-control

Feature/enhance motor control

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,42 +30,50 @@ 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
                         app.start_application(with_control=True)
                         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
                         app.start_application(with_control=True)
                         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()
                     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
                         app.start_application(with_control=True)
                         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 +85,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):

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()
+                            self.assertEqual(fake_control_params.call_count, 2)
 
-                            # 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()
+                            self.assertEqual(fake_control_params.call_count, 2)
+                            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()

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")

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)

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
 
@@ -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()
@@ -70,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:
@@ -85,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
@@ -96,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:
@@ -107,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!')
@@ -150,6 +150,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}')
 

visualswarm/behavior/behavior.py

@@ -4,20 +4,21 @@
 """
 import datetime
 import logging
+from math import floor
 
 import numpy as np
 
 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
 
 # using main logger
 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:
@@ -30,18 +31,23 @@ def VPF_to_behavior(VPF_stream, control_stream):
     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,
+                      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)
 
     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
-        psi += dpsi
-        psi = psi % (2 * np.pi)
+        # psi += dpsi
+        # psi = psi % (2 * np.pi)
 
         if monitoring.SAVE_CONTROL_PARAMS:
 
@@ -64,7 +70,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

visualswarm/contrib/behavior.py

@@ -6,19 +6,11 @@
 # Velocity Parameters
 GAM = 0.2
 V0 = 0
-ALP0 = 0.5
-ALP1 = 0.015
+ALP0 = 0.5  # overall speed scale (limited by possible motor speed)
+ALP1 = 0.015  # ~ 1 / equilibrium distance
 ALP2 = 0
 
 # Heading Vector Parameters
-BET0 = 0.5
+BET0 = 0.3  # overall responsiveness of heading change (turning "speed")
 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

visualswarm/contrib/control.py

@@ -2,5 +2,8 @@
 @author: mezdahun
 @description: Motor Control related parameters
 """
+# Serial port on which the Thymio is available
+THYMIO_DEVICE_PORT = "/dev/ttyACM0"
 
-ENABLE_MOTOR_CONTROL = True
+# Motor scale correction to put the motor scales into the right region
+MOTOR_SCALE_CORRECTION = 100