drone: Fix modules from renaming

drone/modules/animation.py

@@ -8,17 +8,20 @@ import rospy
 import logging
 import threading
 
-try:
-    from FlightLib import FlightLib
-except ImportError:
-    print("Can't import FlightLib")
-try:
-    from FlightLib import LedLib
-except ImportError:
-    print("Can't import LedLib")
-
 logger = logging.getLogger(__name__)
 
+# Import flight control
+try:
+    import modules.flight as flight
+except ImportError:
+    logger.debug("Can't import flight control module!")
+
+# Import led control
+try:
+    import modules.led as led
+except ImportError:
+    logger.debug("Can't import led control module for Raspberry Pi!")
+
 interrupt_event = threading.Event()
 
 def moving(f1, f2, delta, x=True, y=True, z=True):
@@ -350,7 +353,7 @@ class Animation(object):
 
 try:
     def execute_frame(frame, frame_id='aruco_map', use_leds=True,
-                    flight_func=FlightLib.navto, auto_arm=False, flight_kwargs=None, interrupter=interrupt_event):
+                    flight_func=flight.navto, auto_arm=False, flight_kwargs=None, interrupter=interrupt_event):
         if flight_kwargs is None:
             flight_kwargs = {}
         if frame.pose_is_valid():
@@ -359,27 +362,27 @@ try:
             logger.debug("Frame pose is not valid for flying")
         if use_leds:
             if frame.get_color:
-                LedLib.fill(*color)
+                led.fill(*color)
 
     def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,
                 interrupter=interrupt_event):
         if use_leds:
-            LedLib.wipe_to(255, 0, 0, interrupter=interrupter)
-        result = FlightLib.takeoff(height=z, timeout_takeoff=timeout, frame_id=frame_id,
+            led.wipe_to(255, 0, 0, interrupter=interrupter)
+        result = flight.takeoff(height=z, timeout_takeoff=timeout, frame_id=frame_id,
                                 emergency_land=safe_takeoff, interrupter=interrupter)
         if result == 'not armed' or result == 'timeout':
             raise Exception('STOP')  # Raise exception to clear task_manager if copter can't arm
         if use_leds:
-            LedLib.blink(0, 255, 0, wait=50, interrupter=interrupter)
+            led.blink(0, 255, 0, wait=50, interrupter=interrupter)
 
 
     def land(z=1.5, descend=False, timeout=5.0, frame_id='aruco_map', use_leds=True,
             interrupter=interrupt_event):
         if use_leds:
-            LedLib.blink(255, 0, 0, interrupter=interrupter)
-        FlightLib.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter)
+            led.blink(255, 0, 0, interrupter=interrupter)
+        flight.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter)
         if use_leds:
-            LedLib.off()
+            led.off()
 
 except NameError:
     print("Can't create flying functions")

drone/modules/client_core.py

@@ -10,14 +10,14 @@ import selectors2 as selectors
 
 from contextlib import closing
 
-import inspect  # Add parent dir to PATH to import messaging_lib
-current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
-parent_dir = os.path.dirname(current_dir)
-sys.path.insert(0, parent_dir)
+# Add parent dir to PATH to import messaging_lib and config_lib
+current_dir = (os.path.dirname(os.path.realpath(__file__)))
+lib_dir = os.path.realpath(os.path.join(current_dir, '../../lib'))
+sys.path.insert(0, lib_dir)
 
 logger = logging.getLogger(__name__)
 
-import messaging_lib as messaging
+import messaging
 from config import ConfigManager
 
 active_client = None  # needs to be refactored: Singleton \ factory callbacks

drone/modules/mavros_wrapper.py

@@ -136,7 +136,7 @@ def load_param_file(px4_file):
     try:
         px4_params = open(px4_file)
     except IOError:
-        logger.error("File {} can't be opened".format(filepath))
+        logger.error("File {} can't be opened".format(px4_file))
         result = False
     else:
         with open(px4_file) as px4_params: