Added blocking write to trajectory memory

cflib/crazyflie/mem/trajectory_memory.py

@@ -24,6 +24,7 @@
 import struct
 
 from .memory_element import MemoryElement
+from cflib.utils.callbacks import Syncer
 
 logger = logging.getLogger(__name__)
 
@@ -183,7 +184,7 @@ def write_data(self, write_finished_cb, write_failed_cb=None, start_addr=0x00):
         Write trajectory data to the Crazyflie.
         The trajectory in self.trajectory is written to the Crazyflie.
         By default the trajectory is written to address 0 of the Crazyflie trajectory memory, but it is possible to
-        use a different address. This can be interesting if you want to define more than one trajectory but requires
+        use a different address. This can be interesting if you want to define more than one trajectory but it requires
         careful handling of the addresses to avoid overwriting trajectories and staying within the trajectory memory.
 
         @param write_finished_cb A callback that is called when the write trajectory is uploaded.
@@ -201,6 +202,19 @@ def write_data(self, write_finished_cb, write_failed_cb=None, start_addr=0x00):
         self.mem_handler.write(self, start_addr, data, flush_queue=True)
         return len(data)
 
+    def write_data_sync(self, start_addr=0x00):
+        """
+        Same functionality as write_data() but synchronous (blocking)
+
+        Args:
+            start_addr (hexadecimal, optional): The address in the trajectory memory to upload the trajectory to.
+            Defaults to 0x00.
+        """
+        syncer = Syncer()
+        self.write_data(syncer.success_cb, write_failed_cb=syncer.failure_cb, start_addr=start_addr)
+        syncer.wait()
+        return syncer.is_success
+
     def write_done(self, mem, addr):
         if self._write_finished_cb and mem.id == self.id:
             logger.debug('Write trajectory data done')

examples/autonomy/autonomous_sequence_high_level.py

@@ -62,31 +62,6 @@
 ]
 
 
-class Uploader:
-    def __init__(self):
-        self._is_done = False
-        self._success = True
-
-    def upload(self, trajectory_mem):
-        print('Uploading data')
-        trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed)
-
-        while not self._is_done:
-            time.sleep(0.2)
-
-        return self._success
-
-    def _upload_done(self, mem, addr):
-        print('Data uploaded')
-        self._is_done = True
-        self._success = True
-
-    def _upload_failed(self, mem, addr):
-        print('Data upload failed')
-        self._is_done = True
-        self._success = False
-
-
 def wait_for_position_estimator(scf):
     print('Waiting for estimator to find position...')
 
@@ -158,7 +133,7 @@ def upload_trajectory(cf, trajectory_id, trajectory):
         trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw))
         total_duration += duration
 
-    upload_result = Uploader().upload(trajectory_mem)
+    upload_result = trajectory_mem.write_data_sync()
     if not upload_result:
         print('Upload failed, aborting!')
         sys.exit(1)

examples/autonomy/autonomous_sequence_high_level_compressed.py

@@ -58,31 +58,6 @@
 ]
 
 
-class Uploader:
-    def __init__(self):
-        self._is_done = False
-        self._success = True
-
-    def upload(self, trajectory_mem):
-        print('Uploading data')
-        trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed)
-
-        while not self._is_done:
-            time.sleep(0.2)
-
-        return self._success
-
-    def _upload_done(self, mem, addr):
-        print('Data uploaded')
-        self._is_done = True
-        self._success = True
-
-    def _upload_failed(self, mem, addr):
-        print('Data upload failed')
-        self._is_done = True
-        self._success = False
-
-
 def wait_for_position_estimator(scf):
     print('Waiting for estimator to find position...')
 
@@ -145,7 +120,7 @@ def upload_trajectory(cf, trajectory_id, trajectory):
 
     trajectory_mem.trajectory = trajectory
 
-    upload_result = Uploader().upload(trajectory_mem)
+    upload_result = trajectory_mem.write_data_sync()
     if not upload_result:
         print('Upload failed, aborting!')
         sys.exit(1)

examples/qualisys/qualisys_hl_commander.py

@@ -154,31 +154,6 @@ async def _close(self):
         self.connection.disconnect()
 
 
-class Uploader:
-    def __init__(self):
-        self._is_done = False
-        self._success = True
-
-    def upload(self, trajectory_mem):
-        print('Uploading data')
-        trajectory_mem.write_data(self._upload_done, write_failed_cb=self._upload_failed)
-
-        while not self._is_done:
-            time.sleep(0.2)
-
-        return self._success
-
-    def _upload_done(self, mem, addr):
-        print('Data uploaded')
-        self._is_done = True
-        self._success = True
-
-    def _upload_failed(self, mem, addr):
-        print('Data upload failed')
-        self._is_done = True
-        self._success = False
-
-
 def wait_for_position_estimator(scf):
     print('Waiting for estimator to find position...')
 
@@ -287,7 +262,7 @@ def upload_trajectory(cf, trajectory_id, trajectory):
         trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw))
         total_duration += duration
 
-    Uploader().upload(trajectory_mem)
+    trajectory_mem.write_data_sync()
     cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.trajectory))
     return total_duration