From a0b7b9c455bd6a76cd1740faa1dd3bcb926897b1 Mon Sep 17 00:00:00 2001 From: daniel Date: Sun, 9 Feb 2025 23:57:10 -0600 Subject: [PATCH 01/11] upload rosbag to onedrive --- GEMstack/onboard/execution/logging.py | 73 ++++++++++++++++++++++++--- 1 file changed, 66 insertions(+), 7 deletions(-) diff --git a/GEMstack/onboard/execution/logging.py b/GEMstack/onboard/execution/logging.py index 31ebd962b..e1ec8b74d 100644 --- a/GEMstack/onboard/execution/logging.py +++ b/GEMstack/onboard/execution/logging.py @@ -8,7 +8,8 @@ import subprocess import numpy as np import cv2 - +import requests +from msal import PublicClientApplication class LoggingManager: """A top level manager of the logging process. This is responsible for creating log folders, log metadata files, and for replaying components from log @@ -129,11 +130,19 @@ def log_components(self,components : List[str]) -> None: def log_ros_topics(self, topics : List[str], rosbag_options : str = '') -> Optional[str]: if topics: - command = ['rosbag','record','--output-name={}'.format(os.path.join(self.log_folder,'vehicle.bag'))] - command += rosbag_options.split() - command += topics - self.rosbag_process = subprocess.Popen(command, stdin=subprocess.PIPE, stdout=subprocess.PIPE) - return ' '.join(command) + rosbag_command = 'rosbag record --output-name={} {} {}'.format( + os.path.join(self.log_folder, 'vehicle.bag'), + rosbag_options, + ' '.join(topics) + ) + full_command = f'source catkin_ws/devel/setup.bash && {rosbag_command}' + + # Run the command in a bash shell + self.rosbag_process = subprocess.Popen( + ['bash', '-c', full_command], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE + ) return None def set_vehicle_time(self, vehicle_time : float) -> None: @@ -207,7 +216,7 @@ def dump_debug(self): else: isevent[col] = True f.write(','.join(columns)+'\n') - nrows = max(len(v[col]) for col in v) + nrows = max((len(v[col]) for col in v), default=0) for i in range(nrows): row = [] for col,vals in v.items(): @@ -263,6 +272,8 @@ def log_component_stderr(self, component : str, msg : List[str]) -> None: self.component_output_loggers[component][1].write(timestr + ': ' + l + '\n') def close(self): + + self.dump_debug() self.debug_messages = {} if self.rosbag_process is not None: @@ -276,6 +287,54 @@ def close(self): print('Log file size in MegaBytes is {}'.format(loginfo.st_size / (1024 * 1024))) print('-------------------------------------------') self.rosbag_process = None + + + + record_bag = input("Do you want to upload this Rosbag? Y/N (default: Y): ") or "Y" + + + if(record_bag not in ["N", "no", "n", "No"]): + + # Azure App credentials (trying not to hardcode) + # export CLIENT_ID= "" + # export TENANT_ID ="" + CLIENT_ID = os.getenv("CLIENT_ID") + TENANT_ID = os.getenv("TENANT_ID") + AUTHORITY = f'https://login.microsoftonline.com/{TENANT_ID}' + SCOPES = ['Files.ReadWrite.All'] + + app = PublicClientApplication(CLIENT_ID, authority=AUTHORITY) + accounts = app.get_accounts() + + + if accounts: + result = app.acquire_token_silent(SCOPES, account=accounts[0]) + else: + print("Opening Authentication Window") + + result = app.acquire_token_interactive(SCOPES) + + if 'access_token' in result: + access_token = result['access_token'] + headers = { + 'Authorization': f'Bearer {access_token}', + 'Content-Type': 'application/octet-stream' + } + file_path = os.path.join(self.log_folder,'vehicle.bag') + file_name = os.path.basename(file_path) + + upload_url = f'https://graph.microsoft.com/v1.0/me/drive/root:/{file_name}:/content' + + with open(file_path, 'rb') as file: + response = requests.put(upload_url, headers=headers, data=file) + + if response.status_code == 201 or response.status_code == 200: + print(f"✅ Successfully uploaded '{file_name}' to OneDrive.") + else: + print(f"❌ Upload failed: {response.status_code} - {response.text}") + else: + print("❌ Authentication failed.") + def __del__(self): self.close() From d2fc472e03663b5045d7cf90002e6950bffcd830 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Mon, 10 Feb 2025 11:47:23 -0600 Subject: [PATCH 02/11] update name --- GEMstack/onboard/execution/logging.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/GEMstack/onboard/execution/logging.py b/GEMstack/onboard/execution/logging.py index e1ec8b74d..1a205ae18 100644 --- a/GEMstack/onboard/execution/logging.py +++ b/GEMstack/onboard/execution/logging.py @@ -136,7 +136,6 @@ def log_ros_topics(self, topics : List[str], rosbag_options : str = '') -> Optio ' '.join(topics) ) full_command = f'source catkin_ws/devel/setup.bash && {rosbag_command}' - # Run the command in a bash shell self.rosbag_process = subprocess.Popen( ['bash', '-c', full_command], @@ -320,10 +319,10 @@ def close(self): 'Authorization': f'Bearer {access_token}', 'Content-Type': 'application/octet-stream' } - file_path = os.path.join(self.log_folder,'vehicle.bag') + file_path = os.path.join(self.log_folder, 'vehicle.bag') + print(self.log_folder[1]) file_name = os.path.basename(file_path) - - upload_url = f'https://graph.microsoft.com/v1.0/me/drive/root:/{file_name}:/content' + upload_url = f'https://graph.microsoft.com/v1.0/me/drive/root:/Rosbags/{self.log_folder[5:]+ " " + file_name}:/content' with open(file_path, 'rb') as file: response = requests.put(upload_url, headers=headers, data=file) From 9d336fd4b8f9e7e1f2a4aa6881bd3dee62f247e6 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Tue, 11 Feb 2025 12:44:31 -0600 Subject: [PATCH 03/11] change to share with me --- GEMstack/onboard/execution/logging.py | 33 ++++++++++----------------- 1 file changed, 12 insertions(+), 21 deletions(-) diff --git a/GEMstack/onboard/execution/logging.py b/GEMstack/onboard/execution/logging.py index 1a205ae18..c5ada03b8 100644 --- a/GEMstack/onboard/execution/logging.py +++ b/GEMstack/onboard/execution/logging.py @@ -130,18 +130,11 @@ def log_components(self,components : List[str]) -> None: def log_ros_topics(self, topics : List[str], rosbag_options : str = '') -> Optional[str]: if topics: - rosbag_command = 'rosbag record --output-name={} {} {}'.format( - os.path.join(self.log_folder, 'vehicle.bag'), - rosbag_options, - ' '.join(topics) - ) - full_command = f'source catkin_ws/devel/setup.bash && {rosbag_command}' - # Run the command in a bash shell - self.rosbag_process = subprocess.Popen( - ['bash', '-c', full_command], - stdin=subprocess.PIPE, - stdout=subprocess.PIPE - ) + command = ['rosbag','record','--output-name={}'.format(os.path.join(self.log_folder,'vehicle.bag'))] + command += rosbag_options.split() + command += topics + self.rosbag_process = subprocess.Popen(command, stdin=subprocess.PIPE, stdout=subprocess.PIPE) + return ' '.join(command) return None def set_vehicle_time(self, vehicle_time : float) -> None: @@ -294,22 +287,21 @@ def close(self): if(record_bag not in ["N", "no", "n", "No"]): - # Azure App credentials (trying not to hardcode) - # export CLIENT_ID= "" - # export TENANT_ID ="" - CLIENT_ID = os.getenv("CLIENT_ID") - TENANT_ID = os.getenv("TENANT_ID") + CLIENT_ID = '845ade48-ce2e-49d3-ab66-f0419a3460f0' + TENANT_ID = "44467e6f-462c-4ea2-823f-7800de5434e3" + + AUTHORITY = f'https://login.microsoftonline.com/{TENANT_ID}' SCOPES = ['Files.ReadWrite.All'] app = PublicClientApplication(CLIENT_ID, authority=AUTHORITY) accounts = app.get_accounts() + print("Opening Authentication Window") if accounts: result = app.acquire_token_silent(SCOPES, account=accounts[0]) else: - print("Opening Authentication Window") result = app.acquire_token_interactive(SCOPES) @@ -320,9 +312,8 @@ def close(self): 'Content-Type': 'application/octet-stream' } file_path = os.path.join(self.log_folder, 'vehicle.bag') - print(self.log_folder[1]) - file_name = os.path.basename(file_path) - upload_url = f'https://graph.microsoft.com/v1.0/me/drive/root:/Rosbags/{self.log_folder[5:]+ " " + file_name}:/content' + file_name = self.log_folder[5:]+ "_" + os.path.basename(file_path) + upload_url = f'https://graph.microsoft.com/v1.0/me/drive/sharedWithMe/Rosbags/{file_name}:/content' with open(file_path, 'rb') as file: response = requests.put(upload_url, headers=headers, data=file) From 86b968cf167d01bd256ab9fa2cc545a37b8bf8b8 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Tue, 11 Feb 2025 13:42:59 -0600 Subject: [PATCH 04/11] url fix --- GEMstack/onboard/execution/logging.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/execution/logging.py b/GEMstack/onboard/execution/logging.py index c5ada03b8..7e8a84402 100644 --- a/GEMstack/onboard/execution/logging.py +++ b/GEMstack/onboard/execution/logging.py @@ -313,7 +313,7 @@ def close(self): } file_path = os.path.join(self.log_folder, 'vehicle.bag') file_name = self.log_folder[5:]+ "_" + os.path.basename(file_path) - upload_url = f'https://graph.microsoft.com/v1.0/me/drive/sharedWithMe/Rosbags/{file_name}:/content' + upload_url = f'https://graph.microsoft.com/v1.0/drives/b!r8UV8D4x3E2BtZZ7BCsmXkluecl4_LtGks5ml-JzZoIsKNgi6n5kSYav_vojyk-B/items/01H5P3RBES56VFCT3I6NAISSB4OPNNFDDB:/{file_name}:/content' with open(file_path, 'rb') as file: response = requests.put(upload_url, headers=headers, data=file) From 82ef25246f943690ecaf9dc4ef6e80ceb2634ece Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Tue, 11 Feb 2025 20:39:16 -0600 Subject: [PATCH 05/11] moved url to config file --- GEMstack/onboard/execution/logging.py | 27 ++++++++++++++++++++++----- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/GEMstack/onboard/execution/logging.py b/GEMstack/onboard/execution/logging.py index 7e8a84402..31c3d6f55 100644 --- a/GEMstack/onboard/execution/logging.py +++ b/GEMstack/onboard/execution/logging.py @@ -10,6 +10,7 @@ import cv2 import requests from msal import PublicClientApplication +import json class LoggingManager: """A top level manager of the logging process. This is responsible for creating log folders, log metadata files, and for replaying components from log @@ -286,10 +287,23 @@ def close(self): if(record_bag not in ["N", "no", "n", "No"]): - - CLIENT_ID = '845ade48-ce2e-49d3-ab66-f0419a3460f0' - TENANT_ID = "44467e6f-462c-4ea2-823f-7800de5434e3" - + def load_config(config_path="onedrive_config.json"): + try: + with open(config_path, "r") as f: + config = json.load(f) + return config + except Exception as e: + print(f"Error loading configuration file: {e}") + exit(1) + + config = load_config() + + # Not private but for reusability in future semesters: + # Retrieve values from the config + CLIENT_ID = config.get("CLIENT_ID") + TENANT_ID = config.get("TENANT_ID") + DRIVE_ID = config.get("DRIVE_ID") + ITEM_ID = config.get("ITEM_ID") AUTHORITY = f'https://login.microsoftonline.com/{TENANT_ID}' SCOPES = ['Files.ReadWrite.All'] @@ -313,7 +327,10 @@ def close(self): } file_path = os.path.join(self.log_folder, 'vehicle.bag') file_name = self.log_folder[5:]+ "_" + os.path.basename(file_path) - upload_url = f'https://graph.microsoft.com/v1.0/drives/b!r8UV8D4x3E2BtZZ7BCsmXkluecl4_LtGks5ml-JzZoIsKNgi6n5kSYav_vojyk-B/items/01H5P3RBES56VFCT3I6NAISSB4OPNNFDDB:/{file_name}:/content' + upload_url = ( + f'https://graph.microsoft.com/v1.0/drives/{DRIVE_ID}/items/' + f'{ITEM_ID}:/{file_name}:/content' + ) with open(file_path, 'rb') as file: response = requests.put(upload_url, headers=headers, data=file) From 8b9b24134c1dc86ee3d75a15eb90cc4d42b7a12e Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Sun, 23 Feb 2025 10:31:52 -0600 Subject: [PATCH 06/11] component replay fix and added ros topic replay --- .gitignore | 6 +- GEMstack/onboard/execution/entrypoint.py | 1 + GEMstack/onboard/execution/execution.py | 16 +- GEMstack/onboard/execution/logging.py | 216 ++++++++++++++++------- GEMstack/state/vehicle.py | 2 +- GEMstack/utils/klampt_visualization.py | 2 +- GEMstack/utils/logging.py | 2 +- launch/fixed_route.yaml | 8 +- launch/gather_data.yaml | 4 +- 9 files changed, 179 insertions(+), 78 deletions(-) diff --git a/.gitignore b/.gitignore index 5d34da6bb..32580a5d6 100644 --- a/.gitignore +++ b/.gitignore @@ -168,4 +168,8 @@ cython_debug/ .vscode/ setup/zed_sdk.run -cuda/ \ No newline at end of file +cuda/ +onedrive_config.json +zed_sdk.run +ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run +ros.asc diff --git a/GEMstack/onboard/execution/entrypoint.py b/GEMstack/onboard/execution/entrypoint.py index b1e7a8d10..39606ff91 100644 --- a/GEMstack/onboard/execution/entrypoint.py +++ b/GEMstack/onboard/execution/entrypoint.py @@ -98,6 +98,7 @@ def caution_callback(k,variant): logmeta = config.load_config_recursive(os.path.join(logfolder,'meta.yaml')) replay_topics = replay_settings.get('ros_topics',[]) + mission_executor.replay_topics(replay_topics,logfolder) #TODO: launch a roslog replay of the topics in ros_topics, disable in the vehicle interface for (name,s) in pipeline_settings.items(): diff --git a/GEMstack/onboard/execution/execution.py b/GEMstack/onboard/execution/execution.py index a65c2acc5..c08a2e962 100644 --- a/GEMstack/onboard/execution/execution.py +++ b/GEMstack/onboard/execution/execution.py @@ -389,7 +389,7 @@ def make_component(self, config_info, component_name, parent_module=None, extra_ raise if not isinstance(component,Component): raise RuntimeError("Component {} is not a subclass of Component".format(component_name)) - replacement = self.logging_manager.component_replayer(component_name, component) + replacement = self.logging_manager.component_replayer(self.vehicle_interface, component_name, component) if replacement is not None: executor_debug_print(1,"Replaying component {} from long {} with outputs {}",component_name,replacement.logfn,component.state_outputs()) component = replacement @@ -455,6 +455,14 @@ def replay_components(self, replayed_components : list, replay_folder : str): LogReplay objects. """ self.logging_manager.replay_components(replayed_components,replay_folder) + + def replay_topics(self, replayed_topics : list, replay_folder : str): + """Declare that the given components should be replayed from a log folder. + + Further make_component calls to this component will be replaced with + LogReplay objects. + """ + self.logging_manager.replay_topics(replayed_topics,replay_folder) def event(self,event_description : str, event_print_string : str = None): """Logs an event to the metadata and prints a message to the console.""" @@ -494,7 +502,7 @@ def run(self): #start running components for k,c in self.all_components.items(): c.start() - + #start running mission self.state = AllState.zero() self.state.mission.type = MissionEnum.IDLE @@ -559,6 +567,7 @@ def run(self): for k,c in self.all_components.items(): executor_debug_print(2,"Stopping",k) c.stop() + self.logging_manager.close() executor_debug_print(0,"Done with execution loop") @@ -639,6 +648,9 @@ def run_until_switch(self): self.state.t = self.vehicle_interface.time() self.logging_manager.set_vehicle_time(self.state.t) self.last_loop_time = time.time() + #publish ros topics + if(self.logging_manager.rosbag_player): + self.logging_manager.rosbag_player.update_topics(self.state.t) #check for vehicle faults self.check_for_hardware_faults() diff --git a/GEMstack/onboard/execution/logging.py b/GEMstack/onboard/execution/logging.py index 31c3d6f55..145709244 100644 --- a/GEMstack/onboard/execution/logging.py +++ b/GEMstack/onboard/execution/logging.py @@ -11,6 +11,8 @@ import requests from msal import PublicClientApplication import json +import rosbag +import rospy class LoggingManager: """A top level manager of the logging process. This is responsible for creating log folders, log metadata files, and for replaying components from log @@ -18,7 +20,12 @@ class LoggingManager: def __init__(self): self.log_folder = None # type: Optional[str] self.replayed_components = dict() # type Dict[str,str] + self.replayed_topics = dict() # type Dict[str,str] + self.rosbag_player = None + self.logged_components = set() # type: Set[str] + self.logged_topics = set() # type: Set[str] + self.component_output_loggers = dict() # type: Dict[str,list] self.behavior_log = None self.rosbag_process = None @@ -76,16 +83,40 @@ def replay_components(self, replayed_components : list, replay_folder : str): if c not in logged_components: raise ValueError("Replay component",c,"was not logged in",replay_folder,"(see settings.yaml)") self.replayed_components[c] = replay_folder + + def replay_topics(self, replayed_topics : list, replay_folder : str): + """Declare that the given components should be replayed from a log folder. + + Further make_component calls to this component will be replaced with + BagReplay objects. + """ + #sanity check: was this item logged? + settings = config.load_config_recursive(os.path.join(replay_folder,'settings.yaml')) + try: + logged_topics = settings['run']['log']['ros_topics'] + except KeyError: + logged_topics = [] + for c in replayed_topics: + if c not in logged_topics: + raise ValueError("Replay topic",c,"was not logged in",replay_folder,"'s vehicle.bag file (see settings.yaml)") + self.replayed_topics[c] = replay_folder + if(not self.rosbag_player): + self.rosbag_player = RosbagPlayer(replay_folder, self.replayed_topics) + + + + + - def component_replayer(self, component_name : str, component : Component) -> Optional[LogReplay]: + def component_replayer(self, vehicle_interface, component_name : str, component : Component) -> Optional[LogReplay]: if component_name in self.replayed_components: #replace behavior of class with the LogReplay class replay_folder = self.replayed_components[component_name] outputs = component.state_outputs() rate = component.rate() assert rate is not None and rate > 0, "Replayed component {} must have a positive rate".format(component_name) - return LogReplay(outputs, - os.path.join(replay_folder,'behavior_log.json'), + return LogReplay(vehicle_interface, outputs, + os.path.join(replay_folder,'behavior.json'), rate=rate) return None @@ -168,14 +199,14 @@ def debug(self, component : str, item : str, value : Any) -> None: os.mkdir(folder) filename = os.path.join(folder,item+'_%03d.npz'%len(self.debug_messages[component][item])) np.savez(filename,value) - elif isinstance(value,cv2.Mat): - #if really large, save as png - folder = os.path.join(self.log_folder,'debug_{}'.format(component)) - if item not in self.debug_messages[component]: - self.debug_messages[component][item] = [] - os.mkdir(folder) - filename = os.path.join(folder,item+'_%03d.png'%len(self.debug_messages[component][item])) - cv2.imwrite(filename,value) + # elif isinstance(value,cv2.Mat): + # #if really large, save as png + # folder = os.path.join(self.log_folder,'debug_{}'.format(component)) + # if item not in self.debug_messages[component]: + # self.debug_messages[component][item] = [] + # os.mkdir(folder) + # filename = os.path.join(folder,item+'_%03d.png'%len(self.debug_messages[component][item])) + # cv2.imwrite(filename,value) else: if item not in self.debug_messages[component]: self.debug_messages[component][item] = [] @@ -263,7 +294,7 @@ def log_component_stderr(self, component : str, msg : List[str]) -> None: timestr = datetime.datetime.fromtimestamp(self.vehicle_time).strftime("%H:%M:%S.%f")[:-3] for l in msg: self.component_output_loggers[component][1].write(timestr + ': ' + l + '\n') - + def close(self): @@ -287,65 +318,75 @@ def close(self): if(record_bag not in ["N", "no", "n", "No"]): - def load_config(config_path="onedrive_config.json"): - try: - with open(config_path, "r") as f: - config = json.load(f) - return config - except Exception as e: - print(f"Error loading configuration file: {e}") - exit(1) - - config = load_config() - - # Not private but for reusability in future semesters: - # Retrieve values from the config - CLIENT_ID = config.get("CLIENT_ID") - TENANT_ID = config.get("TENANT_ID") - DRIVE_ID = config.get("DRIVE_ID") - ITEM_ID = config.get("ITEM_ID") - - AUTHORITY = f'https://login.microsoftonline.com/{TENANT_ID}' - SCOPES = ['Files.ReadWrite.All'] - - app = PublicClientApplication(CLIENT_ID, authority=AUTHORITY) - accounts = app.get_accounts() - - print("Opening Authentication Window") - - if accounts: - result = app.acquire_token_silent(SCOPES, account=accounts[0]) - else: - - result = app.acquire_token_interactive(SCOPES) - - if 'access_token' in result: - access_token = result['access_token'] - headers = { - 'Authorization': f'Bearer {access_token}', - 'Content-Type': 'application/octet-stream' - } - file_path = os.path.join(self.log_folder, 'vehicle.bag') - file_name = self.log_folder[5:]+ "_" + os.path.basename(file_path) - upload_url = ( - f'https://graph.microsoft.com/v1.0/drives/{DRIVE_ID}/items/' - f'{ITEM_ID}:/{file_name}:/content' - ) - - with open(file_path, 'rb') as file: - response = requests.put(upload_url, headers=headers, data=file) - - if response.status_code == 201 or response.status_code == 200: - print(f"✅ Successfully uploaded '{file_name}' to OneDrive.") - else: - print(f"❌ Upload failed: {response.status_code} - {response.text}") - else: - print("❌ Authentication failed.") + self.upload_to_onedrive() + def __del__(self): self.close() +class OneDrive(): + def __init__(self): + self.config_found = False + try: + with open("onedrive_config.json", "r") as f: + config = json.load(f) + self.CLIENT_ID = config.get("CLIENT_ID") + self.TENANT_ID = config.get("TENANT_ID") + self.DRIVE_ID = config.get("DRIVE_ID") + self.ITEM_ID = config.get("ITEM_ID") + self.credentials = True + + except Exception as e: + print("No Onedrive config file found") + + + def upload_to_onedrive(self): + + + + # Not private but for reusability in future semesters: + # Retrieve values from the config + + + AUTHORITY = f'https://login.microsoftonline.com/{self.TENANT_ID}' + SCOPES = ['Files.ReadWrite.All'] + + app = PublicClientApplication(self.CLIENT_ID, authority=AUTHORITY) + accounts = app.get_accounts() + + print("Opening Authentication Window") + + if accounts: + result = app.acquire_token_silent(SCOPES, account=accounts[0]) + else: + + result = app.acquire_token_interactive(SCOPES) + + if 'access_token' in result: + access_token = result['access_token'] + headers = { + 'Authorization': f'Bearer {access_token}', + 'Content-Type': 'application/octet-stream' + } + file_path = os.path.join(self.log_folder, 'vehicle.bag') + file_name = self.log_folder[5:]+ "_" + os.path.basename(file_path) + upload_url = ( + f'https://graph.microsoft.com/v1.0/drives/{self.DRIVE_ID}/items/' + f'{self.ITEM_ID}:/{file_name}:/content' + ) + + with open(file_path, 'rb') as file: + response = requests.put(upload_url, headers=headers, data=file) + + if response.status_code == 201 or response.status_code == 200: + print(f"✅ Successfully uploaded '{file_name}' to OneDrive.") + else: + print(f"❌ Upload failed: {response.status_code} - {response.text}") + else: + print("❌ Authentication failed.") + + class LogReplay(Component): """Substitutes the output of a component with replayed data from a log file. @@ -400,6 +441,49 @@ def cleanup(self): self.logfile.close() +class RosbagPlayer: + def __init__(self, bag_path, topics): + self.bag = rosbag.Bag(os.path.join(bag_path, 'vehicle.bag'), 'r') + self.current_time = None # Track current position in bag + self.offset = -1 + + self.publishers = {} + for topic, msg, _ in self.bag.read_messages(): + if topic in topics and topic not in self.publishers: + msg_type = type(msg) + self.publishers[topic] = rospy.Publisher(topic, msg_type, queue_size=10) + rospy.loginfo(f"Created publisher for topic: {topic}") + print(f"Created publisher for topic: {topic}") + + + def update_topics(self, target_timestamp): + """ + Plays from the current position in the bag to the given timestamp. + :param target_timestamp: ROS time (rospy.Time) to play until + """ + self.prev_timestamp = target_timestamp + if self.offset <0: + self.offset = target_timestamp - self.bag.get_start_time() + if self.current_time is None: + self.current_time = self.bag.get_start_time() + + first_message = True + #rospy.loginfo(f"Playing from {self.current_time} to {target_timestamp}") + for topic, msg, t in self.bag.read_messages(start_time=rospy.Time(self.current_time )): + if t.to_sec() + self.offset > target_timestamp: + break # Stop when reaching the target time + if first_message and t.to_sec() == self.current_time: + first_message = False + continue + #rospy.loginfo(f'Publishing {msg} to {topic}') + if topic in self.publishers: # Only publish selected topics + self.publishers[topic].publish(msg) + rospy.sleep(0.01) # Simulate real-time playback + + self.current_time = t.to_sec() # Update current position + + def close(self): + self.bag.close() class VehicleBehaviorLogger(Component): def __init__(self,behavior_log, vehicle_interface): diff --git a/GEMstack/state/vehicle.py b/GEMstack/state/vehicle.py index 364046a71..7dd552193 100644 --- a/GEMstack/state/vehicle.py +++ b/GEMstack/state/vehicle.py @@ -29,7 +29,7 @@ class VehicleState: steering_wheel_angle : float #angle of the steering wheel, in radians front_wheel_angle : float #angle of the front wheels, in radians. Related to steering_wheel_angle by a fixed transform heading_rate : float #the rate at which the vehicle is turning, in radians/s. Related to v and front_wheel_angle by a fixed transform - gear : VehicleGearEnum #the current gear + gear : int #the current gear left_turn_indicator : bool = False #whether left turn indicator is on right_turn_indicator : bool = False #whether right turn indicator is on headlights_on : bool = False #whether headlights are on diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 063f690c7..478fe0800 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -8,7 +8,7 @@ #KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves #this is a workaround. We really should find the source of the bug! -MAX_POINTS_IN_CURVE = 50 +MAX_POINTS_IN_CURVE = 30 OBJECT_COLORS = { AgentEnum.CAR : (1,1,0,1), diff --git a/GEMstack/utils/logging.py b/GEMstack/utils/logging.py index 0cda2e55e..b73167f2d 100644 --- a/GEMstack/utils/logging.py +++ b/GEMstack/utils/logging.py @@ -146,7 +146,7 @@ def read(self, else: raise ValueError("Need to provide a time to advance to") msgs = [] - while self.next_item_time < next_t: + while self.next_item_time <= next_t: self.last_read_time = self.next_item_time msgs.append(self.next_item) try: diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index c05de8ff7..92d70152d 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -38,16 +38,16 @@ log: # If True, then record all readings / commands of the vehicle interface. Default False vehicle_interface : True # Specify which components to record to behavior.json. Default records nothing - components : ['state_estimation','trajectory_tracking'] + components : ['state_estimation'] # Specify which components of state to record to state.json. Default records nothing #state: ['all'] # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate #state_rate: 10 replay: # Add items here to set certain topics / inputs to be replayed from logs # Specify which log folder to replay from - log: + log: 'logs/test3' # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" - ros_topics : [] + ros_topics : ['/dummy_topic', '/webcam'] components : [] #usually can keep this constant @@ -76,4 +76,4 @@ variants: #visualization: !include "mpl_visualization.yaml" log_ros: log: - ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file + ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" diff --git a/launch/gather_data.yaml b/launch/gather_data.yaml index 4f3d03d4c..4a229e6b0 100644 --- a/launch/gather_data.yaml +++ b/launch/gather_data.yaml @@ -40,7 +40,7 @@ log: replay: # Add items here to set certain topics / inputs to be replayed from logs # Specify which log folder to replay from log: - ros_topics : [] + ros_topics : ['/ouster/points'] components : [] #usually can keep this constant @@ -51,4 +51,4 @@ variants: log_ros: run: log: - ros_topics: !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file + ros_topics: !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" From 1f6127f75b7b1253c4b84e1d309954776fa3bcb2 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Tue, 25 Feb 2025 14:36:44 -0600 Subject: [PATCH 07/11] added POC end-to-end testing --- GEMstack/onboard/execution/execution.py | 10 ++- GEMstack/onboard/execution/logging.py | 41 ++++------ .../launch/klampt_visualization.yaml | 7 ++ integration_tests/launch/test1.yaml | 79 +++++++++++++++++++ integration_tests/launch/test2.yaml | 79 +++++++++++++++++++ integration_tests/run_test.py | 30 +++++++ integration_tests/scenes/highbay.yaml | 11 +++ integration_tests/scenes/xyhead_demo.yaml | 9 +++ integration_tests/test.py | 19 +++++ integration_tests/testing.py | 79 +++++++++++++++++++ scenes/highbay.yaml | 11 +++ 11 files changed, 349 insertions(+), 26 deletions(-) create mode 100644 integration_tests/launch/klampt_visualization.yaml create mode 100644 integration_tests/launch/test1.yaml create mode 100644 integration_tests/launch/test2.yaml create mode 100644 integration_tests/run_test.py create mode 100644 integration_tests/scenes/highbay.yaml create mode 100644 integration_tests/scenes/xyhead_demo.yaml create mode 100644 integration_tests/test.py create mode 100644 integration_tests/testing.py create mode 100644 scenes/highbay.yaml diff --git a/GEMstack/onboard/execution/execution.py b/GEMstack/onboard/execution/execution.py index c08a2e962..aa86bb298 100644 --- a/GEMstack/onboard/execution/execution.py +++ b/GEMstack/onboard/execution/execution.py @@ -163,7 +163,7 @@ def validate_components(components : Dict[str,ComponentExecutor], provided : Lis else: assert provided_all or i in provided, "Component {} input {} is not provided by previous components".format(k,i) if i not in state: - executor_debug_print("Component {} input {} does not exist in AllState object",k,i) + executor_debug_print(0,"Component {} input {} does not exist in AllState object",k,i) if possible_inputs != ['all']: assert i in possible_inputs, "Component {} is not supposed to receive input {}".format(k,i) outputs = c.c.state_outputs() @@ -176,7 +176,7 @@ def validate_components(components : Dict[str,ComponentExecutor], provided : Lis if 'all' != o: provided.add(o) if o not in state: - executor_debug_print("Component {} output {} does not exist in AllState object",k,o) + executor_debug_print(0,"Component {} output {} does not exist in AllState object",k,o) else: provided_all = True for k,c in components.items(): @@ -481,6 +481,10 @@ def run(self): global LOGGING_MANAGER LOGGING_MANAGER = self.logging_manager #kludge! should refactor to avoid global variables + def signal_handler(sig, frame): + print("Received SIGINT! Cleaning up...") + sys.exit(0) + #sanity checking if self.current_pipeline not in self.pipelines: executor_debug_print(0,"Initial pipeline {} not found",self.current_pipeline) @@ -528,6 +532,8 @@ def run(self): try: executor_debug_print(1,"Executing pipeline {}",self.current_pipeline) next = self.run_until_switch() + import signal + signal.signal(signal.SIGINT, signal_handler) if next is None: #done self.set_exit_reason("normal exit") diff --git a/GEMstack/onboard/execution/logging.py b/GEMstack/onboard/execution/logging.py index 145709244..b3df0af89 100644 --- a/GEMstack/onboard/execution/logging.py +++ b/GEMstack/onboard/execution/logging.py @@ -13,6 +13,7 @@ import json import rosbag import rospy + class LoggingManager: """A top level manager of the logging process. This is responsible for creating log folders, log metadata files, and for replaying components from log @@ -36,6 +37,7 @@ def __init__(self): self.vehicle_time = None self.start_vehicle_time = None self.debug_messages = {} + self.onedrive_manager = None def logging(self) -> bool: return self.log_folder is not None @@ -199,14 +201,14 @@ def debug(self, component : str, item : str, value : Any) -> None: os.mkdir(folder) filename = os.path.join(folder,item+'_%03d.npz'%len(self.debug_messages[component][item])) np.savez(filename,value) - # elif isinstance(value,cv2.Mat): - # #if really large, save as png - # folder = os.path.join(self.log_folder,'debug_{}'.format(component)) - # if item not in self.debug_messages[component]: - # self.debug_messages[component][item] = [] - # os.mkdir(folder) - # filename = os.path.join(folder,item+'_%03d.png'%len(self.debug_messages[component][item])) - # cv2.imwrite(filename,value) + elif isinstance(value,cv2.Mat): + #if really large, save as png + folder = os.path.join(self.log_folder,'debug_{}'.format(component)) + if item not in self.debug_messages[component]: + self.debug_messages[component][item] = [] + os.mkdir(folder) + filename = os.path.join(folder,item+'_%03d.png'%len(self.debug_messages[component][item])) + cv2.imwrite(filename,value) else: if item not in self.debug_messages[component]: self.debug_messages[component][item] = [] @@ -311,21 +313,18 @@ def close(self): print('Log file size in MegaBytes is {}'.format(loginfo.st_size / (1024 * 1024))) print('-------------------------------------------') self.rosbag_process = None - - record_bag = input("Do you want to upload this Rosbag? Y/N (default: Y): ") or "Y" - - if(record_bag not in ["N", "no", "n", "No"]): - self.upload_to_onedrive() + self.onedrive_manager = OneDriveManager() + self.onedrive_manager.upload_to_onedrive(self.log_folder) def __del__(self): self.close() -class OneDrive(): +class OneDriveManager(): def __init__(self): self.config_found = False try: @@ -341,12 +340,7 @@ def __init__(self): print("No Onedrive config file found") - def upload_to_onedrive(self): - - - - # Not private but for reusability in future semesters: - # Retrieve values from the config + def upload_to_onedrive(self, log_folder): AUTHORITY = f'https://login.microsoftonline.com/{self.TENANT_ID}' @@ -369,8 +363,8 @@ def upload_to_onedrive(self): 'Authorization': f'Bearer {access_token}', 'Content-Type': 'application/octet-stream' } - file_path = os.path.join(self.log_folder, 'vehicle.bag') - file_name = self.log_folder[5:]+ "_" + os.path.basename(file_path) + file_path = os.path.join(log_folder, 'vehicle.bag') + file_name = log_folder[5:]+ "_" + os.path.basename(file_path) upload_url = ( f'https://graph.microsoft.com/v1.0/drives/{self.DRIVE_ID}/items/' f'{self.ITEM_ID}:/{file_name}:/content' @@ -386,7 +380,6 @@ def upload_to_onedrive(self): else: print("❌ Authentication failed.") - class LogReplay(Component): """Substitutes the output of a component with replayed data from a log file. @@ -478,7 +471,7 @@ def update_topics(self, target_timestamp): #rospy.loginfo(f'Publishing {msg} to {topic}') if topic in self.publishers: # Only publish selected topics self.publishers[topic].publish(msg) - rospy.sleep(0.01) # Simulate real-time playback + #rospy.sleep(0.01) # Simulate real-time playback self.current_time = t.to_sec() # Update current position diff --git a/integration_tests/launch/klampt_visualization.yaml b/integration_tests/launch/klampt_visualization.yaml new file mode 100644 index 000000000..2265debf3 --- /dev/null +++ b/integration_tests/launch/klampt_visualization.yaml @@ -0,0 +1,7 @@ +type: klampt_visualization.KlamptVisualization +args: + rate: 20 + #Don't include save_as if you don't want to save the video + save_as: + #save_as: "fixed_route_sim.mp4" + diff --git a/integration_tests/launch/test1.yaml b/integration_tests/launch/test1.yaml new file mode 100644 index 000000000..2674d2a1b --- /dev/null +++ b/integration_tests/launch/test1.yaml @@ -0,0 +1,79 @@ +description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking: + type: recovery.StopTrajectoryTracker + print: False +# Driving behavior for the GEM vehicle following a fixed route +drive: + perception: + state_estimation : GNSSStateEstimator + perception_normalization : StandardPerceptionNormalizer + planning: + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + motion_planning: + type: RouteToTrajectoryPlanner + args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + args: {desired_speed: 2.5} #approximately 5mph + print: False +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is Sspecified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : [] + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: 'logs/test5' + # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" + ros_topics : ['/septentrio_gnss/insnavgeod'] + components : ['state_estimation'] + +#usually can keep this constant +computation_graph: !include "../../GEMstack/knowledge/defaults/computation_graph.yaml" + +after: + show_log_folder: True #set to false to avoid showing the log folder + +#on load, variants will overload the settings structure +variants: + #sim variant doesn't execute on the real vehicle + #real variant executes on the real robot + sim: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/highbay.yaml' + + drive: + perception: + state_estimation : OmniscientStateEstimator + agent_detection : OmniscientAgentDetector + visualization: !include "klampt_visualization.yaml" + #visualization: !include "mpl_visualization.yaml" + log_ros: + log: + ros_topics : !include "../../GEMstack/knowledge/defaults/standard_ros_topics.yaml" diff --git a/integration_tests/launch/test2.yaml b/integration_tests/launch/test2.yaml new file mode 100644 index 000000000..c799a509f --- /dev/null +++ b/integration_tests/launch/test2.yaml @@ -0,0 +1,79 @@ +description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking: + type: recovery.StopTrajectoryTracker + print: False +# Driving behavior for the GEM vehicle following a fixed route +drive: + perception: + state_estimation : GNSSStateEstimator + perception_normalization : StandardPerceptionNormalizer + planning: + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + motion_planning: + type: RouteToTrajectoryPlanner + args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + args: {desired_speed: 2.5} #approximately 5mph + print: False +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is Sspecified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : [] + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation', 'agent_detection'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../../GEMstack/knowledge/defaults/computation_graph.yaml" + +after: + show_log_folder: True #set to false to avoid showing the log folder + +#on load, variants will overload the settings structure +variants: + #sim variant doesn't execute on the real vehicle + #real variant executes on the real robot + sim: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + + drive: + perception: + state_estimation : OmniscientStateEstimator + agent_detection : OmniscientAgentDetector + visualization: !include "klampt_visualization.yaml" + #visualization: !include "mpl_visualization.yaml" + log_ros: + log: + ros_topics : !include "../../GEMstack/knowledge/defaults/standard_ros_topics.yaml" diff --git a/integration_tests/run_test.py b/integration_tests/run_test.py new file mode 100644 index 000000000..64d6afadc --- /dev/null +++ b/integration_tests/run_test.py @@ -0,0 +1,30 @@ +from GEMstack.utils import settings,config +import sys + +if __name__=='__main__': + launch_file = None + for arg in sys.argv[1:]: + if arg.startswith('--run='): + launch_file = arg[9:] + break + elif not arg.startswith('--'): + launch_file = arg + break + if launch_file is None: + runconfig = settings.get('run',None) + if runconfig is None: + print("Usage: python3 [--key1=value1 --key2=value2] LAUNCH_FILE.yaml") + print(" Current settings are found in knowledge/defaults/current.yaml") + exit(1) + else: + print("Using default run configuration in knowledge/defaults/current.yaml") + else: + #set the run settings from command line + run_config = config.load_config_recursive(launch_file) + #print(run_config) + settings.set('run',run_config) + if settings.get('run.name',None) is None: + settings.set('run.name',launch_file) + + from GEMstack.onboard.execution import entrypoint + entrypoint.main() diff --git a/integration_tests/scenes/highbay.yaml b/integration_tests/scenes/highbay.yaml new file mode 100644 index 000000000..57b1bdff3 --- /dev/null +++ b/integration_tests/scenes/highbay.yaml @@ -0,0 +1,11 @@ +#TODO: change visualization to display highbay map for replaying/visualization of recorded data +vehicle_state: [0.0, 0.0, 200.0, 0.0, 0.0] +agents: + ped1: + type: pedestrian + position: [15.0, 2.0] + nominal_velocity: 0.5 + target: [15.0,10.0] + behavior: loop + + diff --git a/integration_tests/scenes/xyhead_demo.yaml b/integration_tests/scenes/xyhead_demo.yaml new file mode 100644 index 000000000..c6d97477a --- /dev/null +++ b/integration_tests/scenes/xyhead_demo.yaml @@ -0,0 +1,9 @@ +vehicle_state: [4.0, 5.0, 0.0, 0.0, 0.0] +agents: + ped1: + type: pedestrian + position: [15.0, 2.0] + nominal_velocity: 0.5 + target: [15.0,10.0] + behavior: loop + \ No newline at end of file diff --git a/integration_tests/test.py b/integration_tests/test.py new file mode 100644 index 000000000..0ea4a16f7 --- /dev/null +++ b/integration_tests/test.py @@ -0,0 +1,19 @@ +import subprocess +import time +import subprocess +import os +import yaml +import time +import signal + +config_path = "launch/fixed_route.yaml" +process = subprocess.Popen(["python3", "main.py", "--variant=sim", config_path], text=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) +# for line in iter(process.stdout.readline, ''): +# print(line, end='') +# for line in iter(process.stderr.readline, ''): +# print(line, end='') +# process.wait() +time.sleep(30) # Give it some time to start + +print("Sending SIGINT to process...") +os.kill(process.pid, signal.SIGINT) # Equivalent to Ctrl+C \ No newline at end of file diff --git a/integration_tests/testing.py b/integration_tests/testing.py new file mode 100644 index 000000000..057d0a65c --- /dev/null +++ b/integration_tests/testing.py @@ -0,0 +1,79 @@ +import unittest +import subprocess +import os +import json +from parameterized import parameterized +import time +import signal +from abc import ABC, abstractmethod +# Configuration of each test case. +TEST_CONFIGS = [ + ("test1", "integration_tests/launch/test1.yaml", 10), + ("test2", "integration_tests/launch/test2.yaml", 10), +] + +import os + +def get_last_modified_folder(parent_path): + subdirs = [entry.path for entry in os.scandir(parent_path) if entry.is_dir()] + if not subdirs: + return None + last_modified_folder = max(subdirs, key=os.path.getmtime) + return last_modified_folder + + +class BaseLogValidator(ABC): + + @abstractmethod + def validate(self, log_dir): + """Each test case must implement this method""" + pass + +class ValidateTestCase1(BaseLogValidator): + def validate(self, log_dir): + log_file = os.path.join(log_dir, "behavior.json") + with open(log_file, "r") as file: + for index, line in enumerate(file): + try: + record = json.loads(line) + acceleration = record.get("vehicle", {}).get("data", {}).get("acceleration", 0) + assert abs(acceleration) <= .11, f"Record {index} has acceleration {acceleration} > 1" + except json.JSONDecodeError: + raise AssertionError(f"Malformed JSON in log file at line {index}") + +class ValidateTestCase2(BaseLogValidator): + def validate(self, log_dir): + log_file = os.path.join(log_dir, "behavior.json") + + +class IntegrationTestSuite(unittest.TestCase): + VALIDATORS = { + "test1": ValidateTestCase1(), + "test2": ValidateTestCase2(), + } + def setUp(self): + pass + def tearDown(self): + """Clean up after each test if necessary.""" + pass + + @parameterized.expand(TEST_CONFIGS) + def test_command_execution(self, name, config_path, runtime): + process = subprocess.Popen(["python3", "main.py", "--variant=sim", config_path], text=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + # for line in iter(process.stdout.readline, ''): + # print(line, end='') + time.sleep(runtime) + print("Stopping GEMStack...") + os.kill(process.pid, signal.SIGINT) + process.wait() + + # Validate logs + validator = self.VALIDATORS.get(name) + if validator is None: + self.fail(f"no validator found for {name}") + validator.validate(get_last_modified_folder('logs')) + + + +if __name__ == "__main__": + unittest.main() diff --git a/scenes/highbay.yaml b/scenes/highbay.yaml new file mode 100644 index 000000000..57b1bdff3 --- /dev/null +++ b/scenes/highbay.yaml @@ -0,0 +1,11 @@ +#TODO: change visualization to display highbay map for replaying/visualization of recorded data +vehicle_state: [0.0, 0.0, 200.0, 0.0, 0.0] +agents: + ped1: + type: pedestrian + position: [15.0, 2.0] + nominal_velocity: 0.5 + target: [15.0,10.0] + behavior: loop + + From f7d343126fd31aae11e80ecde4e6516352fba47c Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Tue, 25 Feb 2025 17:26:07 -0600 Subject: [PATCH 08/11] removed unecessary files --- integration_tests/run_test.py | 30 ------------- integration_tests/run_tests.py | 79 ++++++++++++++++++++++++++++++++++ integration_tests/test.py | 19 -------- 3 files changed, 79 insertions(+), 49 deletions(-) delete mode 100644 integration_tests/run_test.py create mode 100644 integration_tests/run_tests.py delete mode 100644 integration_tests/test.py diff --git a/integration_tests/run_test.py b/integration_tests/run_test.py deleted file mode 100644 index 64d6afadc..000000000 --- a/integration_tests/run_test.py +++ /dev/null @@ -1,30 +0,0 @@ -from GEMstack.utils import settings,config -import sys - -if __name__=='__main__': - launch_file = None - for arg in sys.argv[1:]: - if arg.startswith('--run='): - launch_file = arg[9:] - break - elif not arg.startswith('--'): - launch_file = arg - break - if launch_file is None: - runconfig = settings.get('run',None) - if runconfig is None: - print("Usage: python3 [--key1=value1 --key2=value2] LAUNCH_FILE.yaml") - print(" Current settings are found in knowledge/defaults/current.yaml") - exit(1) - else: - print("Using default run configuration in knowledge/defaults/current.yaml") - else: - #set the run settings from command line - run_config = config.load_config_recursive(launch_file) - #print(run_config) - settings.set('run',run_config) - if settings.get('run.name',None) is None: - settings.set('run.name',launch_file) - - from GEMstack.onboard.execution import entrypoint - entrypoint.main() diff --git a/integration_tests/run_tests.py b/integration_tests/run_tests.py new file mode 100644 index 000000000..057d0a65c --- /dev/null +++ b/integration_tests/run_tests.py @@ -0,0 +1,79 @@ +import unittest +import subprocess +import os +import json +from parameterized import parameterized +import time +import signal +from abc import ABC, abstractmethod +# Configuration of each test case. +TEST_CONFIGS = [ + ("test1", "integration_tests/launch/test1.yaml", 10), + ("test2", "integration_tests/launch/test2.yaml", 10), +] + +import os + +def get_last_modified_folder(parent_path): + subdirs = [entry.path for entry in os.scandir(parent_path) if entry.is_dir()] + if not subdirs: + return None + last_modified_folder = max(subdirs, key=os.path.getmtime) + return last_modified_folder + + +class BaseLogValidator(ABC): + + @abstractmethod + def validate(self, log_dir): + """Each test case must implement this method""" + pass + +class ValidateTestCase1(BaseLogValidator): + def validate(self, log_dir): + log_file = os.path.join(log_dir, "behavior.json") + with open(log_file, "r") as file: + for index, line in enumerate(file): + try: + record = json.loads(line) + acceleration = record.get("vehicle", {}).get("data", {}).get("acceleration", 0) + assert abs(acceleration) <= .11, f"Record {index} has acceleration {acceleration} > 1" + except json.JSONDecodeError: + raise AssertionError(f"Malformed JSON in log file at line {index}") + +class ValidateTestCase2(BaseLogValidator): + def validate(self, log_dir): + log_file = os.path.join(log_dir, "behavior.json") + + +class IntegrationTestSuite(unittest.TestCase): + VALIDATORS = { + "test1": ValidateTestCase1(), + "test2": ValidateTestCase2(), + } + def setUp(self): + pass + def tearDown(self): + """Clean up after each test if necessary.""" + pass + + @parameterized.expand(TEST_CONFIGS) + def test_command_execution(self, name, config_path, runtime): + process = subprocess.Popen(["python3", "main.py", "--variant=sim", config_path], text=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + # for line in iter(process.stdout.readline, ''): + # print(line, end='') + time.sleep(runtime) + print("Stopping GEMStack...") + os.kill(process.pid, signal.SIGINT) + process.wait() + + # Validate logs + validator = self.VALIDATORS.get(name) + if validator is None: + self.fail(f"no validator found for {name}") + validator.validate(get_last_modified_folder('logs')) + + + +if __name__ == "__main__": + unittest.main() diff --git a/integration_tests/test.py b/integration_tests/test.py deleted file mode 100644 index 0ea4a16f7..000000000 --- a/integration_tests/test.py +++ /dev/null @@ -1,19 +0,0 @@ -import subprocess -import time -import subprocess -import os -import yaml -import time -import signal - -config_path = "launch/fixed_route.yaml" -process = subprocess.Popen(["python3", "main.py", "--variant=sim", config_path], text=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) -# for line in iter(process.stdout.readline, ''): -# print(line, end='') -# for line in iter(process.stderr.readline, ''): -# print(line, end='') -# process.wait() -time.sleep(30) # Give it some time to start - -print("Sending SIGINT to process...") -os.kill(process.pid, signal.SIGINT) # Equivalent to Ctrl+C \ No newline at end of file From 2d7fa31edb251253f1788943d6eb12e27ee152f9 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Wed, 26 Feb 2025 13:24:35 -0600 Subject: [PATCH 09/11] revert fixed route yaml --- launch/fixed_route.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 4caff7e23..4172b7e3d 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -45,9 +45,9 @@ log: #state_rate: 10 replay: # Add items here to set certain topics / inputs to be replayed from logs # Specify which log folder to replay from - log: 'logs/test3' + log: # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" - ros_topics : ['/dummy_topic', '/webcam'] + ros_topics : [] components : [] #usually can keep this constant From 3dc5cba6d2a0cfb3e9ad49b12df23af6b5d8a2c0 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Wed, 26 Feb 2025 13:39:18 -0600 Subject: [PATCH 10/11] more clean up --- GEMstack/onboard/execution/execution.py | 7 ------- GEMstack/utils/logging.py | 1 - launch/fixed_route.yaml | 2 +- launch/gather_data.yaml | 2 +- requirements.txt | 3 ++- 5 files changed, 4 insertions(+), 11 deletions(-) diff --git a/GEMstack/onboard/execution/execution.py b/GEMstack/onboard/execution/execution.py index a21d0d3a7..161138ec9 100644 --- a/GEMstack/onboard/execution/execution.py +++ b/GEMstack/onboard/execution/execution.py @@ -481,10 +481,6 @@ def run(self): global LOGGING_MANAGER LOGGING_MANAGER = self.logging_manager #kludge! should refactor to avoid global variables - def signal_handler(sig, frame): - print("Received SIGINT! Cleaning up...") - sys.exit(0) - #sanity checking if self.current_pipeline not in self.pipelines: executor_debug_print(0,"Initial pipeline {} not found",self.current_pipeline) @@ -532,8 +528,6 @@ def signal_handler(sig, frame): try: executor_debug_print(1,"Executing pipeline {}",self.current_pipeline) next = self.run_until_switch() - import signal - signal.signal(signal.SIGINT, signal_handler) if next is None: #done self.set_exit_reason("normal exit") @@ -574,7 +568,6 @@ def signal_handler(sig, frame): executor_debug_print(2,"Stopping",k) c.stop() - self.logging_manager.close() executor_debug_print(0,"Done with execution loop") diff --git a/GEMstack/utils/logging.py b/GEMstack/utils/logging.py index b73167f2d..62ecf84e6 100644 --- a/GEMstack/utils/logging.py +++ b/GEMstack/utils/logging.py @@ -1,5 +1,4 @@ from .serialization import deserialize,serialize,deserialize_collection,serialize_collection -import json from typing import Union,Tuple class Logfile: diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 4172b7e3d..6af97165e 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -38,7 +38,7 @@ log: # If True, then record all readings / commands of the vehicle interface. Default False vehicle_interface : True # Specify which components to record to behavior.json. Default records nothing - components : ['state_estimation'] + components : ['state_estimation','trajectory_tracking'] # Specify which components of state to record to state.json. Default records nothing #state: ['all'] # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate diff --git a/launch/gather_data.yaml b/launch/gather_data.yaml index 234056e04..0a81e8272 100644 --- a/launch/gather_data.yaml +++ b/launch/gather_data.yaml @@ -45,7 +45,7 @@ log: replay: # Add items here to set certain topics / inputs to be replayed from logs # Specify which log folder to replay from log: - ros_topics : ['/ouster/points'] + ros_topics : [] components : [] #usually can keep this constant diff --git a/requirements.txt b/requirements.txt index 55c9c5876..79ee0e62b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -15,4 +15,5 @@ open3d #infra msal -parameterized \ No newline at end of file +parameterized +requests \ No newline at end of file From 7c15f2da2ce8c56a9161f3ebbcf1bc389e397d6b Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Wed, 26 Feb 2025 22:49:16 -0600 Subject: [PATCH 11/11] added docs --- GEMstack/utils/klampt_visualization.py | 4 +-- docs/auto-upload.md | 28 +++++++++++++++++++++ docs/end-to-end-testing.md | 34 ++++++++++++++++++++++++++ docs/rostopic-replay.md | 24 ++++++++++++++++++ integration_tests/run_tests.py | 3 ++- 5 files changed, 90 insertions(+), 3 deletions(-) create mode 100644 docs/auto-upload.md create mode 100644 docs/end-to-end-testing.md create mode 100644 docs/rostopic-replay.md diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 478fe0800..741e968e8 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -7,8 +7,8 @@ from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState #KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves -#this is a workaround. We really should find the source of the bug! -MAX_POINTS_IN_CURVE = 30 +#this is a workaround. We really should find the source of the bug! Change to 30 if still not working +MAX_POINTS_IN_CURVE = 50 OBJECT_COLORS = { AgentEnum.CAR : (1,1,0,1), diff --git a/docs/auto-upload.md b/docs/auto-upload.md new file mode 100644 index 000000000..3e3798e12 --- /dev/null +++ b/docs/auto-upload.md @@ -0,0 +1,28 @@ +# OneDrive Upload + +## Config file setup + +An authenticator service must be setup in azure first. This can be done by going to App registrations in Azure and creating a new resgistration + + +To configure the onedrive config, it must be named onedrive_config.json and be in the following format + +```json +{ + "CLIENT_ID": "", + "TENANT_ID": "", + "DRIVE_ID": "", + "ITEM_ID": "" +} +``` + +The Client Id and the tenant id of the authenticator service found in the app registration in azure + +The DRIVE_ID and ITEM_ID of the onedrive folder to upload to can be found with this guide: +https://www.youtube.com/watch?v=3pjS7YTIcgQ + +Anybody can access the same authenticator service for free as long as they have an illinois account. + + +## Usage +When any Ros topics are specified in the yaml file under 'log,' a prompt will appear in the console after finishing the GEMStack execution. If the user specifies that they want to continue upload, a window will appear in browser (will not work in docker or when running headless.) You may login with your illinois account only. diff --git a/docs/end-to-end-testing.md b/docs/end-to-end-testing.md new file mode 100644 index 000000000..00ea15f8f --- /dev/null +++ b/docs/end-to-end-testing.md @@ -0,0 +1,34 @@ +# End-to-End Testing + +## Writing Tests + +Under the GEMStack/GEMStack/integration_tests directory, there are two example test cases (log folders still need to be specified) + + +Each test case is associated with a test config, which is written in the order: + +>Name of test, variant of sim to run, launch file location, runtime of test + +The test must also be written with a custom validation function that inherits from the base validator and mapped to the name in the test suite: + +```python +VALIDATORS = { + "test1": ValidateTestCase1(), + "test2": ValidateTestCase2(), +} +``` + + Each test will run in sequential order and start GEMStack in a separate process over the course of the specified runtime. + + The custom validation funtion will then run on the latest log folder generated (This part needs to be made more robust later). + + The validation function is open ended so there is flexibility to parse the behavior_json file, rosbag, or stdout/stderr files. + + All results and the runtime will be displayed in the terminal at the end. + + ## Running the test suite: + + python3 integration\_tests/run\_tests.py + + + diff --git a/docs/rostopic-replay.md b/docs/rostopic-replay.md new file mode 100644 index 000000000..bc0da2c4f --- /dev/null +++ b/docs/rostopic-replay.md @@ -0,0 +1,24 @@ +# Component Replay and Rostopic Replay + +## Component Replay +Component replay can now play back states from behavior.json. Only the component outputs can be played back. (The vehicle_interface commands cannot be played back). Note that the current simulator maps need to be updated to handle real GEM data + +## Rostopic Replay + +Will create new publishers from the specified topics in the vehicle.bag. The messages will be published with the GEM interface's time, to keep sync with the rest of the execution. This means that the rest of the GEMstack execution must also be running (will not publish on detector_only variant) + + + +## Usage + +Add topics and components to replay in the yaml file: + +```yaml +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + ros_topics : [] + components : [] +``` + + diff --git a/integration_tests/run_tests.py b/integration_tests/run_tests.py index 27a3235cc..b1f48a970 100644 --- a/integration_tests/run_tests.py +++ b/integration_tests/run_tests.py @@ -6,7 +6,7 @@ import time import signal from abc import ABC, abstractmethod -# Configuration of each test case. +# Configuration of each test case. Order is Name, variant, launch file location, runtime TEST_CONFIGS = [ ("test1", "sim", "integration_tests/launch/test1.yaml", 10), ("test2", "real_sim", "integration_tests/launch/test2.yaml", 25), @@ -70,6 +70,7 @@ def tearDown(self): def test_command_execution(self, name, variant, config_path, runtime): command = ["python3", "main.py", f"--variant={variant}", config_path] process = subprocess.Popen(command, text=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + #Uncomment to debug output from execution # for line in iter(process.stdout.readline, ''): # print(line, end='') time.sleep(runtime)