tasking_lib: handle reading first value from task queue

Drone/copter_client.py

@@ -205,7 +205,7 @@ def _command_disarm(**kwargs):
 
 @messaging.message_callback("stop")
 def _command_stop(**kwargs):
-    task_manager.stop()
+    task_manager.reset()
 
 
 @messaging.message_callback("pause")
@@ -226,7 +226,9 @@ def _play_animation(**kwargs):
         print("Can't start animation without animation file!")
         return
 
-    print("Start time = {}, wait for {} seconds".format(start_time, time.time() - start_time))
+    task_manager.reset(interrupt_next_task=False)
+
+    print("Start time = {}, wait for {} seconds".format(start_time, start_time-time.time()))
     # Load animation
     frames = animation.load_animation(os.path.abspath("animation.csv"),
                                         x0=client.active_client.X0 + client.active_client.X0_COMMON,
@@ -245,7 +247,7 @@ def _play_animation(**kwargs):
                                 "z": client.active_client.TAKEOFF_HEIGHT,
                                 "timeout": client.active_client.TAKEOFF_TIME,
                                 "safe_takeoff": client.active_client.SAFE_TAKEOFF,
-                                "frame_id": client.active_client.FRAME_ID,
+                                # "frame_id": client.active_client.FRAME_ID,
                                 "use_leds": client.active_client.USE_LEDS,
                             }
                             )
@@ -273,8 +275,7 @@ def _play_animation(**kwargs):
                             }
                             )
         # Calculate first frame start time
-        frame_time = start_time + 0.5 # TODO Think about arming time
-
+        frame_time = start_time + 0.5 # TODO Think about arming time   
     # Play animation file
     for frame in corrected_frames:
         point, color, yaw = animation.convert_frame(frame)
@@ -299,6 +300,7 @@ def _play_animation(**kwargs):
                         "use_leds": client.active_client.USE_LEDS,
                     },
                     )
+    #print(task_manager.task_queue)
 
 
 

Drone/tasking_lib.py

@@ -77,7 +77,7 @@ def add_task(self, timestamp, priority, task_function,
         self._wait_interrupt_event.clear()
         self._running_event.set()
 
-        #print(self.task_queue)
+        # #print(self.task_queue)
 
     def pop_task(self):
         with self._task_queue_lock:
@@ -100,9 +100,6 @@ def stop(self):
     def shutdown(self, timeout=5.0):
         self.stop()
         self._shutdown_event.set()
-        # self._wait_interrupt_event.set()
-        # self._task_interrupt_event.set()
-        # self._running_event.clear() #already exists in pause
         self._processor_thread.join(timeout=timeout)
 
     def pause(self, interrupt=True):
@@ -118,33 +115,39 @@ def resume(self, time_to_start_next_task=0.0):
             next_task_time = self.task_queue[0][0]
             if time_to_start_next_task > next_task_time:
                 self._timeshift = time_to_start_next_task - next_task_time 
-        self._running_event.set()
         self._wait_interrupt_event.clear()
         self._task_interrupt_event.clear()
+        self._running_event.set()
         #logger.info("Task queue resumed with timeshift {}".format(self._timeshift))
         #print("Task queue resumed with timeshift {}".format(self._timeshift))
 
-    def reset(self):
+    def reset(self, interrupt_next_task=True):
         self.stop()
         self.resume()
-        self._reset_event.set()
+        if interrupt_next_task:
+            self._reset_event.set()
 
     def execute_task(self):
         with self._task_queue_lock:
-            if self.task_queue:
+            try:
                 start_time, priority, count, task = self.task_queue[0]
-            else: 
+            except Exception as e:
+                #print("Task queue checking exception: {}".format(e))
                 self._timeshift = 0.0
+                self._wait_interrupt_event.clear()
+                self._task_interrupt_event.clear()
+                self._running_event.clear()
                 return
 
         task_start_time = start_time + self._timeshift
         #logger.info("Waiting util task execution time:{}".format(task_start_time))
-        #print("Waiting util task execution time:{}".format(task_start_time))
+        #print("Waiting until task execution time:{}".format(task_start_time))
         wait(task_start_time, self._wait_interrupt_event)
 
         if not self._wait_interrupt_event.is_set():
             #logger.info("Executing task {}".format(task))
-            #print("Executing task {}".format(task))
+            #print("{} Executing task {}".format(time.time(),task))
+            #print("Interrupter is set: {}".format(self._task_interrupt_event.is_set()))
             try:
                 task.func(*task.args, interrupter=self._task_interrupt_event, **task.kwargs)
             except Exception as e:
@@ -160,13 +163,21 @@ def execute_task(self):
             return
 
         if time.time() > start_time:
-            start_time_n, priority_n, count_n, task_n = self.task_queue[0]
+            try:
+                start_time_n, priority_n, count_n, task_n = self.task_queue[0]
+            except Exception as e:
+                #print("Timeout checking exception: {}".format(e))
+                self._timeshift = 0.0
+                self._wait_interrupt_event.clear()
+                self._task_interrupt_event.clear()
+                self._running_event.clear()
+                return
             if (task_n == task) and (start_time_n == start_time):
                 self.pop_task()
-            #try:
-                #print("Pop {} function!".format(task.func.__name__))
-            #except Exception as e:
-                #print("Pop something!") 
+                #try:
+                    #print("Pop {} function!".format(task.func.__name__))
+                #except Exception as e:
+                    #print("Pop something!") 
 
         if self._task_interrupt_event.is_set():
             self._task_interrupt_event.clear()