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

Repair play animation, pause and stop also work now

Browse Source
This commit is contained in:
Arthur Golubtsov
2019-06-11 14:19:32 +03:00
parent 2516906fe1
commit f9fd81da1f
7 changed files with 111 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
Drone/FlightLib/FlightLib.py
View File

@@ -14,8 +14,8 @@ from std_srvs.srv import Trigger
logger = logging.getLogger(__name__)
# create proxy service
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
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)
@@ -152,12 +152,12 @@ def selfcheck():
def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, **kwargs):
set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw)
telemetry = get_telemetry(frame_id=frame_id)
#telemetry = get_telemetry(frame_id=frame_id)
logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z))
print('Telemetry now: | z: {:.3f}'.format(telemetry.z))
#logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
#print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
#logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z))
#print('Telemetry now: | z: {:.3f}'.format(telemetry.z))
return True
@@ -293,8 +293,8 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
logger.info("Arming, going to OFFBOARD mode")
# Arming check
set_rates(thrust=0.1, auto_arm=True)
telemetry = get_telemetry(frame_id=frame_id)
set_rates(thrust=0.05, auto_arm=True)
telemetry = get_telemetry()
rate = rospy.Rate(freq)
time_start = time.time()
@@ -303,9 +303,9 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
logger.warning("Takeoff function interrupted!")
print("Takeoff function interrupted!")
interrupter.clear()
return
return 'interrupted'
telemetry = get_telemetry(frame_id=frame_id)
telemetry = get_telemetry()
logger.info("Arming...")
print("Arming...")
time_passed = time.time() - time_start
@@ -315,7 +315,7 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
if not telemetry.armed:
logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed))
print('Arming timed out! | time: {:3f} seconds'.format(time_passed))
return False
return 'not armed'
else:
break
rate.sleep()
@@ -326,14 +326,14 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
# Reach height
z0 = get_telemetry().z
z_dest = z + z0
navigate(z=z, speed=speed, frame_id=frame_id, auto_arm=True)
navigate(z=z, speed=speed, yaw = float('nan'), auto_arm=True)
current_diff = abs(get_telemetry().z - z_dest)
while (current_diff > tolerance) or wait:
if interrupter.is_set():
logger.warning("Flight function interrupted!")
print("Flight function interrupted!")
interrupter.clear()
return
return 'interrupted'
current_diff = abs(get_telemetry().z - z_dest)
logger.info("Takeoff...")
@@ -349,11 +349,11 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
logger.info("Preforming emergency land")
print("Preforming emergency land")
land(descend=False, interrupter=interrupter)
return False
return 'time out'
else:
break
rate.sleep()
logger.info("Takeoff succeeded!")
print("Takeoff succeeded!")
return True
return 'success'

4
Drone/FlightLib/LedLib.py
View File

@@ -4,7 +4,7 @@ import time
from rpi_ws281x import *
from tasking_lib import wait as wait_until
# LED strip configuration:
LED_COUNT = 29 # Number of LED pixels.
LED_COUNT = 60 # Number of LED pixels.
LED_PIN = 21 # GPIO pin connected to the pixels (18 uses PWM!) (10 uses SPI /dev/spidev0.0).
LED_FREQ_HZ = 800000 # LED signal frequency in hertz (usually 800khz)
LED_DMA = 10 # DMA channel to use for generating signal (try 10)
@@ -27,7 +27,7 @@ b_prev = 0
direct = False
l = 0
wait_ms = 10
wait_ms = 5.0
INTERRUPTER = threading.Event()
INTERRUPTER_UNSET = threading.Event()

11
Drone/animation_lib.py
View File

@@ -22,6 +22,7 @@ def get_id(filepath="animation.csv"):
except IOError:
logging.error("File {} can't be opened".format(filepath))
id = "No animation"
return id
else:
with animation_file:
csv_reader = csv.reader(
@@ -81,7 +82,7 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
def convert_frame(frame):
return (frame['x'], frame['y'], frame['z']), (frame["red"], frame["green"], frame["blue"]), frame["yaw"]
return ((frame['x'], frame['y'], frame['z']), (frame['red'], frame['green'], frame['blue']), frame['yaw'])
def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
@@ -111,22 +112,24 @@ 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, timeout=5000, frame_id='aruco_map', 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
FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff,
result = FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff,
interrupter=interrupter)
if result == 'not armed':
raise Exception('STOP') # Raise exception to clear task_manager if copter can't arm
if interrupter.is_set():
return
if use_leds:
LedLib.blink(0, 255, 0, wait=50, interrupter=interrupter)
def land(z=1.5, descend=False, timeout=5000, frame_id='aruco_map', use_leds=True,
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)

3
Drone/client_config.ini
View File

@@ -1,7 +1,7 @@
[SERVER]
port = 25000
broadcast_port = 8181
host = 192.168.43.168
host = 192.168.11.169
buffer_size = 1024
[FILETRANSFER]
@@ -19,6 +19,7 @@ takeoff_height = 1.5
takeoff_time = 8.0
safe_takeoff = False
reach_first_point_time = 8.0
land_time = 3.0
x0_common = 0
y0_common = 0

88
Drone/copter_client.py
View File

@@ -22,6 +22,7 @@ class CopterClient(client.Client):
self.TAKEOFF_TIME = self.config.getfloat('COPTERS', 'takeoff_time')
self.SAFE_TAKEOFF = self.config.getboolean('COPTERS', 'safe_takeoff')
self.RFP_TIME = self.config.getfloat('COPTERS', 'reach_first_point_time')
self.LAND_TIME = self.config.getfloat('COPTERS', 'land_time')
self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common')
self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common')
@@ -127,55 +128,62 @@ def _command_stop(**kwargs):
@messaging.message_callback("start")
def _play_animation(**kwargs):
gap = 0.25
start_time = kwargs["time"] # TODO
print('start time = {}'.format(start_time))
start_time = float(kwargs["time"]) # TODO
if (animation.get_id() == 'No animation'):
print("Can't start animation without animation file!")
return
print("Start time = {}, wait for {} seconds".format(start_time, time.time()-start_time))
frames = animation.load_animation(os.path.abspath("animation.csv"),
x0=client.active_client.X0 + client.active_client.X0_COMMON,
y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
)
task_manager.add_task(start_time, -1, animation.takeoff,
task_kwargs={
"z": client.active_client.TAKEOFF_HEIGHT,
"timeout": client.active_client.TAKEOFF_TIME,
"safe_takeoff": client.active_client.SAFE_TAKEOFF,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
}
)
task_manager.add_task(start_time, 0, animation.takeoff,
task_kwargs={
"z": client.active_client.TAKEOFF_HEIGHT,
"timeout": client.active_client.TAKEOFF_TIME,
"safe_takeoff": client.active_client.SAFE_TAKEOFF,
#"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
}
)
rfp_time = start_time + client.active_client.TAKEOFF_TIME + gap
task_manager.add_task(rfp_time, -1, animation.execute_frame,
task_kwargs={
"point": animation.convert_frame(frames[0]),
"timeout": client.active_client.RFP_TIME,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.reach_point,
}
)
rfp_time = start_time + client.active_client.TAKEOFF_TIME
task_manager.add_task(rfp_time, 0, animation.execute_frame,
task_kwargs={
"point": animation.convert_frame(frames[0])[0],
"color": animation.convert_frame(frames[0])[1],
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.reach_point,
}
)
animation_time = rfp_time + client.active_client.RFP_TIME + gap
frame_time = rfp_time + client.active_client.RFP_TIME
frame_delay = 0.125 # TODO from animation file
task_manager.add_task(animation_time, -1, animation.execute_animation,
task_kwargs={
"frames": frames,
"frame_delay": frame_delay,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
}
)
for frame in frames:
task_manager.add_task(frame_time, 0, animation.execute_frame,
task_kwargs={
"point": animation.convert_frame(frame)[0],
"color": animation.convert_frame(frame)[1],
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.navto,
}
)
frame_time += frame_delay
land_time = animation_time + len(frames)*frame_delay + gap
task_manager.add_task(land_time, -1, animation.land,
task_kwargs={
"z": client.active_client.TAKEOFF_HEIGHT,
"timeout": client.active_client.TAKEOFF_TIME,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
},
)
land_time = frame_time + client.active_client.LAND_TIME
task_manager.add_task(land_time, 0, animation.land,
task_kwargs={
"timeout": client.active_client.TAKEOFF_TIME,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
},
)
if __name__ == "__main__":

66
Drone/tasking_lib.py
View File

@@ -21,7 +21,7 @@ def wait(end, interrupter=INTERRUPTER, maxsleep=0.1): # Added features to inter
time.sleep(diff / 2)
else:
logger.warning("Waiting was interrupted!")
print("Waiting was interrupted!")
#print("Waiting was interrupted!")
class TaskManager(object):
@@ -39,7 +39,7 @@ class TaskManager(object):
self._task_interrupt_event = threading.Event()
self._shutdown_event = threading.Event()
self._timeshift = 0
self._timeshift = 0.0
def add_task(self, timestamp, priority, task_function,
task_args=(), task_kwargs={}, task_delayable=False):
@@ -62,17 +62,17 @@ class TaskManager(object):
if self.task_queue[0] != entry_old:
self._task_interrupt_event.set()
print("Task queue updated with more priority task")
#print("Task queue updated with more priority task")
if self._reset_event.is_set():
self._task_interrupt_event.set()
self._reset_event.clear()
print("Task queue updated after reset")
#print("Task queue updated after reset")
self._wait_interrupt_event.clear()
self._running_event.set()
print(self.task_queue)
#print(self.task_queue)
def pop_task(self):
with self._task_queue_lock:
@@ -81,13 +81,13 @@ class TaskManager(object):
raise KeyError('Pop from an empty priority queue')
def start(self, timeouts=False):
print("Task manager is started")
logger.info("Task manager is started")
#print("Task manager is started")
#logger.info("Task manager is started")
self._processor_thread.start()
self.resume()
def stop(self):
self._timeshift = 0
self._timeshift = 0.0
self.pause(interrupt=True)
with self._task_queue_lock:
del self.task_queue[:]
@@ -105,8 +105,8 @@ class TaskManager(object):
self._wait_interrupt_event.set()
self._task_interrupt_event.set()
self._running_event.clear()
logger.info("Task queue paused")
print("Task queue paused")
#logger.info("Task queue paused")
#print("Task queue paused")
def resume(self, time_to_start_next_task = 0):
if self.task_queue:
@@ -116,8 +116,8 @@ class TaskManager(object):
self._running_event.set()
self._wait_interrupt_event.clear()
self._task_interrupt_event.clear()
logger.info("Task queue resumed with timeshift {}".format(self._timeshift))
print("Task queue resumed with timeshift {}".format(self._timeshift))
#logger.info("Task queue resumed with timeshift {}".format(self._timeshift))
#print("Task queue resumed with timeshift {}".format(self._timeshift))
def reset(self):
self.stop()
@@ -129,24 +129,28 @@ class TaskManager(object):
if self.task_queue:
start_time, priority, count, task = self.task_queue[0]
else:
self._timeshift = 0
self._timeshift = 0.0
return
logger.info("Waiting util task execution time:{}".format(start_time + self._timeshift))
print("Waiting util task execution time:{}".format(start_time + self._timeshift))
wait(start_time + self._timeshift, self._wait_interrupt_event)
task_start_time = start_time + self._timeshift
#logger.info("Waiting util task execution time:{}".format(task_start_time))
#print("Waiting util task execution time:{}".format(task_start_time))
wait(task_start_time, self._wait_interrupt_event)
if not self._wait_interrupt_event.is_set():
logger.info("Executing task {}".format(task))
print("Executing task {}".format(task))
#logger.info("Executing task {}".format(task))
#print("Executing task {}".format(task))
try:
task.func(*task.args, interrupter=self._task_interrupt_event, **task.kwargs)
except Exception as e:
logger.error("Error '{}' occurred in task {}".format(e, task))
print("Error '{}' occurred in task {}".format(e, task))
#print("Error '{}' occurred in task {}".format(e, task))
#if str(e) == 'STOP':
# self.reset()
# return
else:
logger.warning("Task interrupted before execution")
print("Task interrupted before execution")
#logger.warning("Task interrupted before execution")
#print("Task interrupted before execution")
self._wait_interrupt_event.clear()
return
@@ -154,30 +158,30 @@ class TaskManager(object):
start_time_n, priority_n, count_n, task_n = self.task_queue[0]
if (task_n == task) and (start_time_n == start_time):
self.pop_task()
try:
print("Pop {} function!".format(task.func.__name__))
except Exception as e:
print("Pop something!")
#try:
#print("Pop {} function!".format(task.func.__name__))
#except Exception as e:
#print("Pop something!")
if self._task_interrupt_event.is_set():
self._task_interrupt_event.clear()
logger.info("Execution done")
print("Execution done")
#logger.info("Execution done")
#print("Execution done")
def _task_processor(self):
logger.info("Tasking thread started")
#logger.info("Tasking thread started")
# self._update_queue() # Initial tick if tasks added before start
while not self._shutdown_event.is_set():
self._running_event.wait()
self.execute_task()
if __name__ == "__main__":
logger.addHandler(logging.StreamHandler())
logger.setLevel(logging.DEBUG)
#logger.addHandler(logging.StreamHandler())
#logger.setLevel(logging.DEBUG)
def printer(stri, interrupter, *args, **kwargs):
logger.info("String: {}, timenow: {}".format(stri, time.time()))
#logger.info("String: {}, timenow: {}".format(stri, time.time()))
wait(time.time()+30, interrupter)
tasker = TaskManager() # Lower priority first!

2
Server/server_qt.py
View File

@@ -106,7 +106,7 @@ class MainWindow(QtWidgets.QMainWindow):
self.ui.pause_button.setText('Resume')
else:
Client.broadcast_message('resume')
self.ui.pause_button.setText('Pause')
self.ui.pause_button.setText('Pause')
@pyqtSlot()
def test_leds(self):