diff --git a/.github/workflows/systemtests.yml b/.github/workflows/systemtests.yml index da29294a4..c8ac91fad 100644 --- a/.github/workflows/systemtests.yml +++ b/.github/workflows/systemtests.yml @@ -39,7 +39,7 @@ jobs: source /opt/ros/humble/setup.bash . install/local_setup.bash export ROS_LOCALHOST_ONLY=1 - python3 src/crazyswarm2/systemtests/run_test.py + python3 src/crazyswarm2/systemtests/test_flights.py - name: Upload files id: step6 @@ -47,9 +47,8 @@ jobs: with: name: pdf_bagfiles_and_logs path: | - ros2_ws/bagfiles ros2_ws/results - ~/.ros/log + diff --git a/systemtests/plotter_class.py b/systemtests/plotter_class.py index beca43e33..5655aedb6 100644 --- a/systemtests/plotter_class.py +++ b/systemtests/plotter_class.py @@ -4,6 +4,7 @@ from matplotlib.backends.backend_pdf import PdfPages import numpy as np from crazyflie_py.uav_trajectory import Trajectory +from pathlib import Path class Plotter: @@ -21,9 +22,10 @@ def __init__(self): self.deviation = [] #list of all indexes where euclidian_distance(ideal - recorded) > EPSILON 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 = 1.0 #this is the altitude given for the takeoff in figure8.py. I should find a better solution than a symbolic constant ? + 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 could be implemented better later ? + self.ALTITUDE_CONST_FIG8 = 1 #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 + self.X_OFFSET_CONST_MULTITRAJ = -0.3 #offest on the x axis between ideal and real trajectory. Reason: ideal trajectory (traj0.csv) starts with offset of 0.3m and CrazyflieServer.startTrajectory() is relative to start position def file_guard(self, pdf_path): msg = None @@ -45,18 +47,22 @@ def file_guard(self, pdf_path): def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): '''Method that reads the csv data of the ideal test trajectory and of the actual recorded trajectory and initializes the attribute arrays''' - #check which test we are plotting : figure8 or multi_trajectory - if("fig8" in rosbag_csvfile): - fig8 = True + #check which test we are plotting : figure8 or multi_trajectory or another one + if("figure8" in rosbag_csvfile): + fig8, m_t = True, False print("Plotting fig8 test data") - else: - fig8 = False + elif "multi_trajectory" in rosbag_csvfile: + fig8, m_t = False, True print("Plotting multi_trajectory test data") + else: + fig8, m_t = False, False + print("Plotting unspecified test data") #get ideal trajectory data self.ideal_traj_csv = Trajectory() try: path_to_ideal_csv = os.path.join(os.path.dirname(os.path.abspath(__file__)),ideal_csvfile) + print(path_to_ideal_csv) self.ideal_traj_csv.loadcsv(path_to_ideal_csv) except FileNotFoundError: print("Plotter : File not found " + path_to_ideal_csv) @@ -93,11 +99,15 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): pos = [0,0,0] #for all recorded datapoints who cannot be matched to a corresponding ideal position we assume the drone is on its ground start position (ie those datapoints are before takeoff or after landing) - self.ideal_traj_x[i], self.ideal_traj_y[i]= pos[0], pos[1] - if(fig8) : #special case : in fig8 test no altitude is given in the trajectory polynomials but is stays constant after takeoff in figure8.py - self.ideal_traj_z[i] = self.ALTITUDE_CONST_FIG8 - else : #for multi_trajectory test the altitude given in the trajectory polynomials is added to the takeoff altitude - self.ideal_traj_z[i] = self.ALTITUDE_CONST_MULTITRAJ + pos[2] + self.ideal_traj_x[i], self.ideal_traj_y[i], self.ideal_traj_z[i]= pos[0], pos[1], pos[2] + + #special cases + if fig8: + self.ideal_traj_z[i] = self.ALTITUDE_CONST_FIG8 #special case: in fig8 no altitude is given in the trajectory polynomials (idealcsv) but is fixed as the takeoff altitude in figure8.py + elif m_t: + self.ideal_traj_z[i] = pos[2] + self.ALTITUDE_CONST_MULTITRAJ #for multi_trajectory the altitude given in the trajectory polynomials is added to the fixed takeoff altitude specified in multi_trajectory.py + self.ideal_traj_x[i] = pos[0] + self.X_OFFSET_CONST_MULTITRAJ #the x-axis is offset by 0.3 m because ideal start position not being (0,0,0) + self.euclidian_dist[i] = np.linalg.norm([self.ideal_traj_x[i]-self.bag_x[i], self.ideal_traj_y[i]-self.bag_y[i], self.ideal_traj_z[i]-self.bag_z[i]]) @@ -341,3 +351,5 @@ def test_passed(self) -> bool : if args.open: import subprocess subprocess.call(["xdg-open", args.pdf]) + + diff --git a/systemtests/run_test.py b/systemtests/run_test.py deleted file mode 100755 index d5f80f554..000000000 --- a/systemtests/run_test.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python3 -#!/usr/bin/env python3 -from subprocess import Popen, PIPE, TimeoutExpired -import time -import os -import signal -from mcap_handler import McapHandler -from datetime import datetime -from plotter_class import Plotter -from pathlib import Path -import shutil -import atexit - - -def clean_process(process:Popen) -> int : - '''Kills process and its children on exit if they aren't already terminated (called with atexit). Returns 0 on success, 1 on failure''' - - if process.poll() == None: - group_id = os.getpgid(process.pid) - print(f"cleaning process {group_id}") - os.killpg(group_id, signal.SIGTERM) - time.sleep(0.01) #necessary delay before first poll - i=0 - while i < 10 and process.poll() == None: #in case the process termination is lazy and takes some time, we wait up to 0.5 sec per process - if process.poll() != None: - return 0 #if we have a returncode-> it terminated - time.sleep(0.05) #if not wait a bit longer - if(i == 9): - print(f"Process group {process} with groupID {group_id} didn't terminate correctly") - return 1 #after 0.5s we stop waiting and consider it did not terminate correctly - return 0 - else: - return 0 #process already terminated - - -def record_start_and_clean(testname:str, max_wait:int, bagfolder:str): - '''Starts recording the /tf topic in a rosbag, starts the test, waits, closes the rosbag and terminate all processes. max_wait is the max amount of time you want to wait - before forcefully terminating the test flight script (in case it never finishes correctly). - 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:] - src = "source " + bagfolder[:index-9] + "install/setup.bash" # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" - - try: - command = f"{src} && ros2 bag record -s mcap -o {testname}_{bagname} /tf" - record_bag = Popen(command, shell=True, cwd=bagfolder, stderr=PIPE, stdout=True, - start_new_session=True, text=True, executable="/bin/bash") - atexit.register(clean_process, record_bag) - - command = f"{src} && ros2 run crazyflie_examples {testname}" - start_flight_test = Popen(command, shell=True, stderr=True, stdout=True, - start_new_session=True, text=True, executable="/bin/bash") - atexit.register(clean_process, start_flight_test) - - start_flight_test.wait(timeout=max_wait) #raise Timeoutexpired after max_wait seconds if start_flight_test didn't finish by itself - clean_process(start_flight_test) - clean_process(record_bag) - - except TimeoutExpired: #if max_wait is exceeded - clean_process(start_flight_test) - clean_process(record_bag) - - except KeyboardInterrupt: #if drone crashes, user can ^C to skip the waiting - clean_process(start_flight_test) - clean_process(record_bag) - - #if something went wrong with the bash command lines in Popen, print the error - if record_bag.stderr != None: - print(testname," record_bag stderr: ", record_bag.stderr.readlines()) - if start_flight_test.stderr != None: - print(testname," start_flight flight stderr: ", start_flight_test.stderr.readlines()) - - -def translate_and_plot(testname:str, bagfolder:str): - '''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" - else: - print("run_test.py : test file not defined") - plotter = Plotter() - plotter.create_figures(test_file, rosbag_csv, output_pdf) #plot the data - - - -if __name__ == "__main__": - - path = Path(__file__) #Path(__file__) in this case "/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 if they exist - if(Path(path.parents[3].joinpath("bagfiles")).exists()): - shutil.rmtree(path.parents[3].joinpath("bagfiles")) - if(Path(path.parents[3].joinpath("results")).exists()): - shutil.rmtree(path.parents[3].joinpath("results")) - if(Path(Path.home() / ".ros/log").exists()): - shutil.rmtree(Path.home() / ".ros/log") - - #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 = str((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, - start_new_session=True, executable="/bin/bash") - atexit.register(clean_process, launch_crazyswarm) #atexit helps us to make sure processes are cleaned even if script exits unexpectedly - - time.sleep(1) - record_start_and_clean("figure8", 20, bagfolder) - record_start_and_clean("multi_trajectory", 80, bagfolder) - - clean_process(launch_crazyswarm) #kill crazyswarm and all of its child processes - - - #test done, now we create the results pdf - translate_and_plot("figure8", bagfolder) - translate_and_plot("multi_trajectory", bagfolder) - - exit(0) diff --git a/systemtests/test_flights.py b/systemtests/test_flights.py new file mode 100644 index 000000000..f2ce6cd05 --- /dev/null +++ b/systemtests/test_flights.py @@ -0,0 +1,162 @@ +import unittest + +from pathlib import Path +import shutil +import os +from plotter_class import Plotter +from mcap_handler import McapHandler +from subprocess import Popen, PIPE, TimeoutExpired +import time +import signal +import atexit + +############################# + +def setUpModule(): + + path = Path(__file__) #Path(__file__) in this case "/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/src/crazyswarm2/systemtests/test_flights.py" ; path.parents[0]=.../systemstests + + #delete results, logs and bags of previous experiments if they exist + if(Path(path.parents[3] / "results").exists()): + shutil.rmtree(path.parents[3] / "results") + + os.makedirs(path.parents[3] / "results") # /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/results + +def tearDownModule(): + pass + + +def clean_process(process:Popen) -> int : + '''Kills process and its children on exit if they aren't already terminated (called with atexit). Returns 0 on termination, 1 if SIGKILL was needed''' + if process.poll() == None: + group_id = os.getpgid(process.pid) + print(f"cleaning process {group_id}") + os.killpg(group_id, signal.SIGTERM) + time.sleep(0.01) #necessary delay before first poll + i=0 + while i < 10 and process.poll() == None: #in case the process termination is lazy and takes some time, we wait up to 0.5 sec per process + if process.poll() != None: + return 0 #if we have a returncode-> it terminated + time.sleep(0.05) #if not wait a bit longer + if(i == 9): + os.killpg(group_id, signal.SIGKILL) + return 1 #after 0.5s we stop waiting, consider it did not terminate correctly and kill it + return 0 + else: + return 0 #process already terminated + + +class TestFlights(unittest.TestCase): + + def __init__(self, methodName: str = "runTest") -> None: + super().__init__(methodName) + self.test_file = None + self.launch_crazyswarm : Popen = None + self.ros2_ws = Path(__file__).parents[3] #/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws + + def idFolderName(self): + return self.id().split(".")[-1] #returns the name of the test_function currently being run, for example "test_figure8" + + # runs once per test_ function + def setUp(self): + + if(Path(Path.home() / ".ros/log").exists()): #delete log files of the previous test + shutil.rmtree(Path.home() / ".ros/log") + self.test_file = None + + # launch server + src = "source " + str(Path(__file__).parents[3] / "install/setup.bash") # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" + command = f"{src} && ros2 launch crazyflie launch.py" + self.launch_crazyswarm = Popen(command, shell=True, stderr=True, stdout=PIPE, text=True, + start_new_session=True, executable="/bin/bash") + atexit.register(clean_process, self.launch_crazyswarm) #atexit helps us to make sure processes are cleaned even if script exits unexpectedly + time.sleep(1) + + + # runs once per test_ function + def tearDown(self) -> None: + clean_process(self.launch_crazyswarm) #kill crazyswarm_server and all of its child processes + + # copy .ros/log files to results folder + if Path(Path.home() / ".ros/log").exists(): + shutil.copytree(Path.home() / ".ros/log", Path(__file__).parents[3] / f"results/{self.idFolderName()}/roslogs") + + return super().tearDown() + + + + def record_start_and_clean(self, testname:str, max_wait:int): + '''Starts recording the /tf topic in a rosbag, starts the test, waits, closes the rosbag and terminate all processes. max_wait is the max amount of time you want to wait + before forcefully terminating the test flight script (in case it never finishes correctly). + NB the testname must be the name of the crayzflie_examples executable (ie the CLI grammar "ros2 run crazyflie_examples testname" must be valid)''' + + src = f"source {str(self.ros2_ws)}/install/setup.bash" + try: + command = f"{src} && ros2 bag record -s mcap -o test_{testname} /tf" + record_bag = Popen(command, shell=True, stderr=PIPE, stdout=True, text=True, + cwd= self.ros2_ws / "results/", start_new_session=True, executable="/bin/bash") + atexit.register(clean_process, record_bag) + + command = f"{src} && ros2 run crazyflie_examples {testname}" + start_flight_test = Popen(command, shell=True, stderr=True, stdout=True, + start_new_session=True, text=True, executable="/bin/bash") + atexit.register(clean_process, start_flight_test) + + start_flight_test.wait(timeout=max_wait) #raise Timeoutexpired after max_wait seconds if start_flight_test didn't finish by itself + clean_process(start_flight_test) + clean_process(record_bag) + + except TimeoutExpired: #if max_wait is exceeded + clean_process(start_flight_test) + clean_process(record_bag) + + except KeyboardInterrupt: #if drone crashes, user can ^C to skip the waiting + clean_process(start_flight_test) + clean_process(record_bag) + + #if something went wrong with the bash command lines in Popen, print the error + if record_bag.stderr != None: + print(testname," record_bag stderr: ", record_bag.stderr.readlines()) + if start_flight_test.stderr != None: + print(testname," start_flight flight stderr: ", start_flight_test.stderr.readlines()) + + + def translate_plot_and_check(self, testname:str) -> bool : + '''Translates rosbag .mcap format to .csv, then uses that csv to plot a pdf. Checks the deviation between ideal and real trajectories, i.e if the drone + successfully followed its given trajectory. Returns True if deviation < epsilon(defined in plotter_class.py) at every timestep, false if not. ''' + + # NB : the mcap filename is almost the same as the folder name but has _0 at the end + inputbag = f"{str(self.ros2_ws)}/results/test_{testname}/test_{testname}_0.mcap" + output_csv = f"{str(self.ros2_ws)}/results/test_{testname}/test_{testname}_0.csv" + + writer = McapHandler() + writer.write_mcap_to_csv(inputbag, output_csv) #translate bag from mcap to csv + output_pdf = f"{str(self.ros2_ws)}/results/test_{testname}/results_{testname}.pdf" + rosbag_csv = output_csv + + plotter = Plotter() + plotter.create_figures(self.test_file, rosbag_csv, output_pdf) #plot the data + return plotter.test_passed() + + + + def test_figure8(self): + self.test_file = "../crazyflie_examples/crazyflie_examples/data/figure8.csv" + # run test + self.record_start_and_clean("figure8", 20) + #create the plot etc + test_passed = self.translate_plot_and_check("figure8") + assert test_passed, "figure8 test failed : deviation larger than epsilon" + + def test_multi_trajectory(self): + self.test_file = "../crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv" + self.record_start_and_clean("multi_trajectory", 80) + test_passed = self.translate_plot_and_check("multi_trajectory") + assert test_passed, "multitrajectory test failed : deviation larger than epsilon" + + + + +if __name__ == '__main__': + unittest.main() +