added all of the changes wolfgang asked (except shutil); saved the pd…

…f in /results folder instead of /bagfiles; made the two tests run consecutively and create two separate PDFs (will merge them later); modified workflow to 1: not delete and recreate the worskspace everytime (hope I don't break everything) 2: upload results, logs and bagfiles

.github/workflows/systemtests.yml

@@ -9,12 +9,10 @@ on:
 jobs:
  build:
  runs-on: self-hosted
- steps:
+ steps:test_file, rosbag_csv, output_pdf
  - name: Create workspace
  run: |
- pwd
- rm -rf ros2_ws
- mkdir -p ros2_ws/src
+ cd ros2_ws/src || mkdir -p ros2_ws/src
  - name: Checkout motion capture package
  run: |
  cd ros2_ws/src
@@ -41,8 +39,22 @@ jobs:
  - name: Upload files
  uses: actions/upload-artifact@v3
  with:
- name: bagfiles
- path: ros2_ws/bagfiles/
+ name: pdf_logs_bagfiles
+ path: |
+ ros2_ws/bagfiles
+ ros2_ws/results
+ .ros/log
+ name:
+ path: ros2_ws/bagfiles
+ with:
+ name: results
+ path: ros2_ws/results/
+ with: 
+ name: logs
+ path: .ros/log/
+
+
+
 
  # - name: build and test ROS 2
  # uses: ros-tooling/action-ros-ci@v0.3

systemtests/mcap_read_write.py → systemtests/mcap_handler.py

@@ -33,6 +33,9 @@ def typename(topic_name):
  del reader
 
  def write_mcap_to_csv(self, inputbag:str, outputfile:str):
+ '''A method which translates an .mcap rosbag file format to a .csv file. 
+ Only written to translate the /tf topic but could easily be extended to other topics'''
+
  try:
  print("Translating .mcap to .csv")
  f = open(outputfile, 'w+')
@@ -48,6 +51,14 @@ def write_mcap_to_csv(self, inputbag:str, outputfile:str):
 
 
 if __name__ == "__main__":
- yo = McapHandler()
- # # yo.write_mcap_to_csv('Home/julien/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/bag_06_11_2023-17:11:23_0.mcap','Home/julien/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/bag_06_11_2023-17:11:23_0.csv')
- yo.write_mcap_to_csv('/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multibag_0.mcap','/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multibag_0.csv')
+
+ #command line utility 
+
+ from argparse import ArgumentParser, Namespace
+ parser = ArgumentParser(description="Translates the /tf topic of an .mcap rosbag file format to a .csv file")
+ parser.add_argument("inputbag", type=str, help="The .mcap rosbag file to be translated")
+ parser.add_argument("outputfile", type=str, help="Output csv file that has to be created/overwritten")
+ args:Namespace = parser.parse_args()
+
+ translator = McapHandler()
+ translator.write_mcap_to_csv(args.inputbag,args.outputfile)

systemtests/newsub.py

@@ -3,50 +3,74 @@
 import time
 import os
 import signal
-from mcap_read_write import McapHandler
+from systemtests.mcap_handler import McapHandler
 from datetime import datetime
-from pathlib import Path
 from plotter_class import Plotter
 
 
 if __name__ == "__main__":
 
+ bagname = 'bag_' + datetime.now().strftime('%d_%m_%Y-%H_%M_%S')
+ f8_bagname = "fig8_" + bagname
+ mt_bagname = "multitraj_" + bagname
+
  command = "source install/setup.bash && 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(3)
- bagname = 'bag_' + datetime.now().strftime('%d_%m_%Y-%H_%M_%S')
- command = f"mkdir bagfiles && cd bagfiles && ros2 bag record -s mcap -o {bagname} /tf"
- start_rosbag = Popen(command, shell=True, stderr=True, 
+ time.sleep(1)
+ command = f"mkdir results && mkdir bagfiles && mkdir bagfiles/{bagname}"
+ create_dirs = Popen(command, shell=True, stderr=True, 
+ stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid) 
+
+ command = f"ros2 bag record -s mcap -o {f8_bagname} /tf"
+ record_fig8_bag = Popen(command, shell=True, stderr=True, cwd=f"bagfiles/{bagname}",
  stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid) 
+
  command = "ros2 run crazyflie_examples figure8"
- start_test = Popen(command, shell=True, stderr=True, 
+ start_fig8 = Popen(command, shell=True, stderr=True, 
+ stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid) 
+ time.sleep(20)
+ 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
+
+ command = f"ros2 bag record -s mcap -o {mt_bagname} /tf"
+ record_multitraj_bag = Popen(command, shell=True, stderr=True, 
+ stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid) 
+
+ command = "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(30)
- os.killpg(os.getpgid(start_test.pid), signal.SIGTERM) #kill start_test process and all of its child processes
- os.killpg(os.getpgid(start_rosbag.pid), signal.SIGTERM) #kill rosbag
+ time.sleep(80)
+ os.killpg(os.getpgid(start_multitraj.pid), signal.SIGTERM)
+ os.killpg(os.getpgid(record_multitraj_bag), 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
 
  #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
  writer = McapHandler()
- #path = str(Path(__file__).parent /"bags/tfbag.mcap")
-
- ### attention le bagname est different du bagfile par un _0 
- inputbag = "bagfiles/" + bagname + '/' + bagname + '_0' + '.mcap'
- output_csv = "bagfiles/" + bagname + '/' + bagname + '_0' + '.csv'
 
+ 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 = "bagfiles/" + bagname + '/' + 'Results_'+ datetime.now().strftime('%d_%m_%Y-%H_%M_%S')
-
+ 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"
- paul = Plotter()
- paul.create_figures(test_file, rosbag_csv, output_pdf)
+ plotter = Plotter()
+ plotter.create_figures(test_file, rosbag_csv, output_pdf)
+
+
+ inputbag = "bagfiles/" + bagname + '/' + mt_bagname + '/' + mt_bagname + '_0' + '.mcap'
+ output_csv = "bagfiles/" + bagname + '/' + mt_bagname + '/' + mt_bagname + '_0' + '.csv'
+ writer.write_mcap_to_csv(inputbag, output_csv)
+ output_pdf_2 = "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)
 
  exit()

systemtests/plotter_class.py

@@ -1,25 +1,14 @@
-
-# import cfusdlog
 import matplotlib.pyplot as plt
 import os
 import sys 
-import yaml
 from matplotlib.backends.backend_pdf import PdfPages
 import numpy as np
-
-# import model #dennis module
-
-#my stuff
 from crazyflie_py.uav_trajectory import Trajectory
-from pathlib import Path
-import pandas as pd
-from mcap_read_write import McapHandler
-from mpl_toolkits.mplot3d import Axes3D
+
 
 class Plotter:
 
  def __init__(self):
- self.output_dir = ''
  self.params = {'experiment':'1','trajectory':'figure8.csv','motors':'standard motors(CF)', 'propellers':'standard propellers(CF)'}
  self.bag_times = np.empty([0])
  self.bag_x = np.empty([0])
@@ -29,7 +18,9 @@ def __init__(self):
  self.ideal_traj_y = np.empty([0])
  self.ideal_traj_z = np.empty([0])
  self.euclidian_dist = np.empty([0])
+ 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 = 0.5 #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
@@ -55,7 +46,6 @@ 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
  print("Plotting fig8 test data")
@@ -66,21 +56,20 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile):
  #get ideal trajectory data
  self.ideal_traj_csv = Trajectory()
  try:
- self.ideal_traj_csv.loadcsv(Path(__file__).parent / ideal_csvfile,)
+ path_to_ideal_csv = os.path.join(os.path.dirname(os.path.abspath(__file__)),ideal_csvfile)
+ self.ideal_traj_csv.loadcsv(path_to_ideal_csv)
  except FileNotFoundError:
- print("Plotter : File not found " + str(Path(__file__).parent / ideal_csvfile,))
+ print("Plotter : File not found " + path_to_ideal_csv)
  exit(1)
 
 
  #get rosbag data
- rosbag_data = pd.read_csv(rosbag_csvfile, names=['time','x','y','z'])
+ rosbag_data = np.loadtxt(rosbag_csvfile, delimiter=",")
 
-
- #we have to split the pandas dataframe into separate numpy arrays to plot them later
- self.bag_times = rosbag_data['time'].to_numpy()
- self.bag_x = rosbag_data['x'].to_numpy()
- self.bag_y = rosbag_data['y'].to_numpy()
- self.bag_z = rosbag_data['z'].to_numpy()
+ self.bag_times = np.array(rosbag_data[:,0])
+ self.bag_x = np.array(rosbag_data[:,1])
+ self.bag_y = np.array(rosbag_data[:,2])
+ self.bag_z = np.array(rosbag_data[:,3])
 
  self.adjust_arrays()
  bag_arrays_size = len(self.bag_times)
@@ -112,11 +101,12 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile):
 
  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]])
+ if self.euclidian_dist[i] > self.EPSILON:
+ self.deviation.append(i)
 
  self.no_match_warning(no_match_in_idealcsv)
 
 
-
  def no_match_warning(self, unmatched_values:list):
  ''' A method which prints a warning saying how many (if any) recorded datapoints could not be matched to an ideal datapoint'''
 
@@ -192,17 +182,18 @@ def adjust_arrays(self):
 
 
 
- def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname_and_path:str):
+ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str):
  '''Method that creates the pdf with the plots'''
 
  self.read_csv_and_set_arrays(ideal_csvfile,rosbag_csvfile)
 
  #create PDF to save figures
- pdf_path = self.output_dir + pdfname_and_path + '.pdf'
+ if(pdfname[-4:] != ".pdf"):
+ pdfname= pdfname + '.pdf'
 
  #check if user wants to overwrite the report file
- # self.file_guard(pdf_path)
- pdf_pages = PdfPages(pdf_path)
+ self.file_guard(pdfname)
+ pdf_pages = PdfPages(pdfname)
 
  #create title page
  text = 'figure8 trajectory test'
@@ -217,10 +208,8 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname_and_path
  fig.text(0.1, 0.1, title_text, size=11)
  pdf_pages.savefig(fig)
 
-
- #plt.style.use('ggplot')
 
-  #create plots
+ #create plots
  fig, ax = plt.subplots()
  ax.plot(self.bag_times, self.ideal_traj_x, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10)
  ax.plot(self.bag_times, self.bag_x, label='Recorded trajectory')
@@ -303,56 +292,40 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname_and_path
 
 
 
-
-
  pdf_pages.close()
- print("Result saved in " + pdfname_and_path + '.pdf')
-
-
- # def create_pdf(self, ideal_csvfile:str, rosbag:str, pdfname_and_path:str):
- # pdf_pages = PdfPages(self.output_dir + pdfname_and_path + '.pdf')
 
+ self.test_passed()
 
- #  fig8_ideal_csv = "/home/jthevenoz/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/figure8.csv"
+ print("Results saved in " + pdfname)
 
- # index = rosbag.find("bag_")
- # bagname = rosbag[index:] 
- # print(bagname)
- # fig8_recorded_csv_filepath = rosbag + "fig8_" + bagname + "fig8_" + bagname[:bagname.index("/")] + "_0.mcap"
- # print(fig8_recorded_csv_filepath)
-
-
-
- # create_figures(fig8_recorded_csv_filepath,fig8_ideal_csv)
-
- # multitraj_ideal_csv = "/home/julien/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv"
- # create_figure(multitraj_ideal_csv)
-
- # pdf_pages.close()
- # print("Result saved in " + pdfname_and_path + '.pdf')
+ def test_passed(self) -> bool :
+ '''Returns True if the deviation between recorded and ideal trajectories of the drone always stayed lower 
+ than EPSILON. Returns False otherwise '''
 
+ nb_dev_points = len(self.deviation)
 
+ if nb_dev_points == 0:
+ print("Test passed")
+ return True
+ else:
+ print(f"The deviation between ideal and recorded trajectories is greater than {self.EPSILON}m for {nb_dev_points}"
+ f"datapoints, which corresponds to {nb_dev_points*0.01}s")
+ return False
 
 if __name__=="__main__":
- #####writing rosbag to csv
- # writer = McapHandler()
- # path = str(Path(__file__).parent /"bags/tfbag.mcap")
- # writer.write_bag_to_csv(path, "tf_rosbag.csv")
-
-
-
- # ideal_csv = "/home/jthevenoz/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/figure8.csv"
- # rosbagcsv = "/home/jthevenoz/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/bag_06_11_2023-17:11:23_0.csv"
- # pdf = "/home/jthevenoz/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/testingtheclass"
-
- ideal_csv_multi = "/home/julien/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv"
- ideal_csv_fig8 = "/home/julien/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/figure8.csv"
- rosbagcsv_multi = "/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multibag_0.csv"
- rosbagcsv_fig8 = "/"
- pdf = "/home/julien/Desktop/IMRC/IMRC-shared5/bags/multibag/multitesting"
-
- paul = Plotter()
- paul.create_figures(ideal_csv_multi,rosbagcsv_multi, pdf)
- import subprocess
- ####directly open the pdf is nice
- subprocess.call(["xdg-open", "/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multitesting.pdf"])
+
+ #command line utility 
+
+ from argparse import ArgumentParser, Namespace
+ parser = ArgumentParser(description="Creates a pdf plotting the recorded trajectory of a drone against its desired trajectory")
+ parser.add_argument("desired_trajectory", type=str, help=".csv file containing (time,x,y,z) of the ideal/desired drone trajectory")
+ parser.add_argument("recorded_trajectory", type=str, help=".csv file containing (time,x,y,z) of the recorded drone trajectory")
+ parser.add_argument("pdf", type=str, help="name of the pdf file you want to create/overwrite")
+ parser.add_argument("--open", help="Open the pdf directly after it is created", action="store_true")
+ args : Namespace = parser.parse_args()
+
+ plotter = Plotter()
+ plotter.create_figures(args.desired_trajectory, args.recorded_trajectory, args.pdf)
+ if args.open:
+ import subprocess
+ subprocess.call(["xdg-open", args.pdf])