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

New FlightLib!(better functions+logging+selfcheck) (everything needs testing)

Browse Source 
Configuration files changed
This commit is contained in:
Artem30801
2019-03-14 18:34:17 +03:00
parent 12644c554b
commit aa7748a319
7 changed files with 350 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

270
Drone/FlightLib2/FlightLib.py Normal file
View File

@@ -0,0 +1,270 @@
#!/usr/bin/python
from __future__ import print_function
import sys
import math
import time
import logging
import threading
import rospy
from clever import srv
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool
from std_srvs.srv import Trigger
module_logger = logging.getLogger("FlightLib.FlightLib")
# create proxy service
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
set_rates = rospy.ServiceProxy('/set_rates', srv.SetRates)
set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
landing = rospy.ServiceProxy('/land', Trigger)
module_logger.info("Proxy services inited")
# globals
FREQUENCY = 1000/25 # HZ
TOLERANCE = 0.2
interrupt_event = threading.Event()
def interrupt():
module_logger.info("Performing function interrupt")
interrupt_event.set()
def init(node_name="CleverSwarmFlight", anon=True, no_signals=True):
module_logger.info("Initing ROS node")
rospy.init_node(node_name, anonymous=anon, disable_signals=no_signals)
module_logger.info("Ros node inited")
def get_distance3d(x1, y1, z1, x2, y2, z2):
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
def check(check_name):
def inner(f):
def wrapper(*args, **kwargs):
result, failures = f(*args, **kwargs)
if failures:
msgs = []
for failure in failures:
msg = "[{}]: Failure: {}".format(check_name, failure)
msgs.append(msg)
module_logger.warning(msg)
return msgs
else:
module_logger.info("[{}]: OK".format(check_name))
return None
return wrapper
return inner
@check("Linear velocity estimation")
def check_linear_speeds():
failures = []
telemetry = get_telemetry(frame_id='body')
speed_limit = 0.1
if telemetry.vx >= speed_limit:
failures.append("X velocity estimation: {:.3f} m/s".format(telemetry.vx))
if telemetry.vy >= speed_limit:
failures.append("Y velocity estimation: {:.3f} m/s".format(telemetry.vy))
if telemetry.vz >= speed_limit:
failures.append("Z velocity estimation: {:.3f} m/s".format(telemetry.vz))
return failures
@check("Angular velocity estimation")
def check_angular_speeds():
failures = []
telemetry = get_telemetry(frame_id='body')
rate_limit = 0.05
if telemetry.pitch_rate >= rate_limit:
failures.append("Pitch rate estimation: {:.3f} rad/s".format(telemetry.pitch_rate))
if telemetry.roll_rate >= rate_limit:
failures.append("Roll rate estimation: {:.3f} rad/s".format(telemetry.roll_rate))
if telemetry.yaw_rate >= rate_limit:
failures.append("Yaw rate estimation: {:.3f} rad/s".format(telemetry.yaw_rate))
return failures
@check("Angles estimation")
def check_angles():
failures = []
telemetry = get_telemetry(frame_id='body')
angle_limit = math.radians(1)
if abs(telemetry.pitch) >= angle_limit:
failures.append("Pitch estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.pitch,
math.degrees(telemetry.pitch)))
if abs(telemetry.roll) >= angle_limit:
failures.append("Roll estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.roll,
math.degrees(telemetry.roll)))
if abs(telemetry.yaw) >= angle_limit:
failures.append("Yaw estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.yaw,
math.degrees(telemetry.yaw)))
return failures
def selfcheck():
msgs = []
msgs.extend(check_linear_speeds())
msgs.extend(check_angular_speeds())
msgs.extend(check_angles())
return msgs
def navto(x, y, z, yaw=float('nan'), frame_id='aruco_map'):
set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw)
telemetry = get_telemetry(frame_id=frame_id)
module_logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
module_logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z))
return True
def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map',
freq=FREQUENCY, timeout=5000, wait=False):
module_logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed)
# waiting for completion
telemetry = get_telemetry(frame_id=frame_id)
rate = rospy.Rate(freq)
time_start = rospy.get_rostime()
while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait:
if interrupt_event.is_set():
module_logger.warning("Flight function interrupted!")
interrupt_event.clear()
break
telemetry = get_telemetry(frame_id=frame_id)
module_logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000
if timeout is not None:
if time_passed >= timeout:
module_logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed / 1000))
return wait
rate.sleep()
else:
module_logger.info("Point reached!")
return True
def reach_attitude(z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map',
freq=FREQUENCY, timeout=5000, wait=False):
module_logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
current_telem = get_telemetry(frame_id=frame_id)
navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.xy, z=z, yaw=yaw, speed=speed)
# waiting for completion
telemetry = get_telemetry(frame_id=frame_id)
rate = rospy.Rate(freq)
time_start = rospy.get_rostime()
while (abs(z - telemetry.z) > tolerance) or wait:
if interrupt_event.is_set():
module_logger.warning("Flight function interrupted!")
interrupt_event.clear()
break
telemetry = get_telemetry(frame_id=frame_id)
module_logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000
if timeout is not None:
if time_passed >= timeout:
module_logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed / 1000))
return wait
else:
return True
rate.sleep()
else:
module_logger.info("Attitude reached!")
return True
def land(descend=True, z=1.0, frame_id_descend="aruco_map", frame_id_land="aruco_map",
timeout_descend=5000, timeout_land=7500, freq=FREQUENCY):
if descend:
module_logger.info("Descending to: | z: {:.3f}".format(z))
reach_attitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq)
landing()
telemetry = get_telemetry(frame_id='aruco_map')
rate = rospy.Rate(1000 / wait_ms)
time_start = rospy.get_rostime()
while telemetry.armed:
if interrupt_event.is_set():
module_logger.warning("Flight function interrupted!")
interrupt_event.clear()
break
telemetry = get_telemetry(frame_id='aruco_map')
module_logger.info("Landing...")
time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000
if timeout_land is not None:
if time_passed >= timeout_land:
module_logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed / 1000))
module_logger.warning("Disarming!")
arming(False)
return False
rate.sleep()
else:
module_logger.info("Landing succeeded!")
return True
def takeoff(z=1.0, speed=0.5, frame_id='body', freq=FREQUENCY,
timeout_arm=750, timeout_takeoff=5000, wait=False, emergency_land=True):
module_logger.info("Starting takeoff!")
module_logger.info("Arming, going to OFFBOARD mode")
navigate(frame_id=frame_id_takeoff, speed=speed_takeoff, auto_arm=True)
telemetry = get_telemetry(frame_id=frame_id)
rate = rospy.Rate(freq)
time_start = rospy.get_rostime()
while (not telemetry.armed) or wait:
if interrupt_event.is_set():
module_logger.warning("Flight function interrupted!")
interrupt_event.clear()
return None
telemetry = get_telemetry(frame_id=frame_id_takeoff)
module_logger.info("Arming...")
time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000
if timeout is not None:
if time_passed >= timeout_arm:
module_logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed / 1000))
return False
rate.sleep()
module_logger.info("Armed!")
result = reach_point(z=z, speed=speed, frame_id=frame_id_takeoff, timeout=timeout_takeoff, freq=freq, wait=wait)
if result:
module_logger.info("Takeoff succeeded!")
else:
module_logger.warning("Takeoff navigation failed")
if emergency_land:
module_logger.info("Preforming emergency land")
land(descend=False)
return result

1
Drone/FlightLib2/__init__.py Normal file
View File

@@ -0,0 +1 @@

61
Drone/client.py
View File

@@ -5,19 +5,29 @@ import struct
import random
import time
import errno
import logging
import threading
import ConfigParser
from contextlib import closing
import rospy
from FlightLib.FlightLib import FlightLib
from FlightLib.FlightLib import LedLib
#from FlightLib.FlightLib import FlightLib
from FlightLib2 import FlightLib
from FlightLib.FlightLib import LedLib # TODO new ledlib
import play_animation
random.seed()
logging.basicConfig( # TODO all prints as logs
level=logging.INFO,
format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
handlers=[
logging.FileHandler("client_logs"),
logging.StreamHandler()
])
NTP_PACKET_FORMAT = "!12I"
NTP_DELTA = 2208988800L # 1970-01-01 00:00:00
NTP_QUERY = '\x1b' + 47 * '\0'
@@ -52,7 +62,7 @@ def reconnect(t=2):
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))
broadcst_client.bind(("", broadcast_port))
while True:
data, addr = broadcst_client.recvfrom(1024)
print("Recieved broadcast message %s from %s"%(data, addr))
@@ -64,7 +74,7 @@ def reconnect(t=2):
def send_all(msg):
clientSocket.sendall(struct.pack('>I', len(msg)) + msg)
clientSocket.sendall(struct.pack('>I', len(msg)) + msg)
def recive_all(n):
@@ -87,7 +97,7 @@ def recive_message():
def form_command(command, args=()):
return " ".join([command, args])
return " ".join([command, args])
def parse_command(command_input):
@@ -121,7 +131,7 @@ def animation_player(running_event, stop_event):
rate = rospy.Rate(1000 / 125)
play_animation.takeoff()
play_animation.animate_frame(frames[0]) #Reach first point at the same time with others
rospy.sleep(5)
rospy.sleep(5) # TODO change to flightlib reach_point
for frame in frames:
running_event.wait()
if stop_event.is_set():
@@ -164,37 +174,44 @@ def stop_animation():
# animation_thread.join()
def selfcheck():
telemetry = FlightLib.get_telemetry('body')
return FlightLib.selfcheck(), telemetry.voltage
CONFIG_PATH = "client_config.ini"
config = ConfigParser.ConfigParser()
config.read(CONFIG_PATH)
port = int(config.get('SERVER', 'port'))
broadcast_port = config.getint('SERVER', 'broadcast_port')
port = config.getint('SERVER', 'port')
host = config.get('SERVER', 'host')
BUFFER_SIZE = int(config.get('SERVER', 'buffer_size'))
BUFFER_SIZE = config.getint('SERVER', 'buffer_size')
USE_NTP = config.getboolean('NTP', 'use_ntp')
NTP_HOST = config.get('NTP', 'host')
NTP_PORT = int(config.get('NTP', 'port'))
NTP_PORT = config.getint('NTP', 'port')
files_directory = config.get('FILETRANSFER', 'files_directory')
animation_file = config.get('COPTER', 'animation_file')
animation_file = config.get('FILETRANSFER', 'animation_file')
COPTER_ID = config.get('COPTER', 'id')
TAKEOFF_HEIGHT = config.getfloat('COPTERS', 'takeoff_height')
TAKEOFF_TIME = config.getint('COPTERS', 'takeoff_time')
USE_LEDS = config.getboolean('PRIVATE', 'use_leds')
play_animation.USE_LEDS = USE_LEDS
COPTER_ID = config.get('PRIVATE', 'id')
if COPTER_ID == 'default':
COPTER_ID = 'copter' + str(random.randrange(9999)).zfill(4)
write_to_config('COPTER', 'id', COPTER_ID)
TAKEOFF_HEIGHT = float(config.get('COPTER', 'takeoff_height'))
USE_LEDS = config.getboolean('COPTER', 'use_leds')
play_animation.USE_LEDS = USE_LEDS
write_to_config('PRIVATE', 'id', COPTER_ID)
rospy.init_node('Swarm_client', anonymous=True)
if USE_LEDS:
LedLib.init_led()
print("Client started on copter:", COPTER_ID)
print("NTP time:", time.ctime(get_ntp_time(NTP_HOST, NTP_PORT)))
if USE_NTP:
print("NTP time:", time.ctime(get_ntp_time(NTP_HOST, NTP_PORT)))
print("System time", time.ctime(time.time()))
reconnect()
@@ -225,9 +242,9 @@ try:
resume_animation()
elif command == 'stop':
stop_animation()
#FlightLib.reach(5, 5, 2)
FlightLib.interrupt()
elif command == 'land':
FlightLib.land1() # TODO dont forget change back to land
play_animation.land() # TODO dont forget change back to land
elif command == 'disarm':
FlightLib.arming(False)
@@ -239,6 +256,8 @@ try:
response = "test_succsess"
elif request_target == 'id':
response = COPTER_ID
elif request_target == 'selfcheck':
response = selfcheck()
send_all(bytes(form_command("response", response)))
print("Request responded with:", response)
except socket.error as e:

