Code reformatting to match PEP, resume command now accepts time

Drone/animation_lib.py

@@ -13,16 +13,20 @@ logger = logging.getLogger(__name__)
 
 interrupt_event = threading.Event()
 
-id = "Empty id"
+anim_id = "Empty id"
+
+# TODO refactor as class
+# TODO separate code for frames transformations (e.g. for gps)
+
 
 def get_id(filepath="animation.csv"):
-    global id
+    global anim_id
     try:
         animation_file = open(filepath)
     except IOError:
         logging.error("File {} can't be opened".format(filepath))
-        id = "No animation"
-        return id
+        anim_id = "No animation"
+        return anim_id
     else:
         with animation_file:
             csv_reader = csv.reader(
@@ -30,20 +34,21 @@ def get_id(filepath="animation.csv"):
             )
             row_0 = csv_reader.next()
             if len(row_0) == 1:
-                id = row_0[0]
-                print("Got animation_id: {}".format(id))
+                anim_id = row_0[0]
+                print("Got animation_id: {}".format(anim_id))
             else:
                 print("No animation id in file")
-    return id
+    return anim_id
+
 
 def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
     imported_frames = []
-    global id
+    global anim_id
     try:
         animation_file = open(filepath)
     except IOError:
         logging.error("File {} can't be opened".format(filepath))
-        id = "No animation"
+        anim_id = "No animation"
     else:
         with animation_file:
             csv_reader = csv.reader(
@@ -51,8 +56,8 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
             )
             row_0 = csv_reader.next()
             if len(row_0) == 1:
-                id = row_0[0]
-                print("Got animation_id: {}".format(id))
+                anim_id = row_0[0]
+                print("Got animation_id: {}".format(anim_id))
             else:
                 print("No animation id in file")
                 frame_number, x, y, z, yaw, red, green, blue = row_0
@@ -87,7 +92,6 @@ def convert_frame(frame):
 
 def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
                   flight_func=FlightLib.navto, flight_kwargs=None, interrupter=interrupt_event):
-
     if flight_kwargs is None:
         flight_kwargs = {}
 
@@ -112,7 +116,7 @@ def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True,
         tasking.wait(next_frame_time, interrupter)
 
 
-def takeoff(z=1.5, safe_takeoff=True, frame_id = 'map', timeout=5.0, use_leds=True,
+def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,
             interrupter=interrupt_event):
     print(interrupter.is_set())
     if use_leds:
@@ -120,9 +124,9 @@ def takeoff(z=1.5, safe_takeoff=True, frame_id = 'map', timeout=5.0, use_leds=Tr
     if interrupter.is_set():
         return
     result = FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff,
-                      interrupter=interrupter)
+                               interrupter=interrupter)
     if result == 'not armed':
-        raise Exception('STOP')         # Raise exception to clear task_manager if copter can't arm
+        raise Exception('STOP')  # Raise exception to clear task_manager if copter can't arm
     if interrupter.is_set():
         return
     if use_leds:

Drone/copter_client.py

@@ -12,7 +12,7 @@ import messaging_lib as messaging
 import tasking_lib as tasking
 import animation_lib as animation
 
-#logging.basicConfig(  # TODO all prints as logs
+# logging.basicConfig(  # TODO all prints as logs
 #    level=logging.DEBUG, # INFO
 #    format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
 #    handlers=[
@@ -21,7 +21,8 @@ import animation_lib as animation
 
 logger = logging.getLogger(__name__)
 
-#import ros_logging
+
+# import ros_logging
 
 class CopterClient(client.Client):
     def load_config(self):
@@ -45,7 +46,7 @@ class CopterClient(client.Client):
         rospy.init_node('Swarm_client', anonymous=True)
         if self.USE_LEDS:
             LedLib.init_led(self.LED_PIN)
-        
+
         task_manager_instance.start()
 
         super(CopterClient, self).start()
@@ -56,10 +57,12 @@ def _response_selfcheck():
     check = FlightLib.selfcheck()
     return check if check else "OK"
 
+
 @messaging.request_callback("anim_id")
 def _response_animation_id():
     return animation.get_id()
 
+
 @messaging.request_callback("batt_voltage")
 def _response_batt():
     return FlightLib.get_telemetry('body').voltage
@@ -69,47 +72,50 @@ def _response_batt():
 def _response_cell():
     return FlightLib.get_telemetry('body').cell_voltage
 
+
 @messaging.message_callback("test")
 def _command_test(**kwargs):
     print("test")
 
+
 @messaging.message_callback("service_restart")
 def _command_service_restart(**kwargs):
     os.system("systemctl restart" + kwargs["name"])
 
 
 @messaging.message_callback("led_test")
-def _command_led_test(*args, **kwargs):
+def _command_led_test(**kwargs):
     LedLib.chase(255, 255, 255)
     time.sleep(2)
     LedLib.off()
 
+
 @messaging.message_callback("led_fill")
-def _command_emergency_led_fill(**kwargs):
+def _command_led_fill(**kwargs):
     r = g = b = 0
-    
+
     try:
         r = kwargs["red"]
     except KeyError:
         pass
-    
+
     try:
         g = kwargs["green"]
     except KeyError:
         pass
-    try:    
+    try:
         b = kwargs["blue"]
-    except KeyError: 
+    except KeyError:
         pass
-    
-    LedLib.fill(r, g, b)
 
+    LedLib.fill(r, g, b)
 
 
 @messaging.message_callback("flip")
 def _copter_flip():
     FlightLib.flip()
 
