From b744ab083ab94f5903738b1764e7008bd9e6a6c0 Mon Sep 17 00:00:00 2001 From: JulienThevenoz Date: Wed, 22 Nov 2023 16:19:35 +0100 Subject: [PATCH] latest version --- systemtests/newsub.py | 75 +++++++++++++++++++++--------------- systemtests/plotter_class.py | 6 +-- 2 files changed, 46 insertions(+), 35 deletions(-) diff --git a/systemtests/newsub.py b/systemtests/newsub.py index 15c3112f8..0ef4fe3c2 100755 --- a/systemtests/newsub.py +++ b/systemtests/newsub.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from subprocess import Popen +from subprocess import Popen, PIPE import time import os import signal @@ -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) diff --git a/systemtests/plotter_class.py b/systemtests/plotter_class.py index 8f9874de8..00fc45540 100644 --- a/systemtests/plotter_class.py +++ b/systemtests/plotter_class.py @@ -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 @@ -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]