diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index 9d79ad574..8a3125ecd 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/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 \ No newline at end of file diff --git a/launch/fixed_route_combined.yaml b/launch/fixed_route_combined.yaml new file mode 100644 index 000000000..85a68b44b --- /dev/null +++ b/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"