working car move but no stop

Author: GEM e4 user

GEMstack/onboard/perception/pedestrian_detection.py

@@ -415,6 +415,8 @@ def find_vels_and_ids(self, agents: List[AgentState]):
                 agents[idx].velocity = (0, 0, 0)
                 self.current_agents[id] = agents[idx]
                 backOrder[idx] = id
+        print("BACKORDER")
+        print(backOrder)
         return backOrder
 
     # Calculates whether 2 agents overlap. True if they do, false if not
@@ -536,7 +538,7 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n
     def ouster_oak_callback(self, cv_image: cv2.Mat, lidar_points: np.ndarray):
 
         # Update times for basic velocity calculation
-        self.prev_time = self.curr_time
+        # self.prev_time = self.curr_t
         # self.curr_time = datetime.now() # Updating in update function now
 
         self.cv_image = cv_image

GEMstack/onboard/planning/pure_pursuit.py

@@ -21,6 +21,8 @@ def __init__(self, lookahead = None, lookahead_scale = None, crosstrack_gain = N
         self.wheel_angle_range = [settings.get('vehicle.geometry.min_wheel_angle'),settings.get('vehicle.geometry.max_wheel_angle')]
         self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')]
 
+        self.max_decel = 100
+
         if desired_speed is not None:
             self.desired_speed_source = desired_speed
         else:

geonav_transform

@@ -0,0 +1 @@
+Subproject commit 8894342ca6821514f5401ccf63516041d07e44b5

launch/fixed_route.yaml

@@ -18,10 +18,10 @@ drive:
     planning:
         route_planning:
             type: StaticRoutePlanner
-            args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] #forward_15m
-        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
+            args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] #forward_15m
+        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]

launch/fixed_route_combined.yaml

@@ -15,6 +15,7 @@ drive:
         state_estimation : GNSSStateEstimator
         perception_normalization : StandardPerceptionNormalizer
         agent_detection : pedestrian_detection.PedestrianDetector2D
+        # agent_detection : pedestrian_detection.FakePedestrianDetector2D
     planning:
         route_planning:
             type: StaticRoutePlanner