diff --git a/.gitignore b/.gitignore index 61a2df38b..5a0b72642 100644 --- a/.gitignore +++ b/.gitignore @@ -171,9 +171,14 @@ cython_debug/ .vscode/ setup/zed_sdk.run cuda/ +onedrive_config.json +zed_sdk.run +ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run +ros.asc + homework/yolov8n.pt homework/yolo11n.pt GEMstack/knowledge/detection/yolov8n.pt GEMstack/knowledge/detection/yolo11n.pt yolov8n.pt -yolo11n.pt \ No newline at end of file +yolo11n.pt 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..161138ec9 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(): @@ -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,7 +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") @@ -630,7 +638,7 @@ def run_until_switch(self): """Runs a pipeline until a switch is requested.""" if self.current_pipeline == 'recovery': self.state.mission.type = MissionEnum.RECOVERY_STOP - + (perception_components,planning_components,other_components) = self.pipelines[self.current_pipeline] components = list(perception_components.values()) + list(planning_components.values()) + list(other_components.values()) + list(self.always_run_components.values()) dt_min = min([c.dt for c in components if c.dt != 0.0]) @@ -639,6 +647,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 31ebd962b..99c107be5 100644 --- a/GEMstack/onboard/execution/logging.py +++ b/GEMstack/onboard/execution/logging.py @@ -8,6 +8,11 @@ import subprocess import numpy as np import cv2 +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 @@ -16,7 +21,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 @@ -27,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 @@ -74,16 +85,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 @@ -207,7 +242,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(): @@ -261,8 +296,10 @@ 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): + + self.dump_debug() self.debug_messages = {} if self.rosbag_process is not None: @@ -276,10 +313,73 @@ 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.onedrive_manager = OneDriveManager() + self.onedrive_manager.upload_to_onedrive(self.log_folder) + + def __del__(self): self.close() +class OneDriveManager(): + 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, log_folder): + + + 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(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' + ) + + 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. @@ -334,6 +434,49 @@ def cleanup(self): self.logfile.close() +class RosbagPlayer: + ''' + Class which manages Ros bag replay. Note that this does not work unless the executor is running + ''' + def __init__(self, bag_path, topics): + self.bag = rosbag.Bag(os.path.join(bag_path, 'vehicle.bag'), 'r') + self.current_time = None + 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 target timestamp. + :param target_timestamp: gem stack time to play until. + Will remember the currenttime and only play from current time to the 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 + 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 + if topic in self.publishers: + self.publishers[topic].publish(msg) + + self.current_time = t.to_sec() + + 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..741e968e8 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -7,7 +7,7 @@ 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! +#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 = { diff --git a/GEMstack/utils/logging.py b/GEMstack/utils/logging.py index 0cda2e55e..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: @@ -146,7 +145,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/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/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/mpl_visualization.yaml b/integration_tests/launch/mpl_visualization.yaml new file mode 100644 index 000000000..b9642e4de --- /dev/null +++ b/integration_tests/launch/mpl_visualization.yaml @@ -0,0 +1,7 @@ +type: mpl_visualization.MPLVisualization +args: + rate: 8 + #Don't include save_as if you don't want to save the video + save_as: + #save_as: "fixed_route_sim.mp4" +multiprocess: True diff --git a/integration_tests/launch/test1.yaml b/integration_tests/launch/test1.yaml new file mode 100644 index 000000000..e779ea9b5 --- /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/test_control' + # 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..c9d5376eb --- /dev/null +++ b/integration_tests/launch/test2.yaml @@ -0,0 +1,101 @@ +description: "Run the yielding trajectory planner on the real vehicle with real perception" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor + +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking : recovery.StopTrajectoryTracker + +# Driving behavior for the GEM vehicle. Runs real pedestrian perception, yield planner, but does not send commands to real vehicle. +drive: + perception: + state_estimation : GNSSStateEstimator + agent_detection : pedestrian_detection.PedestrianDetector2D + perception_normalization : StandardPerceptionNormalizer + planning: + relations_estimation: pedestrian_yield_logic.PedestrianYielder + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../../GEMstack/knowledge/routes/forward_15m.csv','start'] + motion_planning: longitudinal_planning.YieldTrajectoryPlanner + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False + + +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is specified, 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','motion_planning'] + # 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/perception_test' + ros_topics : ['/oak/rgb/camera_info', '/oak/rgb/image_raw', '/ouster/points', '/septentrio_gnss/imu', '/septentrio_gnss/insnavgeod'] + components : [] + +#usually can keep this constant +computation_graph: !include "../../GEMstack/knowledge/defaults/computation_graph.yaml" + +variants: + detector_only: + run: + description: "Run the pedestrian detection code" + drive: + planning: + trajectory_tracking: + visualization: !include "mpl_visualization.yaml" + + real_sim: + run: + description: "Run the pedestrian detection code with real detection and fake simulation" + mode: hardware + vehicle_interface: + type: gem_mixed.GEMRealSensorsWithSimMotionInterface + args: + scene: !relative_path '../../scenes/xyhead_demo.yaml' + mission_execution: StandardExecutor + require_engaged: False + visualization: !include "mpl_visualization.yaml" + drive: + perception: + state_estimation : OmniscientStateEstimator + + + fake_real: + run: + description: "Run the yielding trajectory planner on the real vehicle with faked perception" + drive: + perception: + agent_detection : pedestrian_detection.FakePedestrianDetector2D + + fake_sim: + run: + description: "Run the yielding trajectory planner in simulation with faked perception" + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../../scenes/xyhead_demo.yaml' + visualization: !include "klampt_visualization.yaml" + drive: + perception: + agent_detection : pedestrian_detection.FakePedestrianDetector2D + #agent_detection : OmniscientAgentDetector #this option reads agents from the simulator + state_estimation : OmniscientStateEstimator diff --git a/integration_tests/run_tests.py b/integration_tests/run_tests.py new file mode 100644 index 000000000..b1f48a970 --- /dev/null +++ b/integration_tests/run_tests.py @@ -0,0 +1,90 @@ +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. 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), + +] + +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): + record = json.loads(line) + acceleration = record.get("vehicle", {}).get("data", {}).get("acceleration", 0) + assert abs(acceleration) <= .11, f"Record {index} has acceleration {acceleration} > threshold" + + +class ValidateTestCase2(BaseLogValidator): + def validate(self, log_dir): + log_file = os.path.join(log_dir, "behavior.json") + has_non_empty_agent = False + + with open(log_file, "r") as file: + for _, line in enumerate(file): + record = json.loads(line) + agent_data = record.get("agents", {}) + + if agent_data: + has_non_empty_agent = True + break + + assert has_non_empty_agent, "No agents detected in 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, 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) + 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/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/launch/fixed_route.yaml b/launch/fixed_route.yaml index 7f512f10a..6af97165e 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -75,4 +75,4 @@ variants: visualization: [!include "klampt_visualization.yaml", !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 16f1b5bb7..0a81e8272 100644 --- a/launch/gather_data.yaml +++ b/launch/gather_data.yaml @@ -56,4 +56,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" diff --git a/requirements.txt b/requirements.txt index f8e808d90..79ee0e62b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -11,4 +11,9 @@ dacite # Perception ultralytics lap==0.5.12 -open3d \ No newline at end of file +open3d + +#infra +msal +parameterized +requests \ No newline at end of file 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 + +