latest version

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)

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]