Addressed wolfgang's comments on draft #356

.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
 
 
 

.gitignore

@@ -3,3 +3,5 @@ crazyswarm2_sim_webots/
 *.pyc
 *.o
 *~
+.vscode/
+experiments.py

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:

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)
 

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 :

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()