+
 @messaging.message_callback("takeoff")
 def _command_takeoff(**kwargs):
     task_manager.add_task(time.time(), 0, animation.takeoff,
@@ -139,10 +145,10 @@ def _command_land(**kwargs):
 def _command_disarm(**kwargs):
     task_manager.reset()
     task_manager.add_task(-5, 0, FlightLib.arming_wrapper,
-                        task_kwargs={
-                            "state": False
-                        }
-    )
+                          task_kwargs={
+                              "state": False
+                          }
+                          )
 
 
 @messaging.message_callback("stop")
@@ -157,18 +163,18 @@ def _command_stop(**kwargs):
 
 @messaging.message_callback("resume")
 def _command_stop(**kwargs):
-    task_manager.resume()
+    task_manager.resume(time_to_start_next_task=kwargs.get("time", 0))
 
 
 @messaging.message_callback("start")
 def _play_animation(**kwargs):
     start_time = float(kwargs["time"])  # TODO
 
-    if (animation.get_id() == 'No animation'):
+    if animation.get_id() == 'No animation':
         print("Can't start animation without animation file!")
         return
 
-    print("Start time = {}, wait for {} seconds".format(start_time, time.time()-start_time))
+    print("Start time = {}, wait for {} seconds".format(start_time, time.time() - start_time))
 
     frames = animation.load_animation(os.path.abspath("animation.csv"),
                                       x0=client.active_client.X0 + client.active_client.X0_COMMON,
@@ -176,59 +182,58 @@ def _play_animation(**kwargs):
                                       )
 
     task_manager.add_task(start_time, 0, animation.takeoff,
-                        task_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,
-                            "use_leds": client.active_client.USE_LEDS,
-                        }
-                        )
+                          task_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,
+                              "use_leds": client.active_client.USE_LEDS,
+                          }
+                          )
 
     rfp_time = start_time + client.active_client.TAKEOFF_TIME
     task_manager.add_task(rfp_time, 0, animation.execute_frame,
-                        task_kwargs={
-                            "point": animation.convert_frame(frames[0])[0],
-                            "color": animation.convert_frame(frames[0])[1],
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                            "flight_func": FlightLib.reach_point,
-                        }
-                        )
+                          task_kwargs={
+                              "point": animation.convert_frame(frames[0])[0],
+                              "color": animation.convert_frame(frames[0])[1],
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                              "flight_func": FlightLib.reach_point,
+                          }
+                          )
 
     frame_time = rfp_time + client.active_client.RFP_TIME
     frame_delay = 0.125  # TODO from animation file
     for frame in frames:
+        point, color = animation.convert_frame(frame)  # TODO add param to calculate delta
         task_manager.add_task(frame_time, 0, animation.execute_frame,
-                        task_kwargs={
-                            "point": animation.convert_frame(frame)[0],
-                            "color": animation.convert_frame(frame)[1],
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                            "flight_func": FlightLib.navto,
-                            }
-                        )
+                              task_kwargs={
+                                  "point": point,
+                                  "color": color,
+                                  "frame_id": client.active_client.FRAME_ID,
+                                  "use_leds": client.active_client.USE_LEDS,
+                                  "flight_func": FlightLib.navto,
+                              }
+                              )
         frame_time += frame_delay
 
     land_time = frame_time + client.active_client.LAND_TIME
     task_manager.add_task(land_time, 0, animation.land,
-                        task_kwargs={
-                            "timeout": client.active_client.TAKEOFF_TIME,
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                        },
-                        )
+                          task_kwargs={
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          },
+                          )
 
 
 if __name__ == "__main__":
-    
     copter_client = CopterClient()
     task_manager = tasking.TaskManager()
-    
+
     copter_client.start(task_manager)
 
-    #ros_logging.route_logger_to_ros()
-    #ros_logging.route_logger_to_ros("__main__")
-    #ros_logging.route_logger_to_ros("client")
-    #ros_logging.route_logger_to_ros("messaging")
-
+    # ros_logging.route_logger_to_ros()
+    # ros_logging.route_logger_to_ros("__main__")
+    # ros_logging.route_logger_to_ros("client")
+    # ros_logging.route_logger_to_ros("messaging")

Drone/tasking_lib.py

@@ -10,7 +10,9 @@ Task = collections.namedtuple("Task", ["func", "args", "kwargs", "delayable", ])
 
 INTERRUPTER = threading.Event()
 
-def wait(end, interrupter=INTERRUPTER, maxsleep=0.1):  # Added features to interrupter sleep and set max sleeping interval
+
+def wait(end, interrupter=INTERRUPTER, maxsleep=0.1):
+    # Added features to interrupter sleep and set max sleeping interval
 
     while not interrupter.is_set():  # Basic implementation of pause module until()
         now = time.time()
@@ -83,7 +85,7 @@ class TaskManager(object):
                 return heapq.heappop(self.task_queue)
             raise KeyError('Pop from an empty priority queue')
 
-    def start(self, timeouts=False):
+    def start(self):
         #print("Task manager is started")
         #logger.info("Task manager is started")
         self._processor_thread.start()
@@ -95,13 +97,13 @@ class TaskManager(object):
         with self._task_queue_lock:
             del self.task_queue[:]
 
-    def shutdown(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()
-        self._processor_thread.join(timeout=5)
+        # 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):
         if interrupt:
@@ -111,7 +113,7 @@ class TaskManager(object):
         #logger.info("Task queue paused")
         #print("Task queue paused")
 
-    def resume(self, time_to_start_next_task = 0):
+    def resume(self, time_to_start_next_task=0.0):
         if self.task_queue:
             next_task_time = self.task_queue[0][0]
             if time_to_start_next_task > next_task_time: