New commands for server+client, server ip autosearch, little improvem…

…ents

Drone/client.py

@@ -31,19 +31,36 @@ def get_ntp_time(host, port):
     return unpacked[10] + float(unpacked[11]) / 2**32 - NTP_DELTA
 
 
-def reconnect(t=1):
+def reconnect(t=2):
+    global clientSocket, host, port
     print("Trying to connect to", host, ":", port, "...")
     connected = False
-    global clientSocket
+    attempt_count = 0
     while not connected:
+        print("Waiting for connection, attempt", attempt_count)
         try:
             clientSocket = socket.socket()
+            clientSocket.settimeout(3)
             clientSocket.connect((host, port))
             connected = True
             print("Connection successful")
         except socket.error as e:
             print("Waiting for connection:", e)
             time.sleep(t)
+        attempt_count +=1
+        if attempt_count >= 15:
+            print("Too many attempts. Trying to get new server IP")
+            broadcst_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            broadcst_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+            broadcst_client.bind(("", 8181))
+            while True:
+                data, addr = broadcst_client.recvfrom(1024)
+                print("Recieved broadcast message %s from %s"%(data, addr))
+                if parse_command(data.decode("UTF-8"))[0] == "server_ip":
+                    print("Binding to new IP: ", addr)
+                    host, port = addr
+                    broadcst_client.close()
+                    break
 
 
 def send_all(msg):
@@ -100,16 +117,15 @@ def write_to_config(section, option, value):
 
 def animation_player(running_event, stop_event):
     print("Animation thread activated")
-    rate = rospy.Rate(1000 / 100)
-    # first_frame = play_animation.get_frames()[0]
-    # play_animation.takeoff(round(float(first_frame['x']), 4), round(float(first_frame['y']), 4), round(float(first_frame['z']), 4))
+    frames = play_animation.read_animation_file()
+    rate = rospy.Rate(1000 / 125)
     play_animation.takeoff()
-    for current_frame in play_animation.get_frames():
+    for frame in frames:
         running_event.wait()
         if stop_event.is_set():
             break
 
-        play_animation.do_next_animation(current_frame)
+        play_animation.animate_frame(frame)
         rate.sleep()
     else:
         play_animation.land()
@@ -200,16 +216,22 @@ def stop_animation():
                     dt = starttime - get_ntp_time(NTP_HOST, NTP_PORT)
                     print("Until start:", dt)
                     rospy.Timer(rospy.Duration(dt), start_animation, oneshot=True)
+                elif command == 'takeoff':
+                    play_animation.takeoff()
                 elif command == 'stop':
                     stop_animation()
-                    #FlightLib.takeoff(2)
                     #FlightLib.reach(5, 5, 2)
+                elif command == 'land':
+                    FlightLib.land1()  # TODO dont forget change back to land
+                elif command == 'disarm':
+                    FlightLib.arming(False)
+
                 elif command == 'request':
                     request_target = args[0]
                     print("Got request for:", request_target)
                     response = ""
-                    if request_target == 'someshit':
-                        response = "dont_have_any"
+                    if request_target == 'test':
+                        response = "test_succsess"
                     elif request_target == 'id':
                         response = COPTER_ID
                     send_all(bytes(form_command("response", response)))

Drone/play_animation.py

@@ -6,14 +6,13 @@
 from FlightLib.FlightLib import LedLib
 
 animation_file_path = 'test_animation/test_1.csv'
-frames = []
 USE_LEDS = True
 
 
 def takeoff():
     if USE_LEDS:
         LedLib.wipe_to(255, 0, 0)
-    FlightLib.takeoff1()
+    FlightLib.takeoff1()  # TODO dont forget change back to takeoff
 
 
 def land():
@@ -24,20 +23,21 @@ def land():
         LedLib.off()
 
 
-def do_next_animation(current_frame, x0 = 0, y0 = 0):
-    FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw = 1.57)
+def animate_frame(current_frame, x0=0.0, y0=0.0):
+    FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw=1.57)
     if USE_LEDS:
         LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue'])
 
 
 def read_animation_file(filepath=animation_file_path):
+    imporetd_frames = []
     with open(filepath) as animation_file:
         csv_reader = csv.reader(
             animation_file, delimiter=',', quotechar='|'
         )
         for row in csv_reader:
             frame_number, x, y, z, yaw, red, green, blue = row
-            frames.append({
+            imporetd_frames.append({
                 'number': int(frame_number),
                 'x': float(x),
                 'y': float(y),
@@ -47,11 +47,7 @@ def read_animation_file(filepath=animation_file_path):
                 'green': int(green),
                 'blue': int(blue),
             })
-
-
-def get_frames():
-    global frames
-    return frames
+    return imporetd_frames
 
 
 if __name__ == '__main__':
@@ -60,12 +56,12 @@ def get_frames():
         LedLib.init_led()
     X0 = 0.5
     Y0 = 1.0
-    read_animation_file()
+    frames = read_animation_file()
     rate = rospy.Rate(8)
     takeoff()
-    FlightLib.reach(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw = 1.57)
+    FlightLib.reach(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw=11.57)
     for frame in frames:
-        do_next_animation(frame, x0 = X0, y0 = Y0)
+        animate_frame(frame, x0=X0, y0=Y0)
         rate.sleep()
     land()
     time.sleep(1)

Server/server.py

@@ -37,6 +37,17 @@ def auto_connect():
         Client.clients[addr[0]].connect(c, addr)
 
 
+def ip_broadcast(ip):
+    ip = ip
+    broadcast_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+    broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+    while True:
+        msg = bytes(Client.form_command("server_ip ", ip), "UTF-8")
+        broadcast_sock.sendto(msg, ('255.255.255.255', 8181))  #TODO to config
+        time.sleep(5)
+
+
 NTP_DELTA = 2208988800  # 1970-01-01 00:00:00
 NTP_QUERY = b'\x1b' + bytes(47)
 
@@ -77,7 +88,7 @@ def __init__(self, ip):
 
     def connect(self, client_socket, client_addr):
         print("Client connected")
-        self._send_queue = collections.deque()  # comment for resuming queue after reconnection
+        # self._send_queue = collections.deque()  # comment for resuming queue after reconnection
 
         self.socket = client_socket
         self.addr = client_addr
@@ -117,7 +128,12 @@ def _run(self):
                 if self._send_queue:
                     msg = self._send_queue.popleft()
                     print("Send", msg, "to", self.addr)
-                    self._send_all(msg)
+                    try:
+                        self._send_all(msg)
+                    except socket.error as e:
+                        print("Attempt to send failed")
+                        self._send_queue.appendleft(msg)
+                        raise e
                 else:
                     msg = "ping"
                     # self._send_all(msg)
@@ -142,7 +158,7 @@ def _run(self):
                     pass
 
             except socket.error as e:
-                print("Client error, disconnected", e)
+                print("Client error: {}, disconnected".format(e))
                 self.connected = False
                 self.socket.close()
                 break
@@ -165,9 +181,12 @@ def send(self, *messages):
 
     @staticmethod
     def broadcast(message, force_all=False):
-        for client in Client.clients.values():
-            if (not client.malfunction) or force_all:
-                client.send(message)
+        if Client.clients:
+            for client in Client.clients.values():
+                if (not client.malfunction) or force_all:
+                    client.send(message)
+        else:
+            print("No clients were connected!")
 
     @requires_connect
     def send_file(self, filepath, dest_filename):
@@ -198,6 +217,18 @@ def stop_swarm():
     Client.broadcast("stop")  # для тестирования
 
 
+def land_all():
+    Client.broadcast("land")
+
+
+def disarm_all():
+    Client.broadcast("disarm")
+
+
+def takeoff_all():
+    Client.broadcast("takeoff")
+
+
 def send_animations():
     path = filedialog.askdirectory(title="Animation directory")
     if path:
@@ -245,14 +276,24 @@ def send_starttime(dt=15):
 button_frame = Frame(leftFrame, borderwidth=1, relief="solid")
 button_frame.pack(fill=BOTH, expand=True)
 
+land_all_btn = ttk.Button(button_frame, text="Disarm all", command=disarm_all)
+land_all_btn.pack(side=RIGHT, padx=5, pady=5)
+
+land_all_btn = ttk.Button(button_frame, text="Land all", command=land_all)
+land_all_btn.pack(side=RIGHT, padx=5, pady=5)
+
 stop_all_btn = ttk.Button(button_frame, text="Stop swarm", command=stop_swarm)
 stop_all_btn.pack(side=RIGHT, padx=5, pady=5)
 
+
 send_animation_btn = ttk.Button(button_frame, text="Send animations", command=send_animations)
-send_animation_btn.pack(side=RIGHT, padx=5, pady=5)
+send_animation_btn.pack(side=LEFT, padx=5, pady=5)
+
+send_starttime_btn = Button(button_frame, bg='red', text="Takeoff all", command=takeoff_all)
+send_starttime_btn.pack(side=LEFT, padx=5, pady=5)
 
 send_starttime_btn = ttk.Button(button_frame, text="Start animation after...", command=send_starttime)
-send_starttime_btn.pack(side=RIGHT, padx=5, pady=5)
+send_starttime_btn.pack(side=LEFT, padx=5, pady=5)
 
 
 def gui_update():
@@ -283,6 +324,7 @@ def gui_update():
 autoconnect_thread.daemon = True
 autoconnect_thread.start()
 
+broadcast_thread = threading.Thread(target=ip_broadcast, args=(ip, port, ))
 
 if __name__ == '__main__':
     try: