Infra b/rosbag pipeline

.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
+yolo11n.pt

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

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

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

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

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 = {

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:

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.