Skip to content

Commit

Permalink
latest version
Browse files Browse the repository at this point in the history
  • Loading branch information
@julienthevenoz
julienthevenoz committed Nov 22, 2023
1 parent 0e14265 commit b744ab0
Show file tree
Hide file tree
Showing 2 changed files with 46 additions and 35 deletions.
75 changes: 43 additions & 32 deletions systemtests/newsub.py
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
from subprocess import Popen
from subprocess import Popen, PIPE
import time
import os
import signal
Expand All @@ -10,85 +10,96 @@


if __name__ == "__main__":

path = Path(__file__) #Path(__file__) should be "/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/src/crazyswarm2/systemtests/newsub.py" ; path.parents[0]=.../systemstests

bagname = 'bag_' + datetime.now().strftime('%d_%m_%Y-%H_%M_%S')
f8_bagname = "fig8_" + bagname
mt_bagname = "multitraj_" + bagname
src = "source install/setup.bash"

#path of the file Path(__file__) should be "/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/src/crazyswarm2/systemtests/newsub.py"
#so Path(__file__)parents[0]="...src/crazyswarm2/systemtests" and parents[1]=".../src/crazyswarm2" etc
src = "source " + str(path.parents[3].joinpath("install/setup.bash")) # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash"

#create the folder where we will record the different bags and the folder where the results pdf will be saved
path = Path(__file__)
bagfolder = (path.parents[3].joinpath(f"bagfiles/{bagname}")) # /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/bagfiles/bag_d_m_Y-H_M_S
os.mkdirs(bagfolder, exist_ok=True)
os.mkdirs(path.parents[3].joinpath("results"), exist_ok=True) # /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/results
os.makedirs(bagfolder, exist_ok=True)
os.makedirs(path.parents[3].joinpath("results"), exist_ok=True) # /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/results


print('launch cswarm')
command = f"{src} && ros2 launch crazyflie launch.py"
launch_crazyswarm = Popen(command, shell=True, stderr=True,
stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)
time.sleep(1)
# command = f"mkdir results && mkdir bagfiles && mkdir bagfiles/{bagname}"
# create_dirs = Popen(command, shell=True, stderr=True, cwd = "/home/github/ros2_ws",
# stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)


print('rosbag f8')
command = f"{src} && ros2 bag record -s mcap -o {f8_bagname} /tf"
record_fig8_bag = Popen(command, shell=True, stderr=True, cwd=bagfolder,
stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)


print('run f8')
command = f"{src} && ros2 run crazyflie_examples figure8"
start_fig8 = Popen(command, shell=True, stderr=True,
stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)
time.sleep(20)

print('wait 20')
#wait, startf8 and start_mt and the while loop are there to make keyboard interrupting still
wait=True
startf8=time.time()
while wait and time.time()<(startf8+20):
try:
time.sleep(1)
except KeyboardInterrupt:
wait = False
os.killpg(os.getpgid(start_fig8.pid), signal.SIGTERM) #kill figure8 flight process and all of its child processes
os.killpg(os.getpgid(record_fig8_bag.pid), signal.SIGTERM) #kill rosbag figure8

print('rosbag mt')
command = f"{src} && ros2 bag record -s mcap -o {mt_bagname} /tf"
record_multitraj_bag = Popen(command, shell=True, stderr=True, cwd = bagfolder,
stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)

print('run mt')
command = f"{src} && ros2 run crazyflie_examples multi_trajectory"
start_multitraj = Popen(command, shell=True, stderr=True,
stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)
time.sleep(80)


print('wait 80')
start_mt = time.time()
while wait and time.time()<(start_mt+80):
try:
time.sleep(1)
except KeyboardInterrupt:
wait = False

print('kill')
os.killpg(os.getpgid(start_multitraj.pid), signal.SIGTERM)
os.killpg(os.getpgid(record_multitraj_bag), signal.SIGTERM)
os.killpg(os.getpgid(record_multitraj_bag.pid), signal.SIGTERM)

os.killpg(os.getpgid(launch_crazyswarm.pid), signal.SIGTERM) #kill crazyswarm

print("Flight finished")

#test done, now we compare the ideal and real trajectories
#test done, now we create the results pdf

#create a file that translates the .mcap format of the rosbag to .csv
# NB : the filename is almost the same as the folder name but has _0 at the end
inputbag = bagfolder + '/' + f8_bagname + '/' + f8_bagname + '_0' + '.mcap'
output_csv = bagfolder + '/' + f8_bagname + '/' + f8_bagname + '_0' + '.csv'
inputbag = str(bagfolder) + '/' + f8_bagname + '/' + f8_bagname + '_0' + '.mcap'
output_csv = str(bagfolder) + '/' + f8_bagname + '/' + f8_bagname + '_0' + '.csv'
writer = McapHandler()
writer.write_mcap_to_csv(inputbag, output_csv) #translate bag from mcap to csv
output_pdf = path.parents[3].joinpath("results/Results_fig8_"+ datetime.now().strftime('%d_%m_%Y-%H_%M_%S'))
output_pdf = str(path.parents[3].joinpath("results/Results_fig8_"+ datetime.now().strftime('%d_%m_%Y-%H_%M_%S')))
rosbag_csv = output_csv
test_file = "../crazyflie_examples/crazyflie_examples/data/figure8.csv"
plotter = Plotter()
plotter.create_figures(test_file, rosbag_csv, output_pdf) #plot the data


# inputbag = "bagfiles/" + bagname + '/' + f8_bagname + '/' + f8_bagname + '_0' + '.mcap'
# output_csv = "bagfiles/" + bagname + '/' + f8_bagname + '/' + f8_bagname + '_0' + '.csv'
# writer.write_mcap_to_csv(inputbag, output_csv)
# output_pdf = "results/Results_fig8_"+ datetime.now().strftime('%d_%m_%Y-%H_%M_%S')
# rosbag_csv = output_csv

# test_file = "../crazyflie_examples/crazyflie_examples/data/figure8.csv"
# plotter = Plotter()
# plotter.create_figures(test_file, rosbag_csv, output_pdf)


inputbag = bagfolder + '/' + mt_bagname + '/' + mt_bagname + '_0' + '.mcap'
output_csv = bagfolder + '/' + mt_bagname + '/' + mt_bagname + '_0' + '.csv'
inputbag = str(bagfolder) + '/' + mt_bagname + '/' + mt_bagname + '_0' + '.mcap'
output_csv = str(bagfolder) + '/' + mt_bagname + '/' + mt_bagname + '_0' + '.csv'
writer.write_mcap_to_csv(inputbag, output_csv)
output_pdf_2 = path.parents[3].joinpath("results/Results_mt_"+ datetime.now().strftime('%d_%m_%Y-%H_%M_%S'))
output_pdf_2 = str(path.parents[3].joinpath("results/Results_mt_"+ datetime.now().strftime('%d_%m_%Y-%H_%M_%S')))
rosbag_csv = output_csv
test_file = "../crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv"
plotter.create_figures(test_file, rosbag_csv, output_pdf_2)
Expand Down
6 changes: 3 additions & 3 deletions systemtests/plotter_class.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -157,12 +157,12 @@ def adjust_arrays(self):
break
i+=1

assert (takeoff_index != None) and (landing_index != None), "couldn't find drone takeoff or landing"
assert (takeoff_index != None) and (landing_index != None), "Plotter : couldn't find drone takeoff or landing"


####get rid of datapoints before takeoff and after landing in bag_times, bag_x, bag_y, bag_y

assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "self.bag_* aren't the same size before trimming"
assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "Plotter : self.bag_* aren't the same size before trimming"
index_arr = np.arange(len(self.bag_times))
slicing_arr = np.delete(index_arr, index_arr[takeoff_index:landing_index+1]) #in our slicing array we take out all the indexes of when the drone is in flight so that it only contains the indexes of when the drone is on the ground

Expand All @@ -172,7 +172,7 @@ def adjust_arrays(self):
self.bag_y = np.delete(self.bag_y, slicing_arr)
self.bag_z = np.delete(self.bag_z, slicing_arr)

assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "self.bag_* aren't the same size after trimming"
assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "Plotter : self.bag_* aren't the same size after trimming"

#rewrite bag_times to start at 0 and be written in [s] instead of [ns]
bag_start_time = self.bag_times[0]
Expand Down

0 comments on commit b744ab0

Please sign in to comment.