added summoning planning code

Author: jeffbyju

GEMstack/knowledge/routes/forward_15m.csv

@@ -0,0 +1,16 @@
+0.0,0,0
+1.0,0,0
+2.0,0,0
+3.0,0,0
+4,0,0
+5,0,0
+6,0,0
+7,0,0
+8,0,0
+9,0,0
+10,0,0
+11,0,0
+12,0,0
+13,0,0
+14,0,0
+15,0,0

GEMstack/onboard/planning/summon_component.py

@@ -0,0 +1,37 @@
+from typing import List
+from ..component import Component
+from ...utils import serialization
+from ...state import Route,ObjectFrameEnum, AllState, PlannerEnum
+import os
+import numpy as np
+
+class SummonSim(Component):
+    """Simulates a summoning team component that receives an AllState object and outputs a route object."""
+    def __init__(self):
+        self.counter = 0
+
+    def state_inputs(self):
+        return ["all"]
+
+    def state_outputs(self) -> List[str]:
+        return ["route"]
+
+    def rate(self):
+        return 10.0
+
+    def update(self, state: AllState):      
+        print("SummonSim update with state:", state)
+        base_path = os.path.dirname(__file__)
+        file_path = os.path.join(base_path, "../../knowledge/routes/forward_15m.csv")
+    
+        waypoints = np.loadtxt(file_path, delimiter=',', dtype=float)
+        if waypoints.shape[1] == 3:
+                waypoints = waypoints[:,:2]
+        print("waypoints", waypoints)
+        route = Route(frame=ObjectFrameEnum.START,points=waypoints.tolist())
+        print("route", route)
+        return route
+    
+    
+    
+    

GEMstack/state/__init__.py

@@ -15,7 +15,8 @@
            'VehicleIntent','VehicleIntentEnum',
            'AgentIntent',
            'EntityRelationEnum','EntityRelation','EntityRelationGraph',
-           'MissionEnum','MissionObjective',
+           'MissionEnum','MissionObjective', 'MissionPlan',
+           'PlannerEnum',
            'Route',
            'PredicateValues',
            'AllState']
@@ -32,6 +33,7 @@
 from .agent_intent import AgentIntent
 from .relations import EntityRelation, EntityRelationEnum, EntityRelationGraph
 from .mission import MissionEnum,MissionObjective
-from .route import Route
+from .route import Route, PlannerEnum
+from .mission_plan import MissionPlan
 from .predicates import PredicateValues
 from .all import AllState

launch/summon_trapezoidal_planner_only.yaml

@@ -0,0 +1,82 @@
+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
+# require_engaged: False
+# 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/forward_15m.csv'] #forward_15m
+        summon_component: 
+            type: SummonSim
+        motion_planning: longitudinal_planning.YieldTrajectoryPlanner
+            # 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: [null]
+            # 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 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 : ['/septentrio_gnss/insnavgeod','/pacmod/parsed_tx/vehicle_speed_rpt']
+    # 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','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
+    #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 settingsNone 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", !include "mpl_visualization.yaml"]
+    log_ros:
+        log:
+            ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml"