CopterExpress/clever-show
1
0
Fork 0
mirror of https://github.com/CopterExpress/clever-show.git synced 2026-05-26 15:13:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update fork to upstream

Browse Source
This commit is contained in:
Arthur
2019-03-07 18:36:57 +03:00
parent f19832c3cf aa74947a63
commit e0803aa377
4 changed files with 92 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Drone/FlightLib

Submodule Drone/FlightLib updated: 9993d88fdc...701e7aa4e1

42
Drone/client.py
View File

@@ -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 @@ try:
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)))

22
Drone/play_animation.py
View File

@@ -6,14 +6,13 @@ from FlightLib.FlightLib import FlightLib
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 @@ if __name__ == '__main__':
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)

58
Server/server.py
View File

@@ -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 @@ class Client:
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 @@ class Client:
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 @@ class Client:
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 @@ class Client:
@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 @@ drone_list.pack()
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 @@ autoconnect_thread = threading.Thread(target=auto_connect)
autoconnect_thread.daemon = True
autoconnect_thread.start()
broadcast_thread = threading.Thread(target=ip_broadcast, args=(ip, port, ))
if __name__ == '__main__':
try: