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

Merge with master

Browse Source
This commit is contained in:
Arthur Golubtsov
2019-11-28 16:04:54 +03:00
parent 6ddc9eef1c 9c518a8109
commit 91fc9d8446
21 changed files with 836 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
Drone/FlightLib/FlightLib.py
View File

@@ -44,6 +44,7 @@ FLIP_MIN_Z = 2.0
checklist = []
get_telemetry_lock = threading.Lock()
delta = 0.0
def get_telemetry_locked(*args, **kwargs):
with get_telemetry_lock:
@@ -161,9 +162,6 @@ def check_angles(angle_limit=math.radians(5)):
if abs(telemetry.roll) >= angle_limit:
yield ("Roll estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.roll,
math.degrees(telemetry.roll)))
if abs(telemetry.yaw) >= angle_limit:
yield ("Yaw estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.yaw,
math.degrees(telemetry.yaw)))
def selfcheck():
@@ -174,12 +172,22 @@ def selfcheck():
return checks
def get_delta():
global delta
return delta
def reset_delta():
global delta
delta = 0
def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs):
global delta
set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, auto_arm=auto_arm)
#telemetry = get_telemetry_locked(frame_id=frame_id)
telemetry = get_telemetry_locked(frame_id=frame_id)
delta = get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z)
logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
#logger.info('Delta: {}'.format(delta))
#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))
@@ -272,6 +280,7 @@ def stop(frame_id='body', hold_speed=SPEED):
def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRAME_ID,
timeout_descend=TIMEOUT_DESCEND, timeout_land=TIMEOUT_LAND, freq=FREQUENCY, interrupter=INTERRUPTER):
reset_delta()
if descend:
logger.info("Descending to: | z: {:.3f}".format(z))
#print("Descending to: | z: {:.3f}".format(z))

177
Drone/animation_lib.py
View File

@@ -4,9 +4,16 @@ import copy
import rospy
import logging
import threading
import ConfigParser
from FlightLib import FlightLib
from FlightLib import LedLib
try:
from FlightLib import FlightLib
except ImportError:
print("Can't import FlightLib")
try:
from FlightLib import LedLib
except ImportError:
print("Can't import LedLib")
import tasking_lib as tasking
@@ -14,6 +21,11 @@ logger = logging.getLogger(__name__)
interrupt_event = threading.Event()
config = ConfigParser.ConfigParser()
config.read("client_config.ini")
default_delay = config.getfloat('ANIMATION', 'frame_delay')
anim_id = "Empty id"
# TODO refactor as class
@@ -54,23 +66,20 @@ def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1):
animation_file, delimiter=',', quotechar='|'
)
try:
row_0 = csv_reader.next()
row_frame = csv_reader.next()
except:
return float('nan'), float('nan')
if len(row_frame) == 1:
anim_id = row_frame[0]
logger.debug("Got animation_id: {}".format(row_frame[0]))
row_frame = csv_reader.next()
if len(row_frame) == 2:
logger.debug("Got frame delay: {}".format(row_frame[1]))
row_frame - csv_reader.next()
try:
frame_number, x, y, z, yaw, red, green, blue = row_frame
except:
return float('nan'), float('nan')
if len(row_0) == 1:
anim_id = row_0[0]
logger.debug("Got animation_id: {}".format(anim_id))
try:
frame_number, x, y, z, yaw, red, green, blue = csv_reader.next()
except:
return float('nan'), float('nan')
else:
anim_id = "Empty id"
logger.debug("No animation id in file")
try:
frame_number, x, y, z, yaw, red, green, blue = row_0
except:
return float('nan'), float('nan')
return float(x)*x_ratio, float(y)*y_ratio
@@ -84,6 +93,7 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati
anim_id = "No animation"
else:
with animation_file:
current_frame_delay = default_delay
csv_reader = csv.reader(
animation_file, delimiter=',', quotechar='|'
)
@@ -91,6 +101,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati
if len(row_0) == 1:
anim_id = row_0[0]
logger.debug("Got animation_id: {}".format(anim_id))
elif len(row_0) == 2:
current_frame_delay = float(row_0[1])
logger.debug("Got new frame delay: {}".format(current_frame_delay))
else:
logger.debug("No animation id in file")
frame_number, x, y, z, yaw, red, green, blue = row_0
@@ -103,25 +116,31 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati
'red': int(red),
'green': int(green),
'blue': int(blue),
'delay': current_frame_delay
})
for row in csv_reader:
frame_number, x, y, z, yaw, red, green, blue = row
imported_frames.append({
'number': int(frame_number),
'x': x_ratio*float(x) + x0,
'y': y_ratio*float(y) + y0,
'z': z_ratio*float(z) + z0,
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
})
if len(row) == 2:
current_frame_delay = float(row[1])
else:
frame_number, x, y, z, yaw, red, green, blue = row
imported_frames.append({
'number': int(frame_number),
'x': x_ratio*float(x) + x0,
'y': y_ratio*float(y) + y0,
'z': z_ratio*float(z) + z0,
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
'delay': current_frame_delay
})
return imported_frames
def correct_animation(frames, frame_delay=0.1, min_takeoff_height=0.5, move_delta=0.01, check_takeoff=True, check_land=True):
corrected_frames = copy.deepcopy(frames)
start_action = 'takeoff'
frames_to_start = 0
time_to_start = 0
if len(corrected_frames) == 0:
raise Exception('Nothing to correct!')
# Check takeoff
@@ -133,9 +152,10 @@ def correct_animation(frames, frame_delay=0.1, min_takeoff_height=0.5, move_delt
for i in range(len(corrected_frames)-1):
if corrected_frames[i-frames_to_start+1]['z'] - corrected_frames[i-frames_to_start]['z'] > move_delta:
break
time_to_start += corrected_frames[i-frames_to_start]['delay']
del corrected_frames[i-frames_to_start]
frames_to_start += 1
start_delay = frames_to_start*frame_delay
start_delay = time_to_start
# Check Land
# If copter lands in animation, landing points can be deleted
if (corrected_frames[len(corrected_frames)-1]['z'] < min_takeoff_height) and check_land:
@@ -144,11 +164,11 @@ def correct_animation(frames, frame_delay=0.1, min_takeoff_height=0.5, move_delt
if abs(corrected_frames[i-1]['z'] - corrected_frames[i]['z']) < move_delta:
break
del corrected_frames[i]
#for i in range(len(corrected_frames)-1,0,-1):
# if (abs(corrected_frames[i-1]['x'] - corrected_frames[i]['x']) > move_delta or
# abs(corrected_frames[i-1]['y'] - corrected_frames[i]['y']) > move_delta):
# break
# del corrected_frames[i]
for i in range(len(corrected_frames)-1,0,-1):
if (abs(corrected_frames[i-1]['x'] - corrected_frames[i]['x']) > move_delta or
abs(corrected_frames[i-1]['y'] - corrected_frames[i]['y']) > move_delta):
break
del corrected_frames[i]
return corrected_frames, start_action, start_delay
# Needs for test
@@ -156,60 +176,65 @@ def save_corrected_animation(frames, filename="corrected_animation.csv"):
corrected_animation = open(filename, mode='w+')
csv_writer = csv.writer(corrected_animation, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
for frame in frames:
csv_writer.writerow([frame['number'],frame['x'], frame['y'], frame['z']])
csv_writer.writerow([frame['number'],frame['x'], frame['y'], frame['z'], frame['delay']])
# print frame
corrected_animation.close()
def convert_frame(frame):
return ((frame['x'], frame['y'], frame['z']), (frame['red'], frame['green'], frame['blue']), frame['yaw'])
try:
def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
flight_func=FlightLib.navto, auto_arm=False, flight_kwargs=None, interrupter=interrupt_event):
if flight_kwargs is None:
flight_kwargs = {}
def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
flight_func=FlightLib.navto, auto_arm=False, flight_kwargs=None, interrupter=interrupt_event):
if flight_kwargs is None:
flight_kwargs = {}
flight_func(*point, yaw=yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs)
if use_leds:
if color:
LedLib.fill(*color)
flight_func(*point, yaw=yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs)
if use_leds:
if color:
LedLib.fill(*color)
def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, flight_func=FlightLib.navto,
interrupter=interrupt_event):
next_frame_time = 0
for frame in frames:
def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, flight_func=FlightLib.navto,
interrupter=interrupt_event):
next_frame_time = 0
for frame in frames:
if interrupter.is_set():
logger.warning("Animation playing function interrupted!")
interrupter.clear()
return
execute_frame(*convert_frame(frame), frame_id=frame_id, use_leds=use_leds, flight_func=flight_func,
interrupter=interrupter)
next_frame_time += frame_delay
tasking.wait(next_frame_time, interrupter)
def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,
interrupter=interrupt_event):
if use_leds:
LedLib.wipe_to(255, 0, 0, interrupter=interrupter)
if interrupter.is_set():
logger.warning("Animation playing function interrupted!")
interrupter.clear()
return
execute_frame(*convert_frame(frame), frame_id=frame_id, use_leds=use_leds, flight_func=flight_func,
interrupter=interrupter)
next_frame_time += frame_delay
tasking.wait(next_frame_time, interrupter)
result = FlightLib.takeoff(height=z, timeout_takeoff=timeout, frame_id=frame_id,
emergency_land=safe_takeoff, interrupter=interrupter)
if result == 'not armed' or result == 'timeout':
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 takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, 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.wipe_to(255, 0, 0, interrupter=interrupter)
if interrupter.is_set():
return
result = FlightLib.takeoff(height=z, timeout_takeoff=timeout, frame_id=frame_id,
emergency_land=safe_takeoff, interrupter=interrupter)
if result == 'not armed' or result == 'timeout':
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)
if use_leds:
LedLib.blink(255, 0, 0, interrupter=interrupter)
FlightLib.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter)
if use_leds:
LedLib.off()
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)
FlightLib.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter)
if use_leds:
LedLib.off()
except NameError:
print("Can't create flying functions")

20
Drone/client_config.ini
View File

@@ -13,9 +13,19 @@ use_ntp = False
host = ntp1.stratum2.ru
port = 123
[VISUAL_POSE_WATCHDOG]
timeout = 1.0
action = emergency_land
emergency_land_thrust = 0.45
emergency_land_decrease_thrust_after = 5.0
timeout_to_disarm_after_watchdog_action = 10.0
[TELEMETRY]
frequency = 1
transmit = True
clear_tasks_when_emergency = True
land_if_pos_delta_bigger_than = 3.0
log_cpu_and_memory = True
[ANIMATION]
takeoff_animation_check = True
@@ -31,16 +41,24 @@ takeoff_height = 1.0
takeoff_time = 5.0
safe_takeoff = False
reach_first_point_time = 5.0
land_time = 3.0
land_time = 1.0
x0_common = 0
y0_common = 0
z0_common = 0
yaw = 180
land_timeout = 10.0
[FLOOR FRAME]
parent = aruco_map
<<<<<<< HEAD
x = 2.1
y = 11.3
z = 6.2
=======
x = 2.4
y = 12.4
z = 6.4
>>>>>>> master
roll = 180
pitch = 0
yaw = -90

5
Drone/client_setup.sh
View File

@@ -48,7 +48,7 @@ systemctl restart dhcpcd
cat << EOF | tee /etc/hostname
$3
EOF
sed -i "/127.0.1.1/c 127.0.1.1 $3" /etc/hosts
sed -i "/127.0.1.1/c 127.0.1.1 $3 $3.local" /etc/hosts
# set hostname for ROS
sed -i "/ROS_HOSTNAME/c ROS_HOSTNAME=\'$3\'" /home/pi/.bashrc
@@ -71,8 +71,9 @@ EOF
# change server ip in client_config
sed -i "0,/^host/s/\(^h.*\)/host = $4/" client_config.ini
# enable clever show service
# enable clever show service and visual_pose_watchdog service
systemctl enable clever-show.service
systemctl enable visual_pose_watchdog.service
# restart clever
reboot

113
Drone/copter_client.py
View File

