Merge alpha branch

.github/ISSUE_TEMPLATE/bug_report.md

@@ -0,0 +1,38 @@
+---
+name: Bug report
+about: Create a report to help us improve
+title: ''
+labels: bug
+assignees: ''
+
+---
+
+**Describe the bug**
+A clear and concise description of what the bug is.
+
+**To Reproduce**
+Steps to reproduce the behavior:
+1. Go to '...'
+2. Click on '....'
+3. Scroll down to '....'
+4. See error
+
+**Expected behavior**
+A clear and concise description of what you expected to happen.
+
+**Screenshots**
+If applicable, add screenshots to help explain your problem.
+
+**Desktop (please complete the following information):**
+ - OS: [e.g. iOS]
+ - Browser [e.g. chrome, safari]
+ - Version [e.g. 22]
+
+**Smartphone (please complete the following information):**
+ - Device: [e.g. iPhone6]
+ - OS: [e.g. iOS8.1]
+ - Browser [e.g. stock browser, safari]
+ - Version [e.g. 22]
+
+**Additional context**
+Add any other context about the problem here.

.github/ISSUE_TEMPLATE/feature_request.md

@@ -0,0 +1,20 @@
+---
+name: Feature request
+about: Suggest an idea for this project
+title: ''
+labels: enhancement
+assignees: ''
+
+---
+
+**Is your feature request related to a problem? Please describe.**
+A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+
+**Describe the solution you'd like**
+A clear and concise description of what you want to happen.
+
+**Describe alternatives you've considered**
+A clear and concise description of any alternative solutions or features you've considered.
+
+**Additional context**
+Add any other context or screenshots about the feature request here.

.gitignore

@@ -109,5 +109,15 @@ Drone/test_animation/
 Drone/animation.csv
 Drone/client_logs
 images/
-
+.vscode/
 \.idea/
+
+Drone/_copter_client_old_\.py
+
+Drone/test_cl\.py
+
+Server/testj\.ipynb
+
+Server/tst_client\.py
+
+Server/tst\.py

Drone/FlightLib/FlightLib.py

@@ -22,6 +22,8 @@
 arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
 landing = rospy.ServiceProxy('/land', Trigger)
 
+services_list = [navigate, set_position, set_rates, set_mode, get_telemetry, arming,landing]
+
 logger.info("Proxy services inited")
 
 # globals
@@ -41,9 +43,11 @@
 
 checklist = []
 
-def arming_wrapper(state=False, interrupter=INTERRUPTER):
+
+def arming_wrapper(state=False, *args, **kwargs):
     arming(state)
 
+
 def interrupt():
     logger.info("Performing function interrupt")
     INTERRUPTER.set()
@@ -63,10 +67,8 @@ def check(check_name):
     def inner(f):
         def wrapper(*args, **kwargs):
             failures = f(*args, **kwargs)
-            print(failures)
             msgs = []
             for failure in failures:
-                #print(failure)
                 msg = "[{}]: Failure: {}".format(check_name, failure)
                 msgs.append(msg)
                 logger.warning(msg)
@@ -87,6 +89,16 @@ def _check_nans(*values):
     return any(math.isnan(x) for x in values)
 
 
+@check("Ros services")
+def check_ros_services():
+    timeout = 5.0
+    for service in services_list:
+        try:
+            service.wait_for_service(timeout=timeout)
+        except (rospy.ServiceException, rospy.ROSException) as e:
+            yield ("ROS service {} is not available!".format(service.name))
+
+
 @check("FCU connection")
 def check_connection():
     telemetry = get_telemetry()
@@ -95,7 +107,7 @@ def check_connection():
 
 
 @check("Linear velocity estimation")
-def check_linear_speeds(speed_limit=0.1):
+def check_linear_speeds(speed_limit=0.15):
     telemetry = get_telemetry(frame_id='body')
 
     if _check_nans(telemetry.vx, telemetry.vy, telemetry.vz):
