Add a yaml for summoning, SummoningRoutePlanner, a stadium shape roadgraph, and a roadgraph generator

Author: derekzyn

GEMstack/knowledge/routes/summoning_roadgraph.json

@@ -1,9 +1,10 @@
 from typing import List
 from ..component import Component
 from ...utils import serialization
-from ...state import Route,ObjectFrameEnum
+from ...state import Route,ObjectFrameEnum, AllState, VehicleState, Roadgraph, MissionObjective
 import os
 import numpy as np
+import yaml
 
 class StaticRoutePlanner(Component):
     """Reads a route from disk and returns it as the desired route."""
@@ -39,4 +40,64 @@ def rate(self):
 
     def update(self):
         return self.route
-    
+
+
+def get_all_lane_points(roadgraph: Roadgraph) -> List:
+    all_points = []
+    for value in roadgraph.lanes.values():
+        for pts in value.left.segments:
+            for pt in pts:
+                all_points.append(pt)
+    return all_points
+
+
+# implement route planning method
+def generate_route(roadgraph, destination: List, vehicle: VehicleState, roadgraph_type='roadgraph'):
+    if roadgraph_type == 'roadgraph':
+        all_road_points = get_all_lane_points(roadgraph)
+    elif roadgraph_type == 'point_list':
+        all_road_points = roadgraph
+
+    waypoints = []
+    waypoints.append(destination)
+    return waypoints
+
+
+class SummoningRoutePlanner(Component):
+    """Reads a route from disk and returns it as the desired route."""
+    def __init__(self, roadgraphfn : str, frame : str = 'global'):
+        self.frame = frame
+        base, ext = os.path.splitext(roadgraphfn)
+        if ext in ['.json', '.yml', '.yaml']:
+            with open(roadgraphfn, 'r') as f:
+                self.roadgraph = serialization.load(f)
+                self.all_lane_points = get_all_lane_points(self.roadgraph)
+        elif ext == '.csv':
+            self.roadgraph = np.loadtxt(roadgraphfn,delimiter=',',dtype=float)
+            self.roadgraph_type = 'point_list'
+
+    def state_inputs(self):
+        return ['vehicle', 'mission']
+
+    def state_outputs(self) -> List[str]:
+        return ['route']
+
+    def rate(self):
+        return 1.0
+
+    def update(self, vehicle: VehicleState, mission: MissionObjective):
+        self.vehicle = vehicle
+
+        # TODO: get from the server
+        self.destinations = [(23.0, 3.0, 0.0),(2.0, 6.0, 0.0)]
+
+        waypoints = generate_route(self.roadgraph, self.destinations, self.vehicle,  self.roadgraph_type)
+
+        if self.frame == 'start':
+            self.route = Route(frame=ObjectFrameEnum.START, points=waypoints.tolist())
+        elif self.frame == 'global':
+            self.route = Route(frame=ObjectFrameEnum.GLOBAL, points=waypoints.tolist())
+        elif self.frame == 'cartesian':
+            self.route = Route(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, points=waypoints.tolist())
+
+        return self.route

GEMstack/onboard/planning/route_planning.py

@@ -6,9 +6,10 @@
 __all__ = ['PhysicalObject','ObjectPose','ObjectFrameEnum',
            'Path','Trajectory',
            'VehicleState',
-           'Roadgraph',
+           'Roadgraph', 'RoadgraphLane', 'RoadgraphLaneEnum', 'RoadgraphCurve', 'RoadgraphCurveEnum',
+           'RoadgraphRegion', 'RoadgraphRegionEnum', 'RoadgraphSurfaceEnum', 'RoadgraphConnectionEnum',
            'Roadmap',
-           'Obstacle',
+           'Obstacle', 'ObstacleMaterialEnum',
            'Sign',
            'AgentState','AgentEnum','AgentActivityEnum',
            'SceneState',

GEMstack/state/__init__.py

@@ -0,0 +1,120 @@
+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: 
+            type: pedestrian_yield_logic.PedestrianYielder
+            args:
+                mode: 'real'
+                params: {
+                    'yielder':       'expert', # 'expert', 'analytic', or 'simulation'
+                    'planner':       'milestone', # 'milestone', 'dt', or 'dx'
+                    'desired_speed': 1.0,  # m/s, 1.5 m/s seems max speed? Feb24
+                    'acceleration':  0.75  # m/s2, 0.5 is not enough to start moving. Feb24
+                    }
+        route_planning:
+            # type: StaticRoutePlanner
+            # args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start']
+            type: SummoningRoutePlanner
+            args: [!relative_path '../GEMstack/knowledge/routes/summoning_roadgraph.yaml','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
+    # If True, then make plots of the recorded data specifically from PurePursuitTrajectoryTracker_debug.csv and report metrics. Default False
+    #auto_plot : True
+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 : []
+
+#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:
+    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 "klampt_visualization.yaml"
+            drive: 
+                perception:
+                    state_estimation : OmniscientStateEstimator
+                planning:
+                    relations_estimation: 
+                        type: pedestrian_yield_logic.PedestrianYielder
+                        args:
+                            mode: 'sim'
+
+    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
+                planning:
+                    relations_estimation: 
+                        type: pedestrian_yield_logic.PedestrianYielder
+                        args:
+                            mode: 'sim'

launch/summoning.yaml

@@ -11,4 +11,4 @@ dacite
 # Perception
 ultralytics
 open3d
-sklearn
+scikit-learn