@@ -7,6 +7,7 @@ from clever import srv
import datetime
import logging
import threading
import psutil
import subprocess
import ConfigParser
from collections import namedtuple
@@ -22,13 +23,15 @@ import animation_lib as animation
from mavros_mavlink import *
from std_msgs.msg import Bool
from geometry_msgs.msg import Point, Quaternion, TransformStamped
from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply
import tf2_ros
static_bloadcaster = tf2_ros.StaticTransformBroadcaster()
Telemetry = namedtuple("Telemetry", "git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position")
telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS')
Telemetry = namedtuple("Telemetry", "git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position armed")
telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS', False)
emergency = False
# get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
@@ -75,6 +78,9 @@ class CopterClient(client.Client):
super(CopterClient, self).load_config()
self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency')
self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit')
self.CLEAR_TASKS_WHEN_EMERGENCY = self.config.getboolean('TELEMETRY', 'clear_tasks_when_emergency')
self.LOG_CPU_AND_MEMORY = self.config.getboolean('TELEMETRY', 'log_cpu_and_memory')
self.LAND_POS_DELTA = self.config.getfloat('TELEMETRY', 'land_if_pos_delta_bigger_than')
self.FRAME_ID = self.config.get('COPTERS', 'frame_id')
self.FRAME_FLIPPED_HEIGHT = 0.
self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height')
@@ -82,9 +88,11 @@ class CopterClient(client.Client):
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.LAND_TIMEOUT = self.config.getfloat('COPTERS', 'land_timeout')
self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common')
self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common')
self.Z0_COMMON = self.config.getfloat('COPTERS', 'z0_common')
self.YAW = self.config.get('COPTERS', 'yaw')
self.TAKEOFF_CHECK = self.config.getboolean('ANIMATION', 'takeoff_animation_check')
self.LAND_CHECK = self.config.getboolean('ANIMATION', 'land_animation_check')
self.FRAME_DELAY = self.config.getfloat('ANIMATION', 'frame_delay')
@@ -215,9 +223,11 @@ def configure_hosts(hostname):
index_start = raw_content.find("127.0.1.1", )
index_stop = raw_content.find("\n", index_start)
_ip, current_hostname = raw_content[index_start:index_stop].split()
hosts_array = raw_content[index_start:index_stop].split()
_ip = hosts_array[0]
current_hostname = hosts_array[1]
if current_hostname != hostname:
content = raw_content[:index_start] + "{} {}".format(_ip, hostname) + raw_content[index_stop:]
content = raw_content[:index_start] + "{} {} {}.local".format(_ip, hostname, hostname) + raw_content[index_stop:]
try:
with open(path, 'w') as f:
f.write(content)
@@ -258,8 +268,10 @@ def configure_bashrc(hostname):
@messaging.message_callback("execute")
def _execute(*args, **kwargs):
command = kwargs.get("command", None)
if command:
if command is not None:
logger.info("Executing command: {}".format(command))
execute_command(command)
logger.info("Executing done")
@messaging.message_callback("id")
def _response_id(*args, **kwargs):
@@ -369,6 +381,11 @@ def _calibrate_level(*args, **kwargs):
calibrate('level')
return get_calibration_status()
@messaging.request_callback("load_params")
def _load_params(*args, **kwargs):
result = load_param_file('temp.params')
logger.info("Load parameters to FCU success: {}".format(result))
return result
@messaging.message_callback("test")
def _command_test(*args, **kwargs):
@@ -566,7 +583,6 @@ def _play_animation(*args, **kwargs):
check_takeoff=client.active_client.TAKEOFF_CHECK,
check_land=client.active_client.LAND_CHECK,
)
# Choose start action
if start_action == 'takeoff':
# Takeoff first
@@ -597,12 +613,12 @@ def _play_animation(*args, **kwargs):
# Calculate start time
start_time += start_delay
# Arm
task_manager.add_task(start_time, 0, FlightLib.arming_wrapper,
task_kwargs={
"state": True
}
)
frame_time = start_time + 1.0
#task_manager.add_task(start_time, 0, FlightLib.arming_wrapper,
# task_kwargs={
# "state": True
# }
# )
frame_time = start_time # + 1.0
point, color, yaw = animation.convert_frame(corrected_frames[0])
task_manager.add_task(frame_time, 0, animation.execute_frame,
task_kwargs={
@@ -620,30 +636,39 @@ def _play_animation(*args, **kwargs):
# Play animation file
for frame in corrected_frames:
point, color, yaw = animation.convert_frame(frame)
if client.active_client.YAW == "animation":
yaw = frame["yaw"]
else:
yaw = math.radians(float(client.active_client.YAW))
task_manager.add_task(frame_time, 0, animation.execute_frame,
task_kwargs={
"point": point,
"color": color,
"yaw": yaw,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.navto,
}
)
frame_time += client.active_client.FRAME_DELAY
frame_time += frame["delay"]
# Calculate land_time
land_time = frame_time + client.active_client.LAND_TIME
# Land
task_manager.add_task(land_time, 0, animation.land,
task_kwargs={
"timeout": client.active_client.TAKEOFF_TIME,
"timeout": client.active_client.LAND_TIMEOUT,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
},
)
def telemetry_loop():
global telemetry
global telemetry, emergency
last_state = []
equal_state_counter = 0
max_count = 2
tasks_cleared = False
rate = rospy.Rate(client.active_client.TELEM_FREQ)
while not rospy.is_shutdown():
telemetry = telemetry._replace(animation_id = animation.get_id())
@@ -664,6 +689,7 @@ def telemetry_loop():
try:
ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
if ros_telemetry.connected:
telemetry = telemetry._replace(armed = ros_telemetry.armed)
telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage))
batt_empty_param = get_param('BAT_V_EMPTY')
batt_charged_param = get_param('BAT_V_CHARGED')
@@ -711,20 +737,75 @@ def telemetry_loop():
client.active_client.server_connection.send_message('telem', args={'message':create_telemetry_message(telemetry)})
except AttributeError as e:
logger.debug(e)
if client.active_client.CLEAR_TASKS_WHEN_EMERGENCY:
mode = telemetry.mode
armed = telemetry.armed
last_task = task_manager.get_last_task_name()
state = [mode, armed, last_task]
if state == last_state:
equal_state_counter += 1
else:
equal_state_counter = 0
external_interruption = (mode != "OFFBOARD" and armed == True and last_task not in [None, 'land'])
log_msg = ''
if emergency and external_interruption:
log_msg = "emergency and external interruption"
elif emergency:
log_msg = "emergency"
elif external_interruption:
log_msg = "external interruption"
logger.info("Possible expernal interruption, state_counter = {}".format(equal_state_counter))
if emergency or (external_interruption and equal_state_counter >= max_count):
if not tasks_cleared:
logger.info("Clear task manager because of {}".format(log_msg))
logger.info("Mode: {} | armed: {} | last task: {} ".format(mode, armed, last_task))
task_manager.reset()
FlightLib.reset_delta()
tasks_cleared = True
equal_state_counter = 0
else:
tasks_cleared = False
last_state = state
if client.active_client.LOG_CPU_AND_MEMORY:
cpu_usage = psutil.cpu_percent(interval=None, percpu=True)
mem_usage = psutil.virtual_memory().percent
cpu_temp_info = psutil.sensors_temperatures()['cpu-thermal'][0]
cpu_temp = cpu_temp_info.current
# https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md
throttled_hex = subprocess.check_output("vcgencmd get_throttled", shell=True).split('=')[1]
under_voltage = bool(int(bin(int(throttled_hex,16))[2:][-1]))
power_state = 'normal' if not under_voltage else 'under voltage!'
if cpu_temp_info.critical:
cpu_temp_state = 'critical'
elif cpu_temp_info.high:
cpu_temp_state = 'high'
else:
cpu_temp_state = 'normal'
logger.info("CPU usage: {} | Memory: {} % | T: {} ({}) | Power: {}".format(cpu_usage, mem_usage, cpu_temp, cpu_temp_state, power_state))
delta = FlightLib.get_delta()
logger.info("Delta: {}".format(delta))
if delta > client.active_client.LAND_POS_DELTA:
_command_land()
rate.sleep()
def create_telemetry_message(telemetry):
msg = client.active_client.client_id + '`'
for key in telemetry.__dict__:
msg += telemetry.__dict__[key] + '`'
if key != 'armed':
msg += telemetry.__dict__[key] + '`'
msg += repr(time.time())
return msg
def emergency_callback(data):
global emergency
emergency = data.data
telemetry_thread = threading.Thread(target=telemetry_loop, name="Telemetry getting thread")
if __name__ == "__main__":
copter_client = CopterClient()
task_manager = tasking.TaskManager()
rospy.Subscriber('/emergency', Bool, emergency_callback)
copter_client.start(task_manager)

28
Drone/mavros_mavlink.py
View File

@@ -1,12 +1,14 @@
import rospy
import time
import logging
from mavros_msgs.srv import CommandLong
from mavros_msgs.srv import ParamGet
from mavros_msgs.msg import State
from mavros_msgs.srv import ParamGet, ParamSet
from mavros_msgs.msg import State, ParamValue
from pymavlink.dialects.v20 import common as mavlink
send_command_long = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
get_param = rospy.ServiceProxy('/mavros/param/get', ParamGet)
set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
system_status = -1
heartbeat_sub = None
heartbeat_sub_status = None
@@ -123,3 +125,25 @@ def stop_subscriber():
heartbeat_sub.unregister()
heartbeat_sub_status = False
def load_param_file(px4_file):
result = True
try:
px4_params = open(px4_file)
except IOError:
logging.error("File {} can't be opened".format(filepath))
result = False
else:
with open(px4_file) as px4_params:
for line in px4_params:
param_str_array = line[:-1].split('\t')
param_name = param_str_array[2]
param_value_str = param_str_array[3]
param_type = param_str_array[4]
if param_type == '6':
param_value = ParamValue(integer=int(param_value_str))
else:
param_value = ParamValue(real=float(param_value_str))
if not set_param(param_name, param_value):
result = False
return result

6
Drone/tasking_lib.py
View File

@@ -41,6 +41,8 @@ class TaskManager(object):
self._task_interrupt_event = threading.Event()
self._shutdown_event = threading.Event()
self._last_task = None
self._timeshift = 0.0
def add_task(self, timestamp, priority, task_function,
@@ -85,6 +87,9 @@ class TaskManager(object):
return heapq.heappop(self.task_queue)
raise KeyError('Pop from an empty priority queue')
def get_last_task_name(self):
return self._last_task
def start(self):
#print("Task manager is started")
logger.info("Task manager is started")
@@ -187,6 +192,7 @@ class TaskManager(object):
self.pop_task()
except KeyError as e:
logger.error(str(e))
self._last_task = task.func.__name__
#try:
#print("Pop {} function!".format(task.func.__name__))
#except Exception as e:

144
Drone/visual_pose_watchdog.py Normal file
View File

@@ -0,0 +1,144 @@
import rospy
import sys
import time
import logging
import ConfigParser
from clever.srv import SetAttitude
from sensor_msgs.msg import Range
from mavros_msgs.msg import State
from mavros_msgs.srv import SetMode, CommandBool
from std_msgs.msg import Bool
from geometry_msgs.msg import PoseStamped
config = ConfigParser.ConfigParser()
config.read("client_config.ini")
visual_pose_timeout = config.getfloat('VISUAL_POSE_WATCHDOG', 'timeout')
timeout_action = config.get('VISUAL_POSE_WATCHDOG', 'action')
emergency_land_thrust = config.getfloat('VISUAL_POSE_WATCHDOG', 'emergency_land_thrust')
emergency_land_decrease_thrust_after = config.getfloat('VISUAL_POSE_WATCHDOG', 'emergency_land_decrease_thrust_after')
timeout_to_disarm_after_watchdog_action = config.getfloat('VISUAL_POSE_WATCHDOG', 'timeout_to_disarm_after_watchdog_action')
logging.basicConfig( # TODO all prints as logs
level=logging.DEBUG, # INFO
stream=sys.stdout,
format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
handlers=[
logging.StreamHandler(sys.stdout),
])
handler = logging.StreamHandler(sys.stdout)
handler.setLevel(logging.DEBUG)
formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s")
handler.setFormatter(formatter)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO)
logger.addHandler(handler)
set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
set_attitude = rospy.ServiceProxy('/set_attitude', SetAttitude)
visual_pose_last_timestamp = 0
armed = False
mode = ''
laser_range = 10
emergency = False
rospy.init_node('visual_pose_watchdog')
logger.info('visual_pose_watchdog inited')
logger.info('timeout = {} | timeout_action = {}'.format(visual_pose_timeout, timeout_action))
logger.info('timeout_to_disarm_after_watchdog_action = {}'.format(timeout_to_disarm_after_watchdog_action))
if timeout_action == 'emergency_land':
logger.info('emergency_land_thrust: {}'.format(emergency_land_thrust))
rate = rospy.Rate(10)
def visual_pose_callback(data):
global visual_pose_last_timestamp
visual_pose_last_timestamp = data.header.stamp.to_sec()
def state_callback(data):
global armed, mode
armed = data.armed
mode = data.mode
def laser_callback(data):
global laser_range
laser_range = data.range
def watchdog_callback(event):
global visual_pose_last_timestamp, armed, mode, timeout_action, laser_range, emergency
logger.debug("armed: {} | mode: {} | delta: {} | action: {} | range: {}".format(armed, mode, abs(time.time() - visual_pose_last_timestamp), timeout_action, laser_range))
if abs(time.time() - visual_pose_last_timestamp) > visual_pose_timeout:
if armed:
if timeout_action in ['land', 'emergency_land', 'disarm']:
emergency = True
if timeout_action == 'land':
logger.info('Visual pose data is too old, copter is armed, landing...')
while mode != "AUTO.LAND":
try:
set_mode(custom_mode='AUTO.LAND')
except rospy.ServiceException as e:
logger.info(e)
rate.sleep()
logger.info('Land mode is set')
action_timestamp = time.time()
while armed:
if time.time() - action_timestamp > timeout_to_disarm_after_watchdog_action:
try:
arming(False)
except rospy.ServiceException as e:
logger.info(e)
rate.sleep()
elif timeout_action == 'disarm':
logger.info('Visual pose data is too old, copter is armed, disarming...')
while armed:
try:
arming(False)
except rospy.ServiceException as e:
logger.info(e)
rate.sleep()
elif timeout_action == 'emergency_land':
logger.info('Visual pose data is too old, copter is armed, emergency landing...')
action_timestamp = time.time()
current_thrust = emergency_land_thrust
while armed:
logger.debug("Emergency land | range: {} | thrust: {}".format(laser_range, current_thrust))
try:
set_attitude(thrust = current_thrust, yaw = 0, frame_id = 'body')
except rospy.ServiceException as e:
logger.info(e)
delta = time.time() - action_timestamp
if delta > timeout_to_disarm_after_watchdog_action:
try:
arming(False)
except rospy.ServiceException as e:
logger.info(e)
if (laser_range < 0.1 or delta > emergency_land_decrease_thrust_after) and current_thrust > 0.:
current_thrust -= 0.02
if current_thrust < 0:
current_thrust = 0
rate.sleep()
logger.info('Disarmed')
emergency = False
else:
logger.info('Visual pose data is too old')
rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback)
rospy.Subscriber('/mavros/state', State, state_callback)
rospy.Subscriber('/mavros/distance_sensor/rangefinder', Range, laser_callback)
emergency_pub = rospy.Publisher('/emergency', Bool, queue_size=10)
rospy.Timer(rospy.Duration(0.5), watchdog_callback)
while not rospy.is_shutdown():
emergency_msg = Bool()
emergency_msg.data = emergency
emergency_pub.publish(emergency_msg)
rate.sleep()

72
Server/copter_table_models.py
View File

@@ -1,6 +1,7 @@
import sys
import re
import math
import configparser
import collections
import indexed
from server import ConfigOption
@@ -12,6 +13,9 @@ from PyQt5.QtCore import Qt as Qt
ModelDataRole = 998
ModelStateRole = 999
config = configparser.ConfigParser()
config.read("server_config.ini")
class CopterData:
class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('git_ver', None), ('anim_id', None),
@@ -49,6 +53,13 @@ class StatedCopterData(CopterData):
try:
self.states.__dict__[key] = \
Checks.all_checks[self.attrs_dict.keys().index(key)](value)
if key == 'start_pos':
if (self.__dict__['position'] is not None) and (self.__dict__['start_pos'] is not None):
current_pos = get_position(self.__dict__['position'])
start_pos = get_position(self.__dict__['start_pos'])
delta = get_position_delta(current_pos, start_pos)
if delta != 'NO_POS':
self.states.__dict__[key] = (delta < Checks.start_pos_delta_max)
except KeyError: # No check present for that col
pass
else: # update selfchecked and takeoff_ready
@@ -60,21 +71,32 @@ class StatedCopterData(CopterData):
[self.states[i] for i in Checks.takeoff_checklist]
)
def get_position(pos_string):
pos = []
pos_str = pos_string.split(' ')
if pos_str[0] != 'nan' and pos_str[0] != 'NO_POS':
for i in range(3):
pos.append(float(pos_str[i]))
else:
pos = 'NO_POS'
return pos
def get_position_delta(pos1, pos2):
if pos1 != 'NO_POS' and pos2 != 'NO_POS':
delta_squared = 0
for i in range(3):
delta_squared += (pos1[i]-pos2[i])**2
return math.sqrt(delta_squared)
return 'NO_POS'
class Checks:
all_checks = {}
takeoff_checklist = (3, 4, 6, 7, 8)
current_position = 'NO_POS'
start_position = 'NO_POS'
@classmethod
def get_pos_delta(self):
if self.current_position != 'NO_POS' and self.start_position != 'NO_POS':
delta_squared = 0
for i in range(3):
delta_squared += (self.current_position[i]-self.start_position[i])**2
return math.sqrt(delta_squared)
return 'NO_POS'
battery_min = config.getfloat('CHECKS', 'battery_percentage_min')
start_pos_delta_max = config.getfloat('CHECKS', 'start_pos_delta_max')
time_delta_max = config.getfloat('CHECKS', 'time_delta_max')
class CopterDataModel(QtCore.QAbstractTableModel):
selected_ready_signal = QtCore.pyqtSignal(bool)
@@ -290,7 +312,7 @@ def check_bat(item):
if item == "NO_INFO":
return False
else:
return float(item.split(' ')[1][:-1]) > 50
return float(item.split(' ')[1][:-1]) > Checks.battery_min
@col_check(4)
def check_sys_status(item):
@@ -310,49 +332,31 @@ def check_mode(item):
return None
return (item != "NO_FCU") and not ("CMODE" in item)
@col_check(7)
def check_selfcheck(item):
if not item:
return None
return item == "OK"
@col_check(8)
def check_pos_status(item):
if not item:
return None
str_pos = item.split(' ')
if str_pos[0] != 'nan' and str_pos[0] != 'NO_POS':
Checks.current_position = []
for i in range(3):
Checks.current_position.append(float(str_pos[i]))
return True
Checks.current_position = 'NO_POS'
return False
return str_pos[0] != 'nan' and str_pos[0] != 'NO_POS'
@col_check(9)
def check_start_pos_status(item):
if not item:
return None
str_start_pos = item.split(' ')
if str_start_pos[0] != 'nan' and str_start_pos[0] != 'NO_POS':
Checks.start_position = []
for i in range(3):
Checks.start_position.append(float(str_start_pos[i]))
delta = Checks.get_pos_delta()
if delta == 'NO_POS':
return False
else:
return delta < 1.
return False
return str_start_pos[0] != 'nan' and str_start_pos[0] != 'NO_POS'
@col_check(10)
def check_time_delta(item):
if not item:
return None
return abs(float(item)) < 1
return abs(float(item)) < Checks.time_delta_max
def all_checks(copter_item):
@@ -361,14 +365,12 @@ def all_checks(copter_item):
return False
return True
def takeoff_checks(copter_item):
for col in Checks.takeoff_checklist:
if not Checks.all_checks[col](copter_item[col]):
return False
return True
def flip_checks(copter_item):
for col in Checks.takeoff_checklist:
if col != 4 or col != 7:

14
Server/server.py
View File

@@ -3,6 +3,7 @@ import time
import socket
import random
import logging
import datetime
import threading
import selectors
import collections
@@ -17,11 +18,22 @@ import messaging_lib as messaging
random.seed()
now = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
path = 'server_logs'
if not os.path.exists(path):
try:
os.mkdir(path)
except OSError:
print("Creation of the directory %s failed" % path)
else:
print("Successfully created the directory %s " % path)
logging.basicConfig( # TODO all prints as logs
level=logging.DEBUG,
format="%(asctime)s [%(name)-7.7s] [%(threadName)-19.19s] [%(levelname)-7.7s] %(message)s",
handlers=[
logging.FileHandler("server_logs.log"),
logging.FileHandler("server_logs/{}.log".format(now)),
logging.StreamHandler()
])

5
Server/server_config.ini
View File

@@ -12,3 +12,8 @@ broadcast_delay = 5
use_ntp = False
host = ntp1.stratum2.ru
port = 123
[CHECKS]
battery_percentage_min = 50
start_pos_delta_max = 1
time_delta_max = 1

18
Server/server_gui.py
View File

@@ -238,8 +238,8 @@ class Ui_MainWindow(object):
self.actionFill.setObjectName("actionFill")
self.action_send_any_file = QtWidgets.QAction(MainWindow)
self.action_send_any_file.setObjectName("action_send_any_file")
self.actionSend_any_command = QtWidgets.QAction(MainWindow)
self.actionSend_any_command.setObjectName("actionSend_any_command")
self.action_send_any_command = QtWidgets.QAction(MainWindow)
self.action_send_any_command.setObjectName("action_send_any_command")
self.action_stop_music = QtWidgets.QAction(MainWindow)
self.action_stop_music.setObjectName("action_stop_music")
self.action_remove_row = QtWidgets.QAction(MainWindow)
@@ -248,13 +248,18 @@ class Ui_MainWindow(object):
self.action_send_calibrations.setObjectName("action_send_calibrations")
self.action_reboot_all = QtWidgets.QAction(MainWindow)
self.action_reboot_all.setObjectName("action_reboot_all")
self.action_restart_chrony = QtWidgets.QAction(MainWindow)
self.action_restart_chrony.setObjectName("action_restart_chrony")
self.action_send_fcu_parameters = QtWidgets.QAction(MainWindow)
self.action_send_fcu_parameters.setObjectName("action_send_fcu_parameters")
self.menuDeveloper_mode.addAction(self.action_send_any_file)
self.menuDeveloper_mode.addAction(self.actionSend_any_command)
self.menuDeveloper_mode.addAction(self.action_send_any_command)
self.menuOptions.addAction(self.action_send_animations)
self.menuOptions.addAction(self.action_send_configurations)
self.menuOptions.addAction(self.action_send_launch_file)
self.menuOptions.addAction(self.action_send_Aruco_map)
self.menuOptions.addAction(self.action_send_calibrations)
self.menuOptions.addAction(self.action_send_fcu_parameters)
self.menuOptions.addSeparator()
self.menuOptions.addAction(self.menuDeveloper_mode.menuAction())
self.menuOptions.addSeparator()
@@ -267,10 +272,11 @@ class Ui_MainWindow(object):
self.menuDeveloper_mode_2.addAction(self.action_reboot_all)
self.menuDrone.addAction(self.action_set_z_offset_to_ground)
self.menuDrone.addAction(self.action_reset_z_offset)
self.menuDrone.addSeparator()
self.menuDrone.addAction(self.action_restart_chrony)
self.menuDrone.addAction(self.action_remove_row)
self.menuDrone.addSeparator()
self.menuDrone.addAction(self.menuDeveloper_mode_2.menuAction())
self.menuDrone.addAction(self.action_remove_row)
self.menuMusic.addAction(self.action_select_music_file)
self.menuMusic.addAction(self.action_play_music)
self.menuMusic.addAction(self.action_stop_music)
@@ -332,8 +338,10 @@ class Ui_MainWindow(object):
self.action_test_music_after.setText(_translate("MainWindow", "Test music after"))
self.actionFill.setText(_translate("MainWindow", "fill"))
self.action_send_any_file.setText(_translate("MainWindow", "Send any file"))
self.actionSend_any_command.setText(_translate("MainWindow", "Send any command"))
self.action_send_any_command.setText(_translate("MainWindow", "Send any command"))
self.action_stop_music.setText(_translate("MainWindow", "Stop music"))
self.action_remove_row.setText(_translate("MainWindow", "Remove from table"))
self.action_send_calibrations.setText(_translate("MainWindow", "Send camera calibrations"))
self.action_reboot_all.setText(_translate("MainWindow", "Reboot all"))
self.action_restart_chrony.setText(_translate("MainWindow", "Restart chrony"))
self.action_send_fcu_parameters.setText(_translate("MainWindow", "Send FCU parameters"))

18
Server/server_gui.ui
View File

@@ -391,13 +391,14 @@
<string>Developer mode</string>
</property>
<addaction name="action_send_any_file"/>
<addaction name="actionSend_any_command"/>
<addaction name="action_send_any_command"/>
</widget>
<addaction name="action_send_animations"/>
<addaction name="action_send_configurations"/>
<addaction name="action_send_launch_file"/>
<addaction name="action_send_Aruco_map"/>
<addaction name="action_send_calibrations"/>
<addaction name="action_send_fcu_parameters"/>
<addaction name="separator"/>
<addaction name="menuDeveloper_mode"/>
<addaction name="separator"/>
@@ -425,10 +426,11 @@
</widget>
<addaction name="action_set_z_offset_to_ground"/>
<addaction name="action_reset_z_offset"/>
<addaction name="separator"/>
<addaction name="action_restart_chrony"/>
<addaction name="action_remove_row"/>
<addaction name="separator"/>
<addaction name="menuDeveloper_mode_2"/>
<addaction name="action_remove_row"/>
</widget>
<widget class="QMenu" name="menuMusic">
<property name="title">
@@ -536,7 +538,7 @@
<string>Send any file</string>
</property>
</action>
<action name="actionSend_any_command">
<action name="action_send_any_command">
<property name="text">
<string>Send any command</string>
</property>
@@ -561,6 +563,16 @@
<string>Reboot all</string>
</property>
</action>
<action name="action_restart_chrony">
<property name="text">
<string>Restart chrony</string>
</property>
</action>
<action name="action_send_fcu_parameters">
<property name="text">
<string>Send FCU parameters</string>
</property>
</action>
</widget>
<tabstops>
<tabstop>start_delay_spin</tabstop>

42
Server/server_qt.py
View File

@@ -9,7 +9,7 @@ from PyQt5 import QtWidgets, QtMultimedia
from PyQt5.QtGui import QStandardItemModel, QStandardItem
from PyQt5.QtCore import Qt, pyqtSlot, pyqtSignal, QObject, QUrl
from PyQt5.QtWidgets import QFileDialog, QMessageBox
from PyQt5.QtWidgets import QFileDialog, QMessageBox, QApplication, QWidget, QInputDialog, QLineEdit
from quamash import QEventLoop, QThreadExecutor
# Importing gui form
@@ -152,6 +152,9 @@ class MainWindow(QtWidgets.QMainWindow):
self.ui.action_send_configurations.triggered.connect(self.send_configurations)
self.ui.action_send_Aruco_map.triggered.connect(self.send_aruco)
self.ui.action_send_launch_file.triggered.connect(self.send_launch)
self.ui.action_send_fcu_parameters.triggered.connect(self.send_fcu_parameters)
self.ui.action_send_any_file.triggered.connect(self.send_any_file)
self.ui.action_send_any_command.triggered.connect(self.send_any_command)
self.ui.action_restart_clever.triggered.connect(self.restart_clever)
self.ui.action_restart_clever_show.triggered.connect(self.restart_clever_show)
self.ui.action_update_client_repo.triggered.connect(self.update_client_repo)
@@ -160,6 +163,7 @@ class MainWindow(QtWidgets.QMainWindow):
self.ui.action_reset_start.triggered.connect(self.reset_start)
self.ui.action_set_z_offset_to_ground.triggered.connect(self.set_z_offset_to_ground)
self.ui.action_reset_z_offset.triggered.connect(self.reset_z_offset)
self.ui.action_restart_chrony.triggered.connect(self.restart_chrony)
self.ui.action_select_music_file.triggered.connect(self.select_music_file)
self.ui.action_play_music.triggered.connect(self.play_music)
self.ui.action_stop_music.triggered.connect(self.stop_music)
@@ -416,9 +420,38 @@ class MainWindow(QtWidgets.QMainWindow):
for file in files:
filename = os.path.basename(file)
copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/launch/{}".format(filename))
# copter.client.send_message("service_restart", {"name": "clever"})
@pyqtSlot()
def send_fcu_parameters(self):
path = QFileDialog.getOpenFileName(self, "Select px4 param file", filter="px4 params (*.params)")[0]
if path:
filename = os.path.basename(path)
print("Selected file:", path, filename)
for copter in self.model.user_selected():
copter.client.send_file(path, "temp.params")
copter.client.get_response("load_params", self._print_send_fcu_params_result, callback_args=(copter, ))
def _print_send_fcu_params_result(self, value, copter):
logging.info("Send parameters to {} success: {}".format(copter.client.copter_id, value))
@pyqtSlot()
def send_any_file(self):
path = QFileDialog.getOpenFileName(self, "Select file")[0]
if path:
filename = os.path.basename(path)
print("Selected file:", path, filename)
text, okPressed = QInputDialog.getText(self, "Enter path to send on copter","Destination:", QLineEdit.Normal, "/home/pi/")
if okPressed and text != '':
for copter in self.model.user_selected():
copter.client.send_file(path, text+'/'+filename)
@pyqtSlot()
def send_any_command(self):
text, okPressed = QInputDialog.getText(self, "Enter command to send on copter","Command:", QLineEdit.Normal, "")
if okPressed and text != '':
for copter in self.model.user_selected():
copter.client.send_message("execute", {"command": text})
@pyqtSlot()
def restart_clever(self):
for copter in self.model.user_selected():
copter.client.send_message("service_restart", {"name": "clever"})
@@ -458,6 +491,11 @@ class MainWindow(QtWidgets.QMainWindow):
for copter in self.model.user_selected():
copter.client.send_message("reset_z_offset")
@pyqtSlot()
def restart_chrony(self):
for copter in self.model.user_selected():
copter.client.send_message("repair_chrony")
@pyqtSlot()
def select_music_file(self):
path = QFileDialog.getOpenFileName(self, "Select music file", filter="Music files (*.mp3 *.wav)")[0]

5
builder/assets/clever-show.service
View File

@@ -1,8 +1,7 @@
[Unit]
Description=Clever Show Client Service
Requires=clever.service
Requires=network.target
After=network.target
Requires=roscore.service
After=clever.service
[Service]
WorkingDirectory=/home/pi/clever-show/Drone

15
builder/assets/visual_pose_watchdog.service Normal file
View File

@@ -0,0 +1,15 @@
[Unit]
Description=Visual Pose Watchdog
Requires=roscore.service
After=network.target
[Service]
User=pi
WorkingDirectory=/home/pi/clever-show/Drone
ExecStart=/bin/bash -c ". /home/pi/catkin_ws/devel/setup.sh; \
ROS_HOSTNAME=`hostname`.local /usr/bin/python /home/pi/clever-show/Drone/visual_pose_watchdog.py"
Restart=on-failure
RestartSec=3
[Install]
WantedBy=multi-user.target

3
builder/image-build.sh
View File

@@ -117,8 +117,9 @@ img-chroot ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software.sh'
# Configure image
img-chroot ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-configure.sh'
# Copy service file for clever show client
# Copy service files for clever show client and visual_pose_watchdog
img-chroot ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever-show.service' '/lib/systemd/system/'
img-chroot ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/visual_pose_watchdog.service' '/lib/systemd/system/'
# Copy config files for clever
if [[ -d "${CONFIG_DIR}/launch" ]]; then img-chroot ${IMAGE_PATH} copy ${CONFIG_DIR}'/launch' '/home/pi/catkin_ws/src/clever/clever'; fi

1
builder/image-software.sh
View File

@@ -61,6 +61,7 @@ chrony \
echo_stamp "Install python libs"
my_travis_retry pip install selectors2
my_travis_retry pip install psutil
echo_stamp "Install catkin packages"
cd /home/pi/catkin_ws/src

250
tools/change_landing.py Normal file
View File

@@ -0,0 +1,250 @@
import argparse
import os
import csv
import glob
import copy
import math
import numpy as np
import logging
def generate_line(pos1, pos2, speed, frame_delay = 0.1, start_frame = 0, color = [0, 0, 0], yaw = 0):
dist = np.linalg.norm(pos1-pos2)
delta = pos2 - pos1
frames_count = math.ceil(dist/(speed*frame_delay))
k = 1./frames_count
frames = []
for i in range(int(frames_count)+1):
frame_pos = pos1 + k*i*delta
frames.append({
'number': start_frame + i,
'x': frame_pos[0],
'y': frame_pos[1],
'z': frame_pos[2],
'yaw': yaw,
'red': color[0],
'green': color[1],
'blue': color[2],
})
return frames
def generate_positions(start_pos, nx, ny, dx, dy):
len_x = (nx - 1)*dx
len_y = (ny - 1)*dy
x_center = start_pos[0]
y_center = start_pos[1]
z = start_pos[2]
x0 = x_center - len_x/2.
y0 = y_center + len_y/2.
positions = []
for iy in range(ny):
for ix in range(nx):
positions.append(np.array([x0+dx*ix, y0-dy*iy, z]))
return positions
def parse_positions_file(filename):
names = []
pos = []
try:
pos_file = open(filename)
except IOError:
logging.error("File {} can't be opened".format(filename))
else:
with pos_file:
n_str = pos_file.readline()[:-1].split(' ')
nx = int(n_str[0])
ny = int(n_str[1])
dx = float(n_str[2])
dy = float(n_str[3])
pos_str = pos_file.readline()[:-1].split(' ')
for i in range(3):
pos.append(float(pos_str[i]))
for lines in pos_file:
names.append(pos_file.readline()[:-1])
return nx, ny, dx, dy, pos, names
def cut_to_closest_position(frames, pos, start_frame = -1):
if start_frame == -1:
return frames
else:
distances = np.zeros(len(frames)-start_frame)
for i in range(start_frame, len(frames)):
current_pos = np.array([frames[i]['x'], frames[i]['y'], frames[i]['z']])
distances[i-start_frame] = np.linalg.norm(current_pos - pos)
closest_index = distances.argmin()
cut_frames = frames[:start_frame+closest_index]
return cut_frames
def parse_animation_file(filename):
imported_frames = []
anim_id = ''
try:
animation_file = open(filename)
except IOError:
logging.error("File {} can't be opened".format(filename))
else:
with animation_file:
csv_reader = csv.reader(
animation_file, delimiter=',', quotechar='|'
)
row_0 = csv_reader.next()
if len(row_0) == 1:
anim_id = row_0[0]
logging.debug("Got animation_id: {}".format(anim_id))
else:
logging.debug("No animation id in file")
frame_number, x, y, z, yaw, red, green, blue = row_0
imported_frames.append({
'number': int(frame_number),
'x': float(x),
'y': float(y),
'z': float(z),
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
})
for row in csv_reader:
frame_number, x, y, z, yaw, red, green, blue = row
imported_frames.append({
'number': int(frame_number),
'x': float(x),
'y': float(y),
'z': float(z),
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
})
return imported_frames, anim_id
def cut_file(filename, _from, _to, reverse = False):
imported_frames = []
anim_id = ""
try:
animation_file = open(filename)
except IOError:
logging.error("File {} can't be opened".format(filepath))
else:
with animation_file:
csv_reader = csv.reader(
animation_file, delimiter=',', quotechar='|'
)
row_0 = csv_reader.next()
if len(row_0) == 1:
anim_id = row_0[0]
logging.debug("Got animation_id: {}".format(anim_id))
else:
logging.debug("No animation id in file")
frame_number, x, y, z, yaw, red, green, blue = row_0
imported_frames.append({
'number': int(frame_number),
'x': float(x),
'y': float(y),
'z': float(z),
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
})
for row in csv_reader:
frame_number, x, y, z, yaw, red, green, blue = row
imported_frames.append({
'number': int(frame_number),
'x': float(x),
'y': float(y),
'z': float(z),
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
})
if _to == 0 or _to >= len(imported_frames):
_to = len(imported_frames)-1
path = '{}/cut_{}_{}'.format(os.path.dirname(filename),_from,_to)
if reverse:
path += '_r'
csv_file = open(path+'/'+os.path.basename(filename), mode='w+')
with csv_file:
csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
if anim_id != "":
csv_writer.writerow([anim_id])
if not reverse:
for i in range(_from, _to+1):
csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'],
imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']])
else:
for i in range(_from):
csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'],
imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']])
for i in range(_to, len(imported_frames)):
csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'],
imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']])
print("Successfully created file {}".format(path+'/'+os.path.basename(filename)))
def change_landing(frames, land_pos, speed, start_frame = -1):
cut_frames = cut_to_closest_position(frames, land_pos, start_frame)
last_frame_pos = np.array([cut_frames[-1]['x'], cut_frames[-1]['y'], cut_frames[-1]['z']])
line = generate_line(last_frame_pos, land_pos, speed, start_frame=len(cut_frames))
return cut_frames + line
def save_frames(frames, animation_id, filename):
csv_file = open(filename, mode='w+')
with csv_file:
csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
if animation_id != "":
csv_writer.writerow([animation_id])
for frame in frames:
csv_writer.writerow([frame['number'], frame['x'], frame['y'], frame['z'], frame['yaw'], frame['red'], frame['green'], frame['blue']])
print("Successfully created file {}".format(path+'/'+os.path.basename(filename)))
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Change landing positions")
parser.add_argument('directory', nargs='?', default='.',
help="Directory with animation csv files. Default is '.'")
parser.add_argument('-s','--start', type=int, default=-1,
help="Start count from this frame, default is -1 (from the end)")
parser.add_argument('-p','--posfile', type=str, default="positions.txt",
help="Positions file. Default is positions.txt")
parser.add_argument('-v','--speed', type=float, default=0.5,
help="Copter speed in m/s. Default is 0.5")
args = parser.parse_args()
start_frame = args.start
speed = args.speed
path = '{}/land_{}'.format(args.directory,start_frame)
if not os.path.exists(path):
try:
os.mkdir(path)
except OSError:
print("Creation of the directory %s failed" % path)
else:
print("Successfully created the directory %s " % path)
files = [f for f in glob.glob(args.directory + '/*.csv')]
files.sort()
nx, ny, dx, dy, pos, names = parse_positions_file(args.posfile)
land_positions = generate_positions(pos, nx, ny, dx, dy)
for i in range(len(files)):
filename = os.path.basename(files[i])
frames, animation_id = parse_animation_file(files[i])
land_position = land_positions[i]
new_frames = change_landing(frames, land_position, speed, start_frame)
save_frames(new_frames, animation_id, path+'/'+filename)

52
tools/cut.py
View File

@@ -5,14 +5,14 @@ import glob
import copy
import logging
def cut_file(filename, _from, _to):
def cut_file(filename, _from, _to, reverse = False):
imported_frames = []
anim_id = ""
try:
animation_file = open(filename)
except IOError:
logging.error("File {} can't be opened".format(filepath))
logging.error("File {} can't be opened".format(filename))
else:
with animation_file:
csv_reader = csv.reader(
@@ -52,18 +52,29 @@ def cut_file(filename, _from, _to):
_to = len(imported_frames)-1
path = '{}/cut_{}_{}'.format(os.path.dirname(filename),_from,_to)
print('Path is {}'.format(path))
if reverse:
path += '_r'
csv_file = open(path+'/'+os.path.basename(filename), mode='w+')
with csv_file:
csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
if anim_id != "":
csv_writer.writerow([anim_id])
for i in range(_from, _to+1):
csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'],
imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']])
if not reverse:
for i in range(_from, _to+1):
csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'],
imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']])
else:
for i in range(_from):
csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'],
imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']])
for i in range(_to, len(imported_frames)):
csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'],
imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']])
print("Successfully created file {}".format(path+'/'+filename))
print("Successfully created file {}".format(path+'/'+os.path.basename(filename)))
if __name__ == "__main__":
@@ -74,26 +85,27 @@ if __name__ == "__main__":
help="Cut from this frame, default is 0 (from the beginning)")
parser.add_argument('-t','--to', type=int, default=0,
help="Cut to this frame (including this one), default is 0 (to the end)")
parser.add_argument('-r', '--reverse', action='store_true', help="Reverse cutting, this tool will cut from the beginning to <frm> frame and from <to> frame to the end")
args = parser.parse_args()
_from = args.frm
_to = args.to
path = '{}/cut_{}_{}'.format(args.directory,_from,_to)
if args.reverse:
path += '_r'
try:
os.mkdir(path)
except OSError:
print("Creation of the directory %s failed" % path)
files = [f for f in glob.glob(args.directory + '/*.csv')]
for f in files:
cut_file(f, _from, _to)
else:
print("Successfully created the directory %s " % path)
files = [f for f in glob.glob(args.directory + '/*.csv')]
for f in files:
cut_file(f, _from, _to)
if not os.path.exists(path):
try:
os.mkdir(path)
except OSError:
print("Creation of the directory %s failed" % path)
else:
print("Successfully created the directory %s " % path)
files = [f for f in glob.glob(args.directory + '/*.csv')]
for f in files:
cut_file(f, _from, _to, args.reverse)

2
tools/positions.txt Normal file
View File

@@ -0,0 +1,2 @@
2 6 0.8 1
-0.7 0 1