diff --git a/.github/workflows/systemtests.yml b/.github/workflows/systemtests.yml index 85a59a557..7ca178568 100644 --- a/.github/workflows/systemtests.yml +++ b/.github/workflows/systemtests.yml @@ -45,17 +45,11 @@ jobs: id: step6 uses: actions/upload-artifact@v3 with: - name: pdf_and_bagfiles + name: pdf_bagfiles_and_logs path: | ros2_ws/bagfiles ros2_ws/results - - - name : Upload logs - id: step7 - uses: actions/upload-artifact@v3 - with: - name: logs - path: /home/github/.ros/log + /home/github/.ros/log diff --git a/.gitignore b/.gitignore index 193a80e18..738e759b9 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,5 @@ crazyswarm2_sim_webots/ *.pyc *.o *~ +.vscode/ +experiments.py diff --git a/crazyflie/config/crazyflies.yaml b/crazyflie/config/crazyflies.yaml index 74eb41eca..345c7c0a9 100644 --- a/crazyflie/config/crazyflies.yaml +++ b/crazyflie/config/crazyflies.yaml @@ -2,7 +2,7 @@ robots: cf231: enabled: true - uri: radio://0/60/2M/E7E7E7E7E7 + uri: radio://0/8s0/2M/E7E7E7E7E7 initial_position: [0, 0, 0] type: cf21 # see robot_types # firmware_params: diff --git a/crazyflie_examples/crazyflie_examples/figure8.py b/crazyflie_examples/crazyflie_examples/figure8.py index 2869b41c6..af75d2998 100644 --- a/crazyflie_examples/crazyflie_examples/figure8.py +++ b/crazyflie_examples/crazyflie_examples/figure8.py @@ -21,10 +21,10 @@ def main(): for cf in allcfs.crazyflies: cf.uploadTrajectory(0, 0, traj1) - allcfs.takeoff(targetHeight=0.4, duration=2.0) + allcfs.takeoff(targetHeight=1.0, duration=2.0) timeHelper.sleep(2.5) for cf in allcfs.crazyflies: - pos = np.array(cf.initialPosition) + np.array([0, 0, 0.4]) + pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) cf.goTo(pos, 0, 2.0) timeHelper.sleep(2.5) diff --git a/systemtests/plotter_class.py b/systemtests/plotter_class.py index f138d104e..2db8b5fd2 100644 --- a/systemtests/plotter_class.py +++ b/systemtests/plotter_class.py @@ -22,7 +22,7 @@ def __init__(self): self.EPSILON = 0.05 # euclidian distance in [m] between ideal and recorded trajectory under which the drone has to stay to pass the test self.DELAY_CONST_FIG8 = 4.75 #this is the delay constant which I found by adding up all the time.sleep() etc in the figure8.py file. This should be implemented better later - self.ALTITUDE_CONST_FIG8 = 0.4 #this is the altitude given for the takeoff in figure8.py. I should find a better solution than a symbolic constant ? + self.ALTITUDE_CONST_FIG8 = 1.0 #this is the altitude given for the takeoff in figure8.py. I should find a better solution than a symbolic constant ? self.ALTITUDE_CONST_MULTITRAJ = 1 #takeoff altitude for traj0 in multi_trajectory.py def file_guard(self, pdf_path): @@ -187,6 +187,10 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): self.read_csv_and_set_arrays(ideal_csvfile,rosbag_csvfile) + passed="failed" + if self.passed(): + passed = "passed" + #create PDF to save figures if(pdfname[-4:] != ".pdf"): pdfname= pdfname + '.pdf' @@ -196,16 +200,26 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): pdf_pages = PdfPages(pdfname) #create title page - text = 'figure8 trajectory test' + if "figure8" in ideal_csvfile: + name = "Figure8" + elif "multitrajectory" in ideal_csvfile: + name = "Multi_trajectory" + else: + name = "Unnamed test" + print("Plotter : test name not defined") + + text = f'{name} trajectory test' title_text_settings = f'Settings:\n' title_text_parameters = f'Parameters:\n' for key, value in self.params.items(): title_text_parameters += f" {key}: {value}\n" - title_text_results = f'Results:\n' + f'max error : ' + title_text_results = f'Results: test {passed}\n' + f'max error : ' title_text = text + "\n" + title_text_settings + "\n" + title_text_parameters + "\n" + title_text_results fig = plt.figure(figsize=(5,8)) fig.text(0.1, 0.1, title_text, size=11) + + pdf_pages.savefig(fig) @@ -294,8 +308,6 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): pdf_pages.close() - self.test_passed() - print("Results saved in " + pdfname) def test_passed(self) -> bool : diff --git a/systemtests/run_test.py b/systemtests/run_test.py index c9fc8270c..eda90d3c4 100755 --- a/systemtests/run_test.py +++ b/systemtests/run_test.py @@ -7,102 +7,120 @@ from datetime import datetime from plotter_class import Plotter from pathlib import Path +import shutil -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 +class Waiter: + '''A helper class which sleeps the amount of seconds it is instanciated with. If the user interrupts with ^C during the waiting time (zB if the drone crashes), it will stop sleeping and continue + to the end of the program where all child processes are terminated correctly. All subsequent Waiter() calls will be ignored''' + wait=True - bagname = 'bag_' + datetime.now().strftime('%d_%m_%Y-%H_%M_%S') - f8_bagname = "fig8_" + bagname - mt_bagname = "multitraj_" + bagname - src = "source " + str(path.parents[3].joinpath("install/setup.bash")) # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" + def __init__(self, seconds): + if seconds > 0 : + self.sleep(seconds) - #create the folder where we will record the different bags and the folder where the results pdf will be saved - 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.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 + def sleep(self, seconds): + start=time.time() + while wait and time.time()<(start+seconds): + try: + time.sleep(1) + except KeyboardInterrupt: + wait = False + +def record_start_and_terminate(testname:str, testduration:int, bagfolder:str): + '''Helper function that starts recording the /tf topic in a rosbag, starts the test, waits, closes the rosbag and terminate all processes + NB the testname must be the name of the crayzflie_examples executable (ie the CLI grammar "ros2 run crazyflie_examples testname" must be valid)''' + + index = bagfolder.find("bag_") + bagname = bagfolder[index:] + + if "figure8" in testname: + bagname = "fig8_" + bagname + elif "multi_trajectory" in testname : + bagname = "multitraj_" + bagname + else: + "run_test.py : test not defined" + exit(1) - - - 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"{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) - + src = bagfolder[:index-9] + "install/setup.bash" # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" + command = f"{src} && ros2 bag record -s mcap -o {bagname} /tf" + record_bag = Popen(command, shell=True, stderr=True, cwd=bagfolder, + stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid) command = f"{src} && ros2 run crazyflie_examples figure8" - start_fig8 = Popen(command, shell=True, stderr=True, + start_flight_test = Popen(command, shell=True, stderr=True, stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid) - #wait, startf8 and start_mt and the while loop are there to allow us to ^C during flightest (zB if the drone crashes) but still kill all child processes - #they could be removed if we don't have humans overlooking the test - wait=True - startf8=time.time() - while wait and time.time()<(startf8+20): - try: - time.sleep(1) - except KeyboardInterrupt: - wait = False + Waiter(testduration) #wait x seconds for the crazyflie to fly the test - 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 + os.killpg(os.getpgid(start_flight_test.pid), signal.SIGTERM) #kill flight test and all of its child processes + os.killpg(os.getpgid(record_bag.pid), signal.SIGTERM) #kill rosbag - 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) - 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) +def translate_and_plot(testname:str, bagfolder:str): + '''Helper function that translates rosbag .mcap format to .csv, then uses that csv to plot a pdf ''' + index = bagfolder.find("bag_") + bagname = bagfolder[index:] + # NB : the mcap filename is almost the same as the folder name but has _0 at the end + inputbag = str(bagfolder) + f"/{testname}_{bagname}/{testname}_{bagname}_0.mcap" + output_csv = str(bagfolder) + f"/{testname}_{bagname}/{testname}_{bagname}_0.csv" + writer = McapHandler() + writer.write_mcap_to_csv(inputbag, output_csv) #translate bag from mcap to csv + output_pdf = str(path.parents[3].joinpath(f"results/Results_{testname}_"+ now +".pdf")) + rosbag_csv = output_csv + if "figure8" in testname: + test_file = "../crazyflie_examples/crazyflie_examples/data/figure8.csv" + elif "multi_trajectory" in testname: + test_file = "../crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv" + plotter = Plotter() + plotter.create_figures(test_file, rosbag_csv, output_pdf) #plot the data + + +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 + #delete results, logs and bags of previous experiments + shutil.rmtree(path.parents[3].joinpath("bagfiles")) + shutil.rmtree(path.parents[3].joinpath("results")) + shutil.rmtree(path.parents[8].joinpath(".ros/log")) - start_mt = time.time() - while wait and time.time()<(start_mt+80): - try: - time.sleep(1) - except KeyboardInterrupt: - wait = False + + #create the folder where we will record the different bags and the folder where the results pdf will be saved + now = datetime.now().strftime('%d_%m_%Y-%H_%M_%S') + bagfolder = (path.parents[3].joinpath(f"bagfiles/bag_" + now)) # /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/bagfiles/bag_d_m_Y-H_M_S + os.makedirs(bagfolder) + os.makedirs(path.parents[3].joinpath("results")) # /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/results + + + src = "source " + str(path.parents[3].joinpath("install/setup.bash")) # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" + 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) - print('kill') - os.killpg(os.getpgid(start_multitraj.pid), signal.SIGTERM) - os.killpg(os.getpgid(record_multitraj_bag.pid), signal.SIGTERM) + record_start_and_terminate("figure8", 20, bagfolder) + record_start_and_terminate("multri_trajectory", 80, bagfolder) - os.killpg(os.getpgid(launch_crazyswarm.pid), signal.SIGTERM) #kill crazyswarm + os.killpg(os.getpgid(launch_crazyswarm.pid), signal.SIGTERM) #kill crazyswarm and all of its child processes - print("Flight finished") #test done, now we create the results pdf + translate_and_plot("figure8", bagfolder) + translate_and_plot("multi_trajectory", bagfolder) - #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 = 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 = 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 = 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 = 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) + # 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 = str(path.parents[3].joinpath("results/Results_mt_"+ now + ".pdf")) + # 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) exit()