fixing recent commit revert issues

GEMstack/onboard/perception/pedestrian_detection.py

@@ -133,9 +133,6 @@ def init_debug(self,) -> None:
         self.pub_image = rospy.Publisher("/camera/image_detection", Image, queue_size=1)
 
     def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
-
-        # Edge cases
-
         # Initial vehicle pose data
         if(self.current_vehicle_state == None and self.previous_vehicle_state == None):
             self.current_vehicle_state = vehicle
@@ -150,6 +147,12 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
             self.previous_vehicle_state = self.current_vehicle_state
             self.current_vehicle_state = vehicle
 
+        # Update times for basic velocity calculation
+        self.prev_time = self.curr_time
+        self.curr_time = self.current_vehicle_state.pose.t
+
+        # Edge Cases:
+
         # edge case to handle no pedestrian data
         if(self.current_agent_obj_dims == {}):
             return {}
@@ -163,9 +166,6 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
         if len(self.current_agent_obj_dims['pose']) != len(self.current_agent_obj_dims['dims']):
             raise Exception( f"Length of extracted poses ({len(self.current_agent_obj_dims['pose'])}) and dimensions ({len(self.current_agent_obj_dims['dims'])}) are not equal")
 
-        # Update current time:
-        self.curr_time = vehicle.pose.t
-
         # (f"Global state : {vehicle}")
 
         # Convert pose to start state. Need to use previous_vehicle state as pedestrian info is delayed
@@ -534,11 +534,6 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n
     #     return vels
 
     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.curr_time = datetime.now() # Updating in update function now
-
         self.cv_image = cv_image
         self.lidar_points = lidar_points
 
@@ -629,4 +624,4 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
             if t >= times[0] and t <= times[1]:
                 res['pedestrian0'] = box_to_fake_agent((0,0,0,0))
                 print("Detected a pedestrian")
-        return res
+        return res

launch/fixed_route_combined.yaml

@@ -0,0 +1,81 @@
+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
+        agent_detection : pedestrian_detection.PedestrianDetector2D
+    planning:
+        route_planning:
+            type: StaticRoutePlanner
+            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]
+            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 : pedestrian_detection.PedestrianDetector2D
+            visualization: [!include "klampt_visualization.yaml", !include "mpl_visualization.yaml"]
+    log_ros:
+        log:
+            ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml"