Merge branch 'parking' of https://github.com/krishauser/GEMstack into parking

Author: KenC1014

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -54,4 +54,4 @@ components:
       outputs: 
   - signaling:
       inputs: [intent] 
-      outputs: 
+      outputs: 

GEMstack/onboard/parking_entry_point.py

@@ -21,7 +21,7 @@ def speed_callback(msg):
 def parking_space_callback(msg):
     global detected_space
     global endpoints
-    detected_space = true
+    detected_space = True
     endpoints = msg
     rospy.loginfo(f"Vehicle Speed: {msg.vehicle_speed} m/s | Valid: {msg.vehicle_speed_valid}")
 

GEMstack/onboard/planning/parking_component.py

@@ -0,0 +1,36 @@
+from typing import List
+from ..component import Component
+from ...utils import serialization
+from ...state import Route,ObjectFrameEnum, AllState, PlannerEnum, MissionPlan
+import os
+import numpy as np
+import time
+
+class ParkingSim(Component):
+    def __init__(self):
+        self.start_time = time.time()
+        pass
+
+    def state_inputs(self):
+        return ["all"]
+
+    def rate(self):
+        return 10.0
+
+    def state_outputs(self)-> List[str]:
+        return ["mission_plan"]
+    
+    def update(self, state: AllState):
+        # Calculate elapsed time since initialization.
+        elapsed_time = time.time() - self.start_time
+        
+        # After 4 seconds, change the mission plan to use PARKING.
+        if elapsed_time >= 4.0:
+            print("Entering parking mode")
+            mission_plan = MissionPlan(1, 6, 0, PlannerEnum.PARKING)
+        else:
+            print("Entering RRT mode")
+            mission_plan = MissionPlan(1, 6, 0, PlannerEnum.RRT_STAR)
+
+        print("ParkingSim update with state:",mission_plan)
+        return mission_plan

GEMstack/onboard/planning/route_planning_component.py

@@ -0,0 +1,55 @@
+import os
+from typing import Dict, List
+
+import numpy as np
+from GEMstack.onboard.component import Component
+from GEMstack.state.agent import AgentState
+from GEMstack.state.all import AllState
+from GEMstack.state.physical_object import ObjectFrameEnum
+from GEMstack.state.route import PlannerEnum, Route
+
+
+
+class RoutePlanningComponent(Component):
+    """Reads a route from disk and returns it as the desired route."""
+    def __init__(self):
+        print("Route Planning Component init")
+        self.planner = None
+        self.route = None
+        
+    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("Route Planner's mission:", state.mission_plan.planner_type.value)
+        # print("type of mission plan:", type(PlannerEnum.RRT_STAR))
+        # print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.RRT_STAR.value)
+        # print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.PARKING.value)
+        # print("Mission plan:", state.mission_plan)
+        # print("Vehicle x:", state.vehicle.pose.x)
+        # print("Vehicle y:", state.vehicle.pose.y)
+        # print("Vehicle yaw:", state.vehicle.pose.yaw)
+        if state.mission_plan.planner_type.value == PlannerEnum.PARKING.value:
+            print("I am in PARKING mode")
+            # Return a route after doing some processing based on mission plan REMOVE ONCE OTHER PLANNERS ARE IMPLEMENTED
+            base_path = os.path.dirname(__file__)
+            file_path = os.path.join(base_path, "../../knowledge/routes/forward_15m_extra.csv")
+        
+            waypoints = np.loadtxt(file_path, delimiter=',', dtype=float)
+            if waypoints.shape[1] == 3:
+                    waypoints = waypoints[:,:2]
+            print("waypoints", waypoints)
+            self.route = Route(frame=ObjectFrameEnum.START,points=waypoints.tolist())
+        elif state.mission_plan.planner_type.value == PlannerEnum.RRT_STAR.value:
+            print("I am in RRT mode")
+            
+        else:
+            print("Unknown mode")
+        
+        return self.route

launch/parking_route_planner.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:
+        parking_component:
+            type: ParkingSim
+        route_planning_component:
+            type: RoutePlanningComponent
+        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"