Adding new files for state machine logic

animeshsingh98 · animeshsingh98 · commit de4d6d080fb5 · 2025-04-06T18:42:14.000-05:00
diff --git a/GEMstack/onboard/execution/entrypoint_new.py b/GEMstack/onboard/execution/entrypoint_new.py
@@ -0,0 +1,177 @@
+from ...utils import settings,config
+from ..component import Component
+from .execution import EXECUTION_PREFIX,ExecutorBase,ComponentExecutor,load_computation_graph,make_class
+import multiprocessing
+from typing import Dict,List,Optional
+import sys
+import os
+
+def main():
+    """The main entrypoint for the execution stack."""
+    #first, process settings variants
+    if settings.get('variant',None) is not None:
+        variant = settings.get('variant')
+        variants = variant.split(',')
+        def caution_callback(k,variant):
+            print(EXECUTION_PREFIX,"variant {} defines key {} which is not found in normal settings".format(variant,k))
+        for v in variants:
+            if v not in settings.get('variants',{}):
+                if v not in settings.get('run.variants',{}):
+                    print(EXECUTION_PREFIX,"Error, variant",v,"not found in settings")
+                    print(EXECUTION_PREFIX,"Available variants are",list(settings.get('variants',{}).keys())+list(settings.get('run.variants',{}).keys()))
+                    return 1
+                else:
+                    overrides = settings.get('run.variants.'+v)
+                    print(EXECUTION_PREFIX,"APPYING VARIANT",overrides)
+                    config.update_recursive(settings.settings(),overrides,lambda k:caution_callback(k,v))
+            else:
+                overrides = settings.get('variants.'+v)
+                print(EXECUTION_PREFIX,"APPYING VARIANT",overrides)
+                config.update_recursive(settings.settings(),overrides,lambda k:caution_callback(k,v))
+        #don't need this to be saved in the log
+        if 'variants' in settings.settings():
+            del settings.settings()['variants']
+        if 'svariant' in settings.get('run'):
+            del settings.get('run')['variants']
+
+    multiprocessing.set_start_method('spawn')
+    runconfig = settings.get('run')
+    mode = settings.get('run.mode')
+    vehicle_interface_settings = settings.get('run.vehicle_interface')
+    mission_executor_settings = settings.get('run.mission_execution')
+    log_settings = settings.get('run.log',{})
+    replay_settings = settings.get('run.replay',{})
+    load_computation_graph()
+
+    #initialize ros node
+    has_ros = False
+    try:
+        import rospy
+        rospy.init_node('GEM_executor',disable_signals=True)
+        has_ros = True
+    except (ImportError,ModuleNotFoundError):
+        if mode == 'simulation':
+            print(EXECUTION_PREFIX,"Warning, ROS not found, but simulation mode requested")
+        else:
+            print(EXECUTION_PREFIX,"Error, ROS not found on system.")
+            raise
+
+    #create top-level components
+    vehicle_interface = make_class(vehicle_interface_settings,'default','GEMstack.onboard.interface')
+    mission_executor = make_class(mission_executor_settings,'execution','GEMstack.onboard.execution',{'vehicle_interface':vehicle_interface})
+    if not isinstance(mission_executor,ExecutorBase):
+        raise ValueError("Mission executor must be an Executor")
+
+    recovery_pipeline_settings = settings.get('run.recovery')
+    visualization_settings = settings.get('run.visualization',None)
+    state_settings = settings.get('run.state')
+
+    # create pipelines and add to executor
+    pipeline_settings = {'recovery': recovery_pipeline_settings}
+
+    for k,v in runconfig.items():
+        if isinstance(v,dict) and ('perception' in v or 'planning' in v or 'other' in v):
+            # possible pipeline
+            if k not in pipeline_settings:
+                print(EXECUTION_PREFIX,"Adding non-standard pipeline",k)
+                pipeline_settings[k] = v
+
+    visualizers = []
+    if isinstance(visualization_settings,dict):
+        #one visualizer
+        visualizers.append(mission_executor.make_component(visualization_settings,'visualization','GEMstack.onboard.visualization',{'vehicle_interface':vehicle_interface}))
+    elif isinstance(visualization_settings,list):
+        #multiple visualizers
+        for v in visualization_settings:
+            visualizers.append(mission_executor.make_component(v,'visualization','GEMstack.onboard.visualization',{'vehicle_interface':vehicle_interface}))
+    for v in visualizers:
+        mission_executor.always_run(v.c.__class__.__name__,v)
+
+    # mark the components that will be replayed rather than run
+    if replay_settings:
+        logfolder = replay_settings.get('log',None)
+        if logfolder is not None:
+            replay_components = replay_settings.get('components',[])
+            mission_executor.replay_components(replay_components,logfolder)
+
+            #TODO: ROS topic replay
+            logmeta = config.load_config_recursive(os.path.join(logfolder,'meta.yaml'))
+            replay_topics = replay_settings.get('ros_topics',[])
+
+            #TODO: launch a roslog replay of the topics in ros_topics, disable in the vehicle interface
+
+    for state in state_settings:
+        pipeline_settings[state] = settings.get('run.' + state)
+
+    for (name,s) in pipeline_settings.items():
+        perception_settings = s.get('perception',{})
+        planning_settings = s.get('planning',{})
+        other_settings = s.get('other',{})
+
+        perception_components = {}   #type: Dict[str,ComponentExecutor]
+        for (k,v) in perception_settings.items():
+            if v is None: continue
+            perception_components[k] = mission_executor.make_component(v,k,'GEMstack.onboard.perception', {'vehicle_interface':vehicle_interface})
+        planning_components = {}   #type: Dict[str,ComponentExecutor]
+        for (k,v) in planning_settings.items():
+            if v is None: continue
+            planning_components[k] = mission_executor.make_component(v,k,'GEMstack.onboard.planning', {'vehicle_interface':vehicle_interface})
+        other_components = {}   #type: Dict[str,ComponentExecutor]
+        for (k,v) in other_settings.items():
+            if v is None: continue
+            other_components[k] = mission_executor.make_component(v,k,'GEMstack.onboard.other', {'vehicle_interface':vehicle_interface})
+
+        mission_executor.add_pipeline(name, perception_components, planning_components, other_components)
+
+    #configure logging
+    if log_settings:
+        topfolder = log_settings.get('folder','logs')
+        prefix = log_settings.get('prefix','')
+        from datetime import datetime
+        default_suffix = datetime.now().strftime('%Y-%m-%d_%H-%M-%S')
+        suffix = log_settings.get('suffix',default_suffix)
+        logfolder = os.path.join(topfolder,prefix+suffix)
+        print(EXECUTION_PREFIX,"Logging to",logfolder)
+        auto_plot = log_settings.get("auto_plot", False)
+        os.makedirs(logfolder,exist_ok=True)
+
+        #configure logging for components
+        mission_executor.set_log_folder(logfolder)
+        mission_executor.set_auto_plot(auto_plot)
+        #configure ROS logging
+        log_topics = log_settings.get('ros_topics',[])
+        rosbag_options = log_settings.get('rosbag_options','')
+        mission_executor.log_ros_topics(log_topics, rosbag_options)
+        #determine whether to log vehicle interface
+        log_vehicle_interface = log_settings.get('vehicle_interface',False)
+        if log_vehicle_interface:
+            mission_executor.log_vehicle_interface(log_vehicle_interface)
+        #determine whether to log components
+        log_components = log_settings.get('components',[])
+        mission_executor.log_components(log_components)
+        #determine whether to log state
+        log_state_attributes = log_settings.get('state',[])
+        log_state_rate = log_settings.get('state_rate',None)
+        if log_state_attributes:
+            mission_executor.log_state(log_state_attributes,log_state_rate)
+
+    vehicle_interface.start()
+    try:
+        mission_executor.run()
+    except Exception as e:
+        raise
+    finally:
+        vehicle_interface.stop()
+
+    if has_ros:
+        #need manual ros node shutdown due to disable_signals=True
+        rospy.signal_shutdown('GEM_executor finished')
+
+    print(EXECUTION_PREFIX,"---------------- DONE ----------------")
+    if log_settings and settings.get('run.after.show_log_folder',False):
+        if sys.platform.startswith('linux'):
+            os.system('nautilus '+logfolder)
+        else:
+            import webbrowser
+            webbrowser.open(logfolder)
+    return 0
diff --git a/GEMstack/onboard/other/api_client.py b/GEMstack/onboard/other/api_client.py
@@ -0,0 +1,39 @@
+from pydantic import BaseModel, Field
+from typing import List, Tuple, Union
+from ..component import Component
+from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum, AgentState
+import requests
+
+
+### MODELS ###
+
+class BoundingBox(BaseModel):
+    x1: float = Field(..., ge=-90, le=90)
+    x2: float = Field(..., ge=-90, le=90)
+    y1: float = Field(..., ge=-90, le=90)
+    y2: float = Field(..., ge=-90, le=90)
+
+
+class BoundingBoxRequester(Component):
+    """Follows the given route.  Brakes if you have to yield or
+    you are at the end of the route, otherwise accelerates to
+    the desired speed.
+    """
+
+    def __init__(self, mode : str = 'real', params : dict = {"api_endpoint": "xxxx"}):
+        self.api_endpoint = params['api_endpoint']
+        self.bounding_box = None
+
+    def state_inputs(self):
+        return ['all']
+
+    def state_outputs(self) -> List[str]:
+        return self.bounding_box
+
+    def rate(self):
+        return 10.0
+
+    def update(self, state: AllState):
+        # request the bounding box, if found then update and then switch the state
+        if self.bounding_box:
+            return
diff --git a/GEMstack/onboard/planning/inspection_plan.py b/GEMstack/onboard/planning/inspection_plan.py
@@ -0,0 +1,74 @@
+tateimport numpy as np
+import matplotlib.pyplot as plt
+
+
+def is_inside_geofence(x, y, xmin, xmax, ymin, ymax):
+    return xmin < x < xmax and ymin < y < ymax
+
+
+def max_visible_arc(circle_center, radius, geofence):
+    xc, yc = circle_center
+    (xmin, ymin), (xmax, ymax) = geofence
+
+    angles = np.linspace(0, 2 * np.pi, 500, endpoint=False)
+    arc_segments = []
+    curr_segment = []
+
+    first_inside = last_inside = False
+
+    for i, theta in enumerate(angles):
+        x = xc + radius * np.cos(theta)
+        y = yc + radius * np.sin(theta)
+
+        inside = is_inside_geofence(x, y, xmin, xmax, ymin, ymax)
+
+        if i == 0:
+            first_inside = inside
+        if i == len(angles) - 1:
+            last_inside = inside
+
+        if inside:
+            curr_segment.append((x, y))
+        else:
+            if curr_segment:
+                arc_segments.append(curr_segment)
+                curr_segment = []
+
+    if curr_segment:
+        arc_segments.append(curr_segment)
+
+    # If arc wraps around from 2π back to 0, combine first and last segments
+    if first_inside and last_inside and len(arc_segments) > 1:
+        arc_segments[0] = arc_segments[-1] + arc_segments[0]
+        arc_segments.pop()
+
+    if not arc_segments:
+        return []
+
+    max_arc = max(arc_segments, key=len)
+    return max_arc
+
+
+def plot_circle_with_geofence_and_arc(circle_center, radius, geofence, rem_pts):
+    fig, ax = plt.subplots()
+
+    # Plot geofence
+    (xmin, ymin), (xmax, ymax) = geofence
+    rect = plt.Rectangle((xmin, ymin), xmax - xmin, ymax - ymin, linewidth=2, edgecolor='red', facecolor='none')
+    ax.add_patch(rect)
+
+    rem_pts = np.array(rem_pts)
+    ax.plot(rem_pts[:, 0], rem_pts[:, 1], color='blue', linewidth=3, label='Max Valid Arc')
+
+    ax.set_aspect('equal')
+    ax.grid(True)
+    plt.legend()
+    plt.show()
+
+
+circle_center = (5, 6)
+radius = 0.5
+geofence = ((0, 0), (8, 8))
+
+rem_points = max_visible_arc(circle_center, radius, geofence)
+plot_circle_with_geofence_and_arc(circle_center, radius, geofence, rem_points)
diff --git a/launch/state_machine.yaml b/launch/state_machine.yaml