18
Drone/client_config.ini
View File

@@ -1,18 +1,26 @@
[SERVER]
port = 25000
broadcast_port = 8181
host = 192.168.1.2
buffer_size = 1024
[FILETRANSFER]
files_directory = files
files_directory = animation
animation_file = animation.csv
[NTP]
use_ntp = True
host = ntp1.stratum2.ru
port = 123
[COPTER]
id = copter2
[COPTERS]
takeoff_height = 1
takeoff_timeout = 7
takeoff_time = 5000
x0_common = 0
y0_common = 0
[PRIVATE]
id = copter2
use_leds = True
animation_file = animation.csv
x0 = 0
y0 = 0

12
Drone/play_animation.py
View File

@@ -1,7 +1,7 @@
import time
import csv
import rospy
from FlightLib.FlightLib import FlightLib
from FlightLib2 import FlightLib
#FlightLib.init('SingleCleverFlight')
from FlightLib.FlightLib import LedLib
@@ -9,22 +9,22 @@ animation_file_path = 'animation.csv'
USE_LEDS = True
def takeoff():
def takeoff(z=1.0):
if USE_LEDS:
LedLib.wipe_to(255, 0, 0)
FlightLib.takeoff1() # TODO dont forget change back to takeoff
FlightLib.takeoff(z=z, wait=True) # TODO dont forget change back to takeoff
def land():
if USE_LEDS:
LedLib.blink(255, 0, 0)
FlightLib.land1()
FlightLib.land()
if USE_LEDS:
LedLib.off()
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)
FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw=1.57) # TODO yaw
if USE_LEDS:
LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue'])
@@ -59,7 +59,7 @@ if __name__ == '__main__':
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=11.57)
FlightLib.reach_point(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw=11.57)
for frame in frames:
animate_frame(frame, x0=X0, y0=Y0)
rate.sleep()