@@ -367,6 +379,7 @@ def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different direc
     if start_telemetry.z < min_z - TOLERANCE:
         logger.warning("Can't do flip! Flip failed!")
         #print("Can't do flip! Flip failed!")
+        return False
     else:                    
         # Flip!    
         set_rates(thrust=1)  # bump up
@@ -383,3 +396,8 @@ def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different direc
         logger.info('Flip succeeded!')
         #print('Flip succeeded!')
         navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw, frame_id=frame_id)   # finish flip
+<<<<<<< HEAD
+=======
+
+        return True
+>>>>>>> alpha

Drone/animation_lib.py

@@ -13,46 +13,51 @@
 
 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(
                 animation_file, delimiter=',', quotechar='|'
             )
             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(
                 animation_file, delimiter=',', quotechar='|'
             )
             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,17 +116,17 @@ 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:
         LedLib.wipe_to(255, 0, 0, interrupter=interrupter)
     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/client.py

@@ -15,6 +15,7 @@
 parent_dir = os.path.dirname(current_dir)
 sys.path.insert(0, parent_dir) 
 
+#logging.basicConfig(level=logging.INFO)
 logger = logging.getLogger(__name__)
 
 import messaging_lib as messaging
@@ -107,7 +108,7 @@ def start(self):
             logger.critical("Caught interrupt, exiting!")
             self.selector.close()
 
-    def _reconnect(self, timeout=2, attempt_limit=5):
+    def _reconnect(self, timeout=3.0, attempt_limit=5):
         logger.info("Trying to connect to {}:{} ...".format(self.server_host, self.server_port))
         attempt_count = 0
         while not self.connected:
@@ -133,7 +134,7 @@ def _reconnect(self, timeout=2, attempt_limit=5):
 
             if attempt_count >= attempt_limit:
                 logger.info("Too many attempts. Trying to get new server IP")
-                self.broadcast_bind()
+                self.broadcast_bind(timeout*2, attempt_limit)
                 attempt_count = 0
 
 
@@ -146,30 +147,42 @@ def _connect(self):
         self._process_connections()
 
 
-    def broadcast_bind(self):
+    def broadcast_bind(self, timeout=3.0, attempt_limit=5):
         broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
         broadcast_client.bind(("", self.broadcast_port))
+        broadcast_client.settimeout(timeout)
+
+        attempt_count = 0
         try:
-            while True:
-                data, addr = broadcast_client.recvfrom(self.BUFFER_SIZE)
-                message = messaging.MessageManager()
-                message.income_raw = data
-                message.process_message()
-                if message.content:
-                    logger.info("Received broadcast message {} from {}".format(message.content, addr))
-                    if message.content["command"] == "server_ip":
-                        args = message.content["args"]
-                        self.server_port = int(args["port"])
-                        self.server_host = args["host"]
-                        self.write_config(False,
-                                          ConfigOption("SERVER", "port", self.server_port),
-                                          ConfigOption("SERVER", "host", self.server_host))
-                        logger.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port))
-                        break
+            while attempt_count <= attempt_limit:
+                try:
+                    data, addr = broadcast_client.recvfrom(self.BUFFER_SIZE)
+                except socket.error as error:
+                    logger.warning("Could not receive broadcast due error: {}".format(error))
+                    attempt_count += 1
+                else:
+                    message = messaging.MessageManager()
+                    message.income_raw = data
+                    message.process_message()
+                    if message.content:
+                        logger.info("Received broadcast message {} from {}".format(message.content, addr))
+                        if message.content["command"] == "server_ip":
+                            args = message.content["args"]
+                            self.server_port = int(args["port"])
+                            self.server_host = args["host"]
+                            self.write_config(False,
+                                              ConfigOption("SERVER", "port", self.server_port),
+                                              ConfigOption("SERVER", "host", self.server_host))
+                            logger.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port))
+                            self.on_broadcast_bind()
+                            break
         finally:
             broadcast_client.close()
 
+    def on_broadcast_bind(self):
+        pass
+
     def _process_connections(self):
         while True:
             events = self.selector.select(timeout=1)