5
Server/server_config.ini
View File

@@ -1,8 +1,9 @@
[SERVER]
port = 25000
broadcast_port = 8181
buffer_size = 1024
[NTP]
use_ntp = True
host = ntp1.stratum2.ru
port = 123
#pool.ntp.org
port = 123

24
Server/server_qt.py
View File

@@ -52,7 +52,7 @@ def ip_broadcast(ip, port):
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
broadcast_sock.sendto(msg, ('255.255.255.255', broadcast_port))
print("Broadcast sent")
time.sleep(5)
@@ -109,7 +109,7 @@ class Client:
if self.copter_id is None:
self.copter_id = self.get_response("id")
print("Got copter id:", self.copter_id)
model.appendRow((QStandardItem(self.copter_id), )) # TODO: get responses for another columns
model.appendRow((QStandardItem(self.copter_id), )) # TODO: get responses for another columns
def _send_all(self, msg):
self.socket.sendall(struct.pack('>I', len(msg)) + msg)
@@ -230,7 +230,7 @@ class Client:
file = open(filepath, 'rb')
chunk = file.read(BUFFER_SIZE)
while chunk:
self._send_queue.append(chunk)
self._send_queue.append(chunk) # TODO os.sendfile
chunk = file.read(BUFFER_SIZE)
file.close()
self.send(Client.form_command("/endoffile"))
@@ -270,7 +270,10 @@ class MainWindow(QtWidgets.QMainWindow):
@pyqtSlot()
def send_starttime(self):
dt = self.ui.start_delay_spin.value()
timenow = time.time() # TODO add NTP
if USE_NTP:
timenow = get_ntp_time(NTP_HOST, NTP_PORT)
else:
timenow = time.time()
print('Now:', time.ctime(timenow), "+ dt =", dt)
Client.send_to_selected(Client.form_command("starttime", (str(timenow + dt),)))
@@ -299,7 +302,6 @@ class MainWindow(QtWidgets.QMainWindow):
def disarm_all(self):
Client.broadcast("disarm")
@pyqtSlot()
def send_animations(self):
path = str(QFileDialog.getExistingDirectory(self, "Select Animation Directory"))
@@ -318,6 +320,7 @@ class MainWindow(QtWidgets.QMainWindow):
# ANS = dr.get_response("someshit")
# print(ANS)
model = QStandardItemModel()
model.setHorizontalHeaderLabels(
('copter ID', 'animation ID', 'battery V', 'battery %', 'selfcheck', 'time UTC')
@@ -331,11 +334,12 @@ config = configparser.ConfigParser()
config.read("server_config.ini")
port = int(config['SERVER']['port'])
broadcast_port = int(config['SERVER']['broadcast_port'])
BUFFER_SIZE = int(config['SERVER']['buffer_size'])
USE_NTP = bool(config['NTP']['use_ntp'])
NTP_HOST = config['NTP']['host']
NTP_PORT = int(config['NTP']['port'])
ServerSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
ServerSocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
host = socket.gethostname()
@@ -347,7 +351,13 @@ if __name__ == "__main__":
window = MainWindow()
print('Server started on', host, ip, ":", port)
# print('Now:', time.ctime(get_ntp_time(NTP_HOST, NTP_PORT)))
if USE_NTP:
now = time.ctime(get_ntp_time(NTP_HOST, NTP_PORT))
else:
now = time.time()
print('Now:', time.ctime(now))
print('Waiting for clients...')
ServerSocket.bind((ip, port))