diff --git a/.gitignore b/.gitignore index 0c36d682f..61a2df38b 100644 --- a/.gitignore +++ b/.gitignore @@ -165,3 +165,15 @@ cython_debug/ # and can be added to the global gitignore or merged into this file. For a more nuclear # option (not recommended) you can uncomment the following to ignore the entire idea folder. #.idea/ + +# ZED run files +**/*.run +.vscode/ +setup/zed_sdk.run +cuda/ +homework/yolov8n.pt +homework/yolo11n.pt +GEMstack/knowledge/detection/yolov8n.pt +GEMstack/knowledge/detection/yolo11n.pt +yolov8n.pt +yolo11n.pt \ No newline at end of file diff --git a/GEMstack/knowledge/calibration/gem_e2_gazebo.yaml b/GEMstack/knowledge/calibration/gem_e2_gazebo.yaml new file mode 100644 index 000000000..33f78368b --- /dev/null +++ b/GEMstack/knowledge/calibration/gem_e2_gazebo.yaml @@ -0,0 +1,59 @@ +calibration_date: "2023-08-31" # Date of calibration YYYY-MM-DD +reference: rear_axle_center # rear axle center +rear_axle_height: 0.273 # height of rear axle center above flat ground +gnss_location: [1.1,0.0,0.3] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py. Note conflict with pure pursuit location? +gnss_yaw: 0.0 # radians +front_radar_location: [1.45,0,0.07] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/platform_launch/launch/white_e2/platform.launch +top_lidar: !include "gem_e2_velodyne.yaml" +front_camera: !include "gem_e2_zed.yaml" +T_frontcamera_vehicle: +- - 0.04979815921110274 + - -0.02889308605338095 + - 0.998341277156963 + - 0.6874899022540225 +- - -0.9986898492450907 + - 0.010343220052680617 + - 0.05011489006575621 + - -0.006750728511462573 +- - -0.011774038000135968 + - -0.9995289850272167 + - -0.02834016107658961 + - -0.07694233627246522 +- - 0.0 + - 0.0 + - 0.0 + - 1.0 +T_toplidar_frontcamera: +- - 0.029096143490519977 + - -0.9995463314295703 + - -0.007781115579835407 + - -0.045768971801312515 +- - -0.005777780139254174 + - 0.007616104089319731 + - -0.999954305063568 + - -0.3868171492765085 +- - 0.9995599190096819 + - 0.02913977151915098 + - -0.005553559684576381 + - -0.6975440651652165 +- - 0.0 + - 0.0 + - 0.0 + - 1.0 +T_toplidar_vehicle: +- - 0.9995177984237671 + - -0.020904183387756348 + - 0.022959932684898376 + - 0.0 +- - 0.020975051447749138 + - 0.9997758865356445 + - -0.0028501423075795174 + - 0.0 +- - -0.0228952094912529 + - 0.003330353880301118 + - 0.9997323155403137 + - 0.33000001311302185 +- - 0.0 + - 0.0 + - 0.0 + - 1.0 diff --git a/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml b/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml new file mode 100644 index 000000000..d6dfc3185 --- /dev/null +++ b/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml @@ -0,0 +1,64 @@ +date: '2025-02-25 22:31:02.057447' +calibration_type: absolute_vehicle_calibration +gnss_location: +- 1.1 +- 0 +- 1.62 +gnss_yaw: 0.0 +rear_axle_height: 0.33 +reference: rear_axle_center +vehicle: gem_e4 +top_lidar: !include "gem_e4_ouster.yaml" +front_camera: !include "gem_e4_oak.yaml" +gnss_reference_point: [40.114001, -88.228837] +T_frontcamera_vehicle: +- - 0.04979815921110274 + - -0.02889308605338095 + - 0.998341277156963 + - 0.6874899022540225 +- - -0.9986898492450907 + - 0.010343220052680617 + - 0.05011489006575621 + - -0.006750728511462573 +- - -0.011774038000135968 + - -0.9995289850272167 + - -0.02834016107658961 + - -0.07694233627246522 +- - 0.0 + - 0.0 + - 0.0 + - 1.0 +T_toplidar_frontcamera: +- - 0.029096143490519977 + - -0.9995463314295703 + - -0.007781115579835407 + - -0.045768971801312515 +- - -0.005777780139254174 + - 0.007616104089319731 + - -0.999954305063568 + - -0.3868171492765085 +- - 0.9995599190096819 + - 0.02913977151915098 + - -0.005553559684576381 + - -0.6975440651652165 +- - 0.0 + - 0.0 + - 0.0 + - 1.0 +T_toplidar_vehicle: +- - 0.9995177984237671 + - -0.020904183387756348 + - 0.022959932684898376 + - 0.0 +- - 0.020975051447749138 + - 0.9997758865356445 + - -0.0028501423075795174 + - 0.0 +- - -0.0228952094912529 + - 0.003330353880301118 + - 0.9997323155403137 + - 0.33000001311302185 +- - 0.0 + - 0.0 + - 0.0 + - 1.0 \ No newline at end of file diff --git a/GEMstack/knowledge/defaults/__init__.py b/GEMstack/knowledge/defaults/__init__.py index ee5354a7c..f7126e6a8 100644 --- a/GEMstack/knowledge/defaults/__init__.py +++ b/GEMstack/knowledge/defaults/__init__.py @@ -1,7 +1,36 @@ from ...utils.config import load_config_recursive -import os -SETTINGS_FILE = os.path.join(os.path.split(__file__)[0],'current.yaml') +import os,sys + + +# Identify the vehicle used and load settings else default to vehicle config at current.yaml' +# default currently uses GEM e4 +vehicle = "default" + +for arg in sys.argv: + if arg.startswith('--'): + k,v = arg.split('=',1) + k = k[2:] + v = v.strip('"') + if k == "vehicle": + vehicle = v + sys.argv.remove(arg) + break + +if(vehicle == 'e2'): + vehicle_config = 'e2.yaml' +elif(vehicle == 'e4'): + vehicle_config = 'current.yaml' +elif(vehicle == 'e2_gazebo'): + vehicle_config = 'e2_gazebo.yaml' +elif(vehicle == 'e4_gazebo'): + vehicle_config = 'e4_gazebo.yaml' +else: + print("Unknown vehicle argument passed") + vehicle = "default" + vehicle_config = 'current.yaml' + +SETTINGS_FILE = os.path.join(os.path.split(__file__)[0],vehicle_config) print("**************************************************************") -print("Loading default settings from",SETTINGS_FILE) +print(f"Loading {vehicle} settings from",SETTINGS_FILE) print("**************************************************************") SETTINGS = load_config_recursive(SETTINGS_FILE) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 645395d4f..5c6331b23 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -1,8 +1,7 @@ # ********* Main settings entry point for behavior stack *********** # Configure settings for the vehicle / vehicle model -# vehicle: !include ../vehicle/gem_e4.yaml -vehicle: !include ../vehicle/gem_e2_gazebo.yaml +vehicle: !include ../vehicle/gem_e4.yaml #arguments for algorithm components here model_predictive_controller: @@ -13,8 +12,8 @@ control: brake_amount : 0.5 brake_speed : 2.0 pure_pursuit: - lookahead: 2.0 - lookahead_scale: 3.0 + lookahead: 4.0 + lookahead_scale: 1.0 crosstrack_gain: 1.0 desired_speed: trajectory longitudinal_control: diff --git a/GEMstack/knowledge/defaults/e2.yaml b/GEMstack/knowledge/defaults/e2.yaml new file mode 100644 index 000000000..ed8d26d19 --- /dev/null +++ b/GEMstack/knowledge/defaults/e2.yaml @@ -0,0 +1,34 @@ +# ********* Main settings entry point for behavior stack *********** + +# Configure settings for the vehicle / vehicle model +vehicle: !include ../vehicle/gem_e2.yaml + +#arguments for algorithm components here +model_predictive_controller: + dt: 0.1 + lookahead: 20 +control: + recovery: + brake_amount : 0.5 + brake_speed : 2.0 + pure_pursuit: + lookahead: 2.0 + lookahead_scale: 3.0 + crosstrack_gain: 1.0 + desired_speed: trajectory + longitudinal_control: + pid_p: 1.0 + pid_i: 0.1 + pid_d: 0.0 + +#configure the simulator, if using +simulator: + dt: 0.01 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + gnss_emulator: + dt: 0.1 #10Hz + #position_noise: 0.1 #10cm noise + #orientation_noise: 0.04 #2.3 degrees noise + #velocity_noise: + # constant: 0.04 #4cm/s noise + # linear: 0.02 #2% noise \ No newline at end of file diff --git a/GEMstack/knowledge/defaults/e2_gazebo.yaml b/GEMstack/knowledge/defaults/e2_gazebo.yaml new file mode 100644 index 000000000..4a8b55c19 --- /dev/null +++ b/GEMstack/knowledge/defaults/e2_gazebo.yaml @@ -0,0 +1,34 @@ +# ********* Main settings entry point for behavior stack *********** + +# Configure settings for the vehicle / vehicle model +vehicle: !include ../vehicle/gem_e2_gazebo.yaml + +#arguments for algorithm components here +model_predictive_controller: + dt: 0.1 + lookahead: 20 +control: + recovery: + brake_amount : 0.5 + brake_speed : 2.0 + pure_pursuit: + lookahead: 4.0 + lookahead_scale: 3.0 + crosstrack_gain: 1.0 + desired_speed: trajectory + longitudinal_control: + pid_p: 0.1 + pid_i: 0.5 + pid_d: 0.5 + +#configure the simulator, if using +simulator: + dt: 0.01 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + gnss_emulator: + dt: 0.1 #10Hz + #position_noise: 0.1 #10cm noise + #orientation_noise: 0.04 #2.3 degrees noise + #velocity_noise: + # constant: 0.04 #4cm/s noise + # linear: 0.02 #2% noise \ No newline at end of file diff --git a/GEMstack/knowledge/defaults/e4_gazebo.yaml b/GEMstack/knowledge/defaults/e4_gazebo.yaml new file mode 100644 index 000000000..7a42a5a8f --- /dev/null +++ b/GEMstack/knowledge/defaults/e4_gazebo.yaml @@ -0,0 +1,34 @@ +# ********* Main settings entry point for behavior stack *********** + +# Configure settings for the vehicle / vehicle model +vehicle: !include ../vehicle/gem_e4_gazebo.yaml + +#arguments for algorithm components here +model_predictive_controller: + dt: 0.1 + lookahead: 20 +control: + recovery: + brake_amount : 0.5 + brake_speed : 2.0 + pure_pursuit: + lookahead: 2.5 + lookahead_scale: 3.0 + crosstrack_gain: 0.5 + desired_speed: trajectory + longitudinal_control: + pid_p: 1.5 + pid_i: 0.2 + pid_d: 0.0 + +#configure the simulator, if using +simulator: + dt: 0.01 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + gnss_emulator: + dt: 0.1 #10Hz + #position_noise: 0.1 #10cm noise + #orientation_noise: 0.04 #2.3 degrees noise + #velocity_noise: + # constant: 0.04 #4cm/s noise + # linear: 0.02 #2% noise \ No newline at end of file diff --git a/GEMstack/knowledge/vehicle/gem_e2_gazebo.yaml b/GEMstack/knowledge/vehicle/gem_e2_gazebo.yaml index bb5aaaa54..c82a8b0dc 100644 --- a/GEMstack/knowledge/vehicle/gem_e2_gazebo.yaml +++ b/GEMstack/knowledge/vehicle/gem_e2_gazebo.yaml @@ -9,5 +9,5 @@ geometry: !include gem_e2_geometry.yaml dynamics: !include gem_e2_dynamics.yaml limits: !include gem_e2_slow_limits.yaml control_defaults: !include gem_e2_control_defaults.yaml -calibration: !include ../calibration/gem_e2.yaml +calibration: !include ../calibration/gem_e2_gazebo.yaml sensors: !include gem_e2_sensor_environment_gazebo.yaml diff --git a/GEMstack/knowledge/vehicle/gem_e2_sensor_environment_gazebo.yaml b/GEMstack/knowledge/vehicle/gem_e2_sensor_environment_gazebo.yaml index 678ec2c3a..94ab558fc 100644 --- a/GEMstack/knowledge/vehicle/gem_e2_sensor_environment_gazebo.yaml +++ b/GEMstack/knowledge/vehicle/gem_e2_sensor_environment_gazebo.yaml @@ -1,6 +1,7 @@ ros_topics: top_lidar: /velodyne_points front_camera: /front_single_camera/image_raw - front_depth: /zed2/zed_node/depth/depth_registered - gnss: /gps/fix + front_depth: /notopic + gps: /gps/fix + imu: /imu \ No newline at end of file diff --git a/GEMstack/knowledge/vehicle/gem_e4_gazebo.yaml b/GEMstack/knowledge/vehicle/gem_e4_gazebo.yaml new file mode 100644 index 000000000..750b41fb1 --- /dev/null +++ b/GEMstack/knowledge/vehicle/gem_e4_gazebo.yaml @@ -0,0 +1,13 @@ +name: GEM +version: e4 +max_gear : 1 +num_wiper_settings : 1 +enable_through_joystick : true #turn this to false to have GEMstack enable control +max_command_rate : 10.0 #for hardware, max rate of commands to send to vehicle over Pacmod +#using !include directives helps maintain reuse of common settings +geometry: !include gem_e4_geometry.yaml +dynamics: !include gem_e4_dynamics.yaml +limits: !include gem_e2_slow_limits.yaml +control_defaults: !include gem_e2_control_defaults.yaml +calibration: !include ../calibration/gem_e4_gazebo.yaml +sensors: !include gem_e4_sensor_environment_gazebo.yaml diff --git a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment_gazebo.yaml b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment_gazebo.yaml new file mode 100644 index 000000000..94ab558fc --- /dev/null +++ b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment_gazebo.yaml @@ -0,0 +1,7 @@ +ros_topics: + top_lidar: /velodyne_points + front_camera: /front_single_camera/image_raw + front_depth: /notopic + gps: /gps/fix + imu: /imu + \ No newline at end of file diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 96f64967a..23d328054 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -1,314 +1,43 @@ from .gem import * -from ...utils import config,conversions,settings +from ...utils import settings import math -import time -import copy -from typing import List -from ...mathutils.dubins import SecondOrderDubinsCar -from ...mathutils.dynamics import simulate -from ...mathutils import transforms -from ...state import VehicleState,ObjectPose,ObjectFrameEnum,Roadgraph,AgentState,AgentEnum,AgentActivityEnum,Obstacle,Sign,AllState -from ...knowledge.vehicle.geometry import front2steer,steer2front,heading_rate -from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration, acceleration_to_pedal_positions -from ...utils.loops import TimedLooper -from dataclasses import replace -from threading import Thread,Lock - +import time # ROS Headers import rospy -from std_msgs.msg import String, Bool, Float32, Float64 -from sensor_msgs.msg import Image,PointCloud2,NavSatFix -try: - from novatel_gps_msgs.msg import NovatelPosition, NovatelXYZ, Inspva -except ImportError: - print("novatel_gps_msgs package missing ") - pass -try: - from septentrio_gnss_driver.msg import INSNavGeod -except ImportError: - print("septentrio_gnss_driver package missing ") - pass - -from radar_msgs.msg import RadarTracks -from tf.transformations import euler_from_quaternion, quaternion_from_euler - -# GEM PACMod Headers -from pacmod_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt +from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix +from geometry_msgs.msg import Vector3Stamped +from sensor_msgs.msg import JointState # For reading joint states from Gazebo +# Changed from AckermannDriveStamped +from ackermann_msgs.msg import AckermannDrive +from rosgraph_msgs.msg import Clock +from tf.transformations import euler_from_quaternion + + +from ...state import ObjectPose,ObjectFrameEnum +from ...knowledge.vehicle.geometry import steer2front +from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration # OpenCV and cv2 bridge import cv2 import numpy as np +from ...utils import conversions +from ...mathutils import transforms +@dataclass +class GNSSReading: + pose: ObjectPose + speed: float + status: str -AGENT_DIMENSIONS = { - 'pedestrian' : (0.5,0.5,1.6), - 'bicyclist' : (1.8,0.5,1.6), - 'car' : (4.0,2.5,1.4), - 'medium_truck': (6.0,2.5,3.0), - 'large_truck': (10.0,2.5,3.5) -} - -AGENT_TYPE_TO_ENUM = { - 'pedestrian' : AgentEnum.PEDESTRIAN, - 'bicyclist' : AgentEnum.BICYCLIST, - 'car' : AgentEnum.CAR, - 'medium_truck': AgentEnum.MEDIUM_TRUCK, - 'large_truck': AgentEnum.LARGE_TRUCK -} - -AGENT_NOMINAL_VELOCITY = { - 'pedestrian' : 1.5, - 'bicyclist' : 5.0, - 'car' : 20.0, - 'medium_truck': 15.0, - 'large_truck': 10.0 -} - -AGENT_NOMINAL_ACCELERATION = { - 'pedestrian' : 2.0, - 'bicyclist' : 2.0, - 'car' : 5.0, - 'medium_truck': 3.0, - 'large_truck': 2.0 -} - -class AgentSimulation: - def __init__(self, config): - self.type = config['type'] - self.position = config['position'][:] - self.velocity = config.get('velocity',[0,0]) - self.nominal_velocity = config.get('nominal_velocity',AGENT_NOMINAL_VELOCITY[self.type]) - self.target = config.get('target',None) - self.target_radius = config.get('target_radius',0.1) - self.target_path = config.get('target_path',None) - self.behavior = config['behavior'] - self.start = self.position[:] - self.yaw = config.get('yaw',0) - - def to_agent_state(self) -> AgentState: - pose = ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN,t=time.time(),x=self.position[0],y=self.position[1],yaw=self.yaw) - activity = AgentActivityEnum.MOVING if self.velocity[0] != 0 or self.velocity[1] != 0 else AgentActivityEnum.STOPPED - return AgentState(pose=pose,dimensions=AGENT_DIMENSIONS[self.type],outline=None, - type=AGENT_TYPE_TO_ENUM[self.type], - activity=activity,velocity=(self.velocity[0],self.velocity[1],0),yaw_rate=0.0) - - def advance(self,dt): - if self.behavior == 'stationary': - self.velocity = [0,0] - return - elif self.behavior == 'target': - if self.target is not None: - self.seek_target(self.target,dt) - elif self.target_path is not None: - raise NotImplementedError("Path following not implemented yet") - elif self.behavior == 'loop': - if self.target is not None: - self.seek_target(self.target,dt) - if np.linalg.norm((self.position[0]-self.target[0],self.position[1]-self.target[1])) < self.target_radius: - self.target,self.start = self.start,self.target - elif self.target_path is not None: - raise NotImplementedError("Path following not implemented yet") - else: - raise ValueError("Unknown behavior "+self.behavior) - - def seek_target(self,target,dt): - dx = target[0] - self.position[0] - dy = target[1] - self.position[1] - d = np.linalg.norm((dx,dy)) - v = np.linalg.norm(self.velocity) - if d < 0.02: - self.velocity = [0,0] - return True - direction = (dx/d,dy/d) - dleft = d - self.target_radius - if v < self.nominal_velocity: - v += AGENT_NOMINAL_ACCELERATION[self.type]*dt - if 0.5*v**2/AGENT_NOMINAL_ACCELERATION[self.type] > dleft: - v -= AGENT_NOMINAL_ACCELERATION[self.type]*dt - self.velocity = [v*direction[0],v*direction[1]] - self.position = transforms.vector_madd(self.position,self.velocity,dt) - - -class GEMDoubleIntegratorSimulation: - """Standard simulation of a second-order Dubins car with a double - integrator controller. The simulation is deterministic and accepts - GEMVehicleReading and GEMVehicleCommand objects. - - Gear switching is instantaneous. Signals are activated instantly. - """ - def __init__(self, scene : str = None): - self.dubins = SecondOrderDubinsCar( - wheelAngleMin=settings.get('vehicle.geometry.min_wheel_angle'), - wheelAngleMax=settings.get('vehicle.geometry.max_wheel_angle'), - velocityMin=-settings.get('vehicle.limits.max_reverse_speed'), - velocityMax=settings.get('vehicle.limits.max_speed'), - accelMin=-settings.get('vehicle.limits.max_acceleration'), - accelMax=settings.get('vehicle.limits.max_deceleration'), - wheelAngleRateMin=-settings.get('vehicle.limits.max_steering_rate'), - wheelAngleRateMax=settings.get('vehicle.limits.max_steering_rate'), - wheelBase=settings.get('vehicle.geometry.wheelbase')) - - self.dt = settings.get('simulator.dt',0.01) - self.roadgraph = None - self.agents = [] - if scene is None: - scene = settings.get('simulator.scene',None) - if isinstance(scene,str): - print("Loading simulator from scene",scene) - scene = config.load_config_recursive(scene) - self.agents = {} - if scene is None: - self.simulation_time = time.time() - self.start_state = (0.0,0.0,0.0) - else: - self.simulation_time = scene.get('time',time.time()) - start_state = scene.get('vehicle_state',[0.0,0.0,0.0,0.0,0.0]) - while len(start_state) < 5: - start_state.append(0.0) - for k,a in scene.get('agents',{}).items(): - self.agents[k] = AgentSimulation(a) - self.cur_vehicle_state = np.array(start_state,dtype=float) - self.last_reading = GEMVehicleReading() - self.last_reading.speed = 0.0 - self.last_reading.steering_wheel_angle = 0.0 - self.last_reading.accelerator_pedal_position = 0.0 - self.last_reading.brake_pedal_position = 0.0 - self.last_reading.gear = 0 - self.last_reading.left_turn_signal = False - self.last_reading.right_turn_signal = False - self.last_reading.horn_on = False - self.last_reading.wiper_level = 0 - self.last_reading.headlights_on = False - #initialize last command - gear = -2 if self.cur_vehicle_state[3] == 0 else -1 if self.cur_vehicle_state[3] < 0 else 1 - steering_wheel_angle = front2steer(self.cur_vehicle_state[4]) - self.last_command = GEMVehicleCommand(gear,0,0,0,0,steering_wheel_angle,0) - - def time(self) -> float: - return self.simulation_time - - def simulate(self, T : float, command : Optional[GEMVehicleCommand]): - if command is not None: - self.last_command = command - for k,a in self.agents.items(): - a.advance(T) - x,y,theta,v,phi = self.cur_vehicle_state - #print("x %.2f y %.2f theta %.2f v %.2f" % (x,y,theta,v)) - #simulate actuators - accelerator_pedal_position = np.clip(self.last_command.accelerator_pedal_position,0.0,1.0) - brake_pedal_position = np.clip(self.last_command.brake_pedal_position,0.0,1.0) - acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1) - acceleration = np.clip(acceleration,*self.dubins.accelRange) - phides = steer2front(self.last_command.steering_wheel_angle) - phides = np.clip(phides,*self.dubins.wheelAngleRange) - h = 0.01 #just for finite differencing - front_wheel_angle_rate = (steer2front(self.last_command.steering_wheel_angle + h*self.last_command.steering_wheel_speed) - steer2front(self.last_command.steering_wheel_angle)) / h - front_wheel_angle_rate_max = 2.0 #TODO: get from vehicle model - phi_deadband = 2*self.dt*front_wheel_angle_rate_max - steering_angle_rate = front_wheel_angle_rate if phides > phi + phi_deadband else \ - (-front_wheel_angle_rate if phides < phi - phi_deadband else 0.0) - - #simulate dynamics - u = np.array([acceleration,steering_angle_rate]) #acceleration, steering angle rate - #print("Accel %.2f, steering angle current %.2f, desired %.2f, rate %.2f" % (acceleration,phi,phides,steering_angle_rate)) - next = simulate(self.dubins, self.cur_vehicle_state, (lambda x,t: u), T, self.dt) - next_state = next['x'][-1] - #braking deadband - if v > 0 and next_state[3] < 0: - next_state[3] = 0 - if v < 0 and next_state[3] > 0: - next_state[3] = 0 - x,y,theta,v,phi = next_state - v = np.clip(v,*self.dubins.velocityRange) - next_state = np.array([x,y,theta,v,phi]) - - #simulate sensors - reading = copy.copy(self.last_reading) - reading.steering_wheel_angle = front2steer(phi) - if acceleration > 0: - reading.brake_pedal_position = 0.0 - reading.accelerator_pedal_position = accelerator_pedal_position - else: - reading.brake_pedal_position = brake_pedal_position - reading.accelerator_pedal_position = 0 - reading.speed = v - if v > 0: - reading.gear = 1 - else: - reading.gear = -1 - #copy signals - reading.left_turn_signal = self.last_command.left_turn_signal - reading.right_turn_signal = self.last_command.right_turn_signal - reading.headlights_on = self.last_command.headlights_on - reading.horn_on = self.last_command.horn_on - reading.wiper_level = self.last_command.wiper_level - self.last_reading = reading - - self.cur_vehicle_state = next_state - self.simulation_time += T - - def pose(self) -> ObjectPose: - x,y,theta,v,phi = self.cur_vehicle_state - return ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN,t=self.simulation_time,x=x,y=y,yaw=theta) - - def state(self) -> VehicleState: - return self.last_reading.to_state(self.pose()) - - def set_pose(self,pose : ObjectPose): - self.cur_vehicle_state[0] = pose.x - self.cur_vehicle_state[1] = pose.y - self.cur_vehicle_state[2] = pose.yaw - - def set_state(self,state : VehicleState): - self.set_pose(state.pose) - self.cur_vehicle_state[3] = state.v - self.cur_vehicle_state[4] = state.front_wheel_angle - - def advance_vehicle_state(self, state : AllState, command : GEMVehicleCommand, T : float) -> AllState: - """Advances the vehicle state by the given amount of time T - under the given command. Agents are not touched. - """ - self.simulation_time = state.t - abs_pose = state.vehicle.to_frame(ObjectFrameEnum.ABSOLUTE_CARTESIAN, state.vehicle.pose, state.start_vehicle_pose) - self.set_state(abs_pose) - self.simulate(T, command) - return replace(state,t=self.simulation_time,vehicle=self.state()) - - - -class GEMDoubleIntegratorSimulationInterface(GEMInterface): - """Standard GEMInterface for a second-order Dubins car model. - The simulator is run in a separate thread so it acts like a real - vehicle interface. - - TODO: agent simulation? - """ - def __init__(self, scene : str = None): +class GEMGazeboInterface(GEMInterface): + """Interface for connecting to the GEM e2 vehicle in Gazebo simulation.""" + + def __init__(self): GEMInterface.__init__(self) - self.simulator = GEMDoubleIntegratorSimulation(scene) - self.real_time_multiplier = settings.get('simulator.real_time_multiplier',1.0) - self.gnss_emulator_settings = settings.get('simulator.gnss_emulator',{}) - self.imu_emulator_settings = settings.get('simulator.imu_emulator',{}) - self.agent_emulator_settings = settings.get('simulator.agent_emulator',{}) - self.gnss_dt = self.gnss_emulator_settings.get('dt',0.1) - self.imu_dt = self.imu_emulator_settings.get('dt',0.05) - self.agent_dt = self.agent_emulator_settings.get('dt',0.1) - # self.last_reading = self.simulator.last_reading - # self.last_command = self.simulator.last_command - self.last_gnss_time = 0 - self.last_imu_time = 0 - self.last_agent_time = 0 - self.gnss_callback = None - self.imu_callback = None - self.agent_detector_callback = None - self.thread_lock = Lock() - self.thread_data = dict() - self.thread = None - - self.max_send_rate = settings.get('vehicle.max_command_rate',10.0) + self.max_send_rate = settings.get('vehicle.max_command_rate', 10.0) self.ros_sensor_topics = settings.get('vehicle.sensors.ros_topics') self.last_command_time = 0.0 self.last_reading = GEMVehicleReading() @@ -316,262 +45,235 @@ def __init__(self, scene : str = None): self.last_reading.steering_wheel_angle = 0.0 self.last_reading.accelerator_pedal_position = 0.0 self.last_reading.brake_pedal_position = 0.0 - self.last_reading.gear = 0 + self.last_reading.gear = 1 self.last_reading.left_turn_signal = False self.last_reading.right_turn_signal = False self.last_reading.horn_on = False self.last_reading.wiper_level = 0 self.last_reading.headlights_on = False + + + - self.speed_sub = rospy.Subscriber("/pacmod/parsed_tx/vehicle_speed_rpt", VehicleSpeedRpt, self.speed_callback) - self.steer_sub = rospy.Subscriber("/pacmod/parsed_tx/steer_rpt", SystemRptFloat, self.steer_callback) - self.global_sub = rospy.Subscriber("/pacmod/parsed_tx/global_rpt", GlobalRpt, self.global_callback) - self.gnss_sub = None + + # IMU data subscriber self.imu_sub = None - self.front_radar_sub = None + self.imu_data = None + + # GNSS data subscriber + self.gnss_sub = None + + # Other sensors self.front_camera_sub = None self.front_depth_sub = None self.top_lidar_sub = None - self.stereo_sub = None + self.front_radar_sub = None + self.faults = [] + # Gazebo vehicle control + self.ackermann_pub = rospy.Publisher( + '/ackermann_cmd', AckermannDrive, queue_size=1) + self.ackermann_cmd = AckermannDrive() + self.last_command = None # Store the last command - # -------------------- PACMod setup -------------------- - # GEM vehicle enable - self.enable_pub = rospy.Publisher('/pacmod/as_rx/enable', Bool, queue_size=1) - self.pacmod_enable = False - - # GEM vehicle gear control, neutral, forward and reverse, publish once - self.gear_pub = rospy.Publisher('/pacmod/as_rx/shift_cmd', PacmodCmd, queue_size=1) - self.gear_cmd = PacmodCmd() - self.gear_cmd.enable = True - self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_NEUTRAL - - # GEM vehicle brake control - self.brake_pub = rospy.Publisher('/pacmod/as_rx/brake_cmd', PacmodCmd, queue_size=1) - self.brake_cmd = PacmodCmd() - self.brake_cmd.enable = False - self.brake_cmd.clear = True - self.brake_cmd.ignore = True - - # GEM vehicle forward motion control - self.accel_pub = rospy.Publisher('/pacmod/as_rx/accel_cmd', PacmodCmd, queue_size=1) - self.accel_cmd = PacmodCmd() - self.accel_cmd.enable = False - self.accel_cmd.clear = True - self.accel_cmd.ignore = True - - # GEM vehicle turn signal control - self.turn_pub = rospy.Publisher('/pacmod/as_rx/turn_cmd', PacmodCmd, queue_size=1) - self.turn_cmd = PacmodCmd() - self.turn_cmd.ui16_cmd = 1 # None - - # GEM vechile steering wheel control - self.steer_pub = rospy.Publisher('/pacmod/as_rx/steer_cmd', PositionWithSpeed, queue_size=1) - self.steer_cmd = PositionWithSpeed() - self.steer_cmd.angular_position = 0.0 # radians, -: clockwise, +: counter-clockwise - self.steer_cmd.angular_velocity_limit = 2.0 # radians/second - + # Subscribe to IMU topic by default + self.imu_sub = rospy.Subscriber("/imu", Imu, self.imu_callback) - # Need to double check on this time - def time(self) -> float: - # return self.simulator.time() - seconds = rospy.get_time() - return seconds - - # Pac mod call backs, need to double check - def speed_callback(self,msg : VehicleSpeedRpt): - self.last_reading.speed = msg.vehicle_speed # forward velocity in m/s + # Subscribe to GNSS Velocitu + self.gnss_vel_sub = rospy.Subscriber("/gps/fix_velocity", Vector3Stamped, self.gnss_vel_callback) + + # Add clock subscription for simulation time + self.sim_time = rospy.Time(0) + self.clock_sub = rospy.Subscriber('/clock', Clock, self.clock_callback) - def steer_callback(self, msg): - self.last_reading.steering_wheel_angle = msg.output - - def global_callback(self, msg): - self.faults = [] - if msg.override_active: - self.faults.append("override_active") - if msg.config_fault_active: - self.faults.append("config_fault_active") - if msg.user_can_timeout: - self.faults.append("user_can_timeout") - if msg.user_can_read_errors: - self.faults.append("user_can_read_errors") - if msg.brake_can_timeout: - self.faults.append("brake_can_timeout") - if msg.steering_can_timeout: - self.faults.append("steering_can_timeout") - if msg.vehicle_can_timeout: - self.faults.append("vehicle_can_timeout") - if msg.subsystem_can_timeout: - self.faults.append("subsystem_can_timeout") - - # Maybe need to change for controls? idk def start(self): - # assert self.thread is None - # print("Running simulator thread...") - # self.thread_data['stop'] = False - # self.thread = Thread(target=self.simulate,args=(self.thread_lock,self.thread_data)) - # self.thread.start() - print("ENABLING PACMOD") - enable_cmd = Bool() - enable_cmd.data = True - self.enable_pub.publish(enable_cmd) - - def stop(self): - print("Stopping simulator thread...") - self.thread_data['stop']= True - self.thread.join() - self.thread = None - print("Done.") - - def hardware_faults(self) -> list: - return [] + print("Starting GEM Gazebo Interface") + + def clock_callback(self, msg): + self.sim_time = msg.clock + + def time(self): + # Return Gazebo simulation time + return self.sim_time.to_sec() + + def imu_callback(self, msg: Imu): + self.imu_data = msg + + def gnss_vel_callback(self, msg): + self.last_reading.speed = np.linalg.norm([msg.vector.x, msg.vector.y] ) + + def get_reading(self) -> GEMVehicleReading: + return self.last_reading - def sensors(self): - #TODO: simulate other sensors? - return ['gnss','imu','agent_detector'] - def subscribe_sensor(self, name, callback, type = None): + def subscribe_sensor(self, name, callback, type=None): if name == 'gnss': - topic = self.ros_sensor_topics[name] - if topic.endswith('fix'): - print("Working") - #GEM e2 Gazebo Simulator - if type is not None and (type is not GNSSReading and type is not NavSatFix): - raise ValueError("Gazebo GEM e2 only supports NavSatFix for GNSS") - if type is NavSatFix: - self.gnss_sub = rospy.Subscriber(topic, NavSatFix, callback) - else: - def callback_with_gnss_reading(NavSatFix_msg: NavSatFix): - # pose = ObjectPose(ObjectFrameEnum.GLOBAL, - # x=NavSatFix_msg.longitude, - # y=NavSatFix_msg.latitude, - # z=NavSatFix_msg.altitude, - # yaw=math.radians(NavSatFix_msg.azimuth), #heading from north in degrees - # roll=math.radians(NavSatFix_msg.roll), - # pitch=math.radians(NavSatFix_msg.pitch), - # ) - # speed = np.sqrt(NavSatFix_msg.east_velocity**2 + NavSatFix_msg.north_velocity**2) - - # unknown roll, pitch, yaw and speed - pose = ObjectPose(ObjectFrameEnum.GLOBAL, - x=NavSatFix_msg.longitude, - y=NavSatFix_msg.latitude, - z=NavSatFix_msg.altitude, - yaw=math.radians(0), #heading from north in degrees - roll=math.radians(0), - pitch=math.radians(0), - t = 0 - ) - speed = 0 - callback(GNSSReading(pose,speed,NavSatFix_msg.status)) - self.gnss_sub = rospy.Subscriber(topic, NavSatFix, callback_with_gnss_reading) - else: - print("not Working") - + topic = self.ros_sensor_topics['gps'] + # Fuse IMU orientation with GNSS position + def gnss_callback_wrapper(gps_msg: NavSatFix): + if self.imu_data is None: + return # Wait for IMU data + + # Get orientation from IMU + quaternion = ( + self.imu_data.orientation.x, + self.imu_data.orientation.y, + self.imu_data.orientation.z, + self.imu_data.orientation.w + ) + print(f"[IMU] Orientation: {quaternion}") + roll, pitch, yaw = euler_from_quaternion(quaternion) + + # Transform yaw to correct frame - Gazebo typically uses ROS standard frame (x-forward) + # while navigation uses x-east reference frame + # Need to convert from Gazebo's frame to navigation heading, then to navigation yaw + + # Assuming Gazebo's yaw is 0 when facing east (ROS REP 103 convention) + # Convert IMU's yaw to heading (CW from North), then to navigation yaw (CCW from East) + # This handles the coordinate frame differences between Gazebo and the navigation frame + # Negate yaw to convert from ROS to heading + heading = transforms.yaw_to_heading(-yaw, degrees=False) + navigation_yaw = transforms.heading_to_yaw( + heading, degrees=False) + + # Create fused pose with transformed yaw + pose = ObjectPose( + frame=ObjectFrameEnum.GLOBAL, + t=gps_msg.header.stamp, + x=gps_msg.longitude, + y=gps_msg.latitude, + z=gps_msg.altitude, + roll=roll, + pitch=pitch, + yaw=navigation_yaw + ) + + # Create GNSS reading with fused data + reading = GNSSReading( + pose=pose, + speed= self.last_reading.speed, + status='FIX' if gps_msg.status.status >= 0 else 'NO_FIX' + ) + # Added debug + print( + f"[GNSS] Raw coordinates: Lat={gps_msg.latitude:.6f}, Lon={gps_msg.longitude:.6f}") + # Added debug + print( + f"[GNSS-FUSED] Orientation: Roll={roll:.2f}, Pitch={pitch:.2f}, Yaw={yaw:.2f} rad") + # Added debug + print(f"[GNSS-FUSED] Speed: {self.last_reading.speed:.2f} m/s") + + callback(reading) + + self.gnss_sub = rospy.Subscriber(topic, NavSatFix, gnss_callback_wrapper) elif name == 'top_lidar': topic = self.ros_sensor_topics[name] if type is not None and (type is not PointCloud2 and type is not np.ndarray): - raise ValueError("GEMHardwareInterface only supports PointCloud2 or numpy array for top lidar") + raise ValueError("GEMGazeboInterface only supports PointCloud2 or numpy array for top lidar") if type is None or type is PointCloud2: self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback) else: - def callback_with_numpy(msg : Image): - #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) + def callback_with_numpy(msg: PointCloud2): points = conversions.ros_PointCloud2_to_numpy(msg, want_rgb=False) callback(points) self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback_with_numpy) - elif name == 'front_radar': - if type is not None and type is not RadarTracks: - raise ValueError("GEMHardwareInterface only supports RadarTracks for front radar") - self.front_radar_sub = rospy.Subscriber("/front_radar/front_radar/radar_tracks", RadarTracks, callback) + + elif name == 'imu': + topic = self.ros_sensor_topics[name] + if type is not None and type is not Imu: + raise ValueError("GEMGazeboInterface only supports Imu for IMU data") + self.imu_sub = rospy.Subscriber(topic, Imu, callback) + elif name == 'front_camera': topic = self.ros_sensor_topics[name] if type is not None and (type is not Image and type is not cv2.Mat): - raise ValueError("GEMHardwareInterface only supports Image or OpenCV for front camera") + raise ValueError("GEMGazeboInterface only supports Image or OpenCV for front camera") if type is None or type is Image: self.front_camera_sub = rospy.Subscriber(topic, Image, callback) else: - def callback_with_cv2(msg : Image): - #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) + def callback_with_cv2(msg: Image): cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8") callback(cv_image) self.front_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2) + # Front depth sensor has not been added to gazebo yet. + # This code is placeholder until we add front depth sensor. elif name == 'front_depth': topic = self.ros_sensor_topics[name] if type is not None and (type is not Image and type is not cv2.Mat): - raise ValueError("GEMHardwareInterface only supports Image or OpenCV for front depth") + raise ValueError("GEMGazeboInterface only supports Image or OpenCV for front depth") if type is None or type is Image: self.front_depth_sub = rospy.Subscriber(topic, Image, callback) else: - def callback_with_cv2(msg : Image): - #print("received image with size",msg.width,msg.height,"encoding",msg.encoding) + def callback_with_cv2(msg: Image): cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough") callback(cv_image) self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2) - - - def send_command(self, command : GEMVehicleCommand): - self.last_command = command - # incorporate controller publisher - - def get_reading(self) -> GEMVehicleReading: - """Returns current read state of the vehicle""" - return self.last_reading - - # PACMod enable callback function - def pacmod_enable_callback(self, msg): - if self.pacmod_enable == False and msg.data == True: - print("PACMod enabled, enabling gear, brake, accel, steer, and turn") - self.send_first_command() - elif self.pacmod_enable == True and msg.data == False: - print("PACMod disabled") - self.pacmod_enable = msg.data - def hardware_faults(self) -> List[str]: - if self.pacmod_enable == False: - return self.faults + ["disengaged"] + # In simulation, we don't have real hardware faults return self.faults - # def simulate(self,lock : Lock, data : dict): - # looper = TimedLooper(self.simulator.dt / self.real_time_multiplier,name="Simulation thread") - # while looper and not data['stop']: - # with lock: - # self.simulator.simulate(self.simulator.dt, self.last_command) - # self.last_reading = self.simulator.last_reading - - # if self.gnss_callback is not None and self.simulator.simulation_time - self.last_gnss_time > self.gnss_dt: - # vehicle_state = self.simulator.state() - # self.gnss_callback(self.gnss_emulator(vehicle_state)) - # self.last_gnss_time = self.simulator.simulation_time - # if self.imu_callback is not None and self.simulator.simulation_time - self.last_imu_time > self.imu_dt: - # pose = ObjectPose(frame=ObjectFrameEnum.CURRENT,t=self.simulation_time,x=0,y=0,yaw=0) - # vehicle_state = self.last_reading.to_state(pose) - # self.imu_callback(vehicle_state) - # self.last_imu_time = self.simulator.simulation_time - # if self.agent_detector_callback is not None and self.simulator.simulation_time - self.last_agent_time > self.agent_dt: - # for k,a in self.simulator.agents.items(): - # self.agent_detector_callback(k,a.to_agent_state()) - # self.last_agent_time = self.simulator.simulation_time - - # def gnss_emulator(self, vehicle_state: VehicleState): - # position_noise = self.gnss_emulator_settings.get('position_noise',0.0) - # orientation_noise = self.gnss_emulator_settings.get('orientation_noise',0.0) - # velocity_noise_const = self.gnss_emulator_settings.get('velocity_noise.constant',0.0) - # velocity_noise_linear = self.gnss_emulator_settings.get('velocity_noise.linear',0.0) - # if position_noise > 0 or orientation_noise > 0 or velocity_noise_const > 0 or velocity_noise_linear > 0: - # # Add noise to the vehicle state - # position = vehicle_state.pose.x,vehicle_state.pose.y - # yaw = vehicle_state.pose.yaw - # v = vehicle_state.v - # position = (np.random.normal(position[0],position_noise), np.random.normal(position[1],position_noise)) - # yaw = np.random.normal(vehicle_state.pose.yaw,orientation_noise) - # v = np.random.normal(v,velocity_noise_const + abs(v)*velocity_noise_linear) - # pose = replace(vehicle_state.pose, x=position[0], y=position[1], yaw=yaw) - # return replace(vehicle_state, pose=pose, v=v) - # return vehicle_state - - - #TODO: agent emulator, imu emulator + def send_command(self, command : GEMVehicleCommand): + # Throttle rate at which we send commands + t = self.time() + if t < self.last_command_time + 1.0/self.max_send_rate: + # Skip command, similar to hardware interface + return + self.last_command_time = t + + # Get current speed + v = self.last_reading.speed + + # Convert pedal to acceleration + accelerator_pedal_position = np.clip(command.accelerator_pedal_position, 0.0, 1.0) + brake_pedal_position = np.clip(command.brake_pedal_position, 0.0, 1.0) + + # Zero out accelerator if brake is active (just like hardware interface) + if brake_pedal_position > 0.0: + accelerator_pedal_position = 0.0 + + # Calculate acceleration from pedal positions + acceleration = pedal_positions_to_acceleration(accelerator_pedal_position, brake_pedal_position, v, 0, 1) + + # Apply reasonable limits to acceleration + max_accel = settings.get('vehicle.limits.max_acceleration', 1.0) + max_decel = settings.get('vehicle.limits.max_deceleration', -2.0) + acceleration = np.clip(acceleration, max_decel, max_accel) + + # Convert wheel angle to steering angle (front wheel angle) + phides = steer2front(command.steering_wheel_angle) + + # Apply steering angle limits + min_wheel_angle = settings.get('vehicle.geometry.min_wheel_angle', -0.6) + max_wheel_angle = settings.get('vehicle.geometry.max_wheel_angle', 0.6) + phides = np.clip(phides, min_wheel_angle, max_wheel_angle) + + # Calculate target speed based on acceleration + # Don't use infinite speed, instead calculate a reasonable target speed + current_speed = v + target_speed = current_speed + + if acceleration > 0: + # Accelerating - set target speed to current speed plus some increment + # This is more realistic than infinite speed + max_speed = settings.get('vehicle.limits.max_speed', 10.0) + target_speed = min(current_speed + acceleration * 0.5, max_speed) + elif acceleration < 0: + # Braking - set target speed to zero if deceleration is significant + if brake_pedal_position > 0.1: + target_speed = 0.0 + + # Create and publish drive message + msg = AckermannDrive() + msg.acceleration = acceleration + msg.speed = target_speed + msg.steering_angle = phides + msg.steering_angle_velocity = command.steering_wheel_speed # Respect steering velocity limit + + # Debug output + print(f"[ACKERMANN] Speed: {msg.speed:.2f}, Accel: {msg.acceleration:.2f}, Steer: {msg.steering_angle:.2f}") + + self.ackermann_pub.publish(msg) + self.last_command = command \ No newline at end of file diff --git a/GEMstack/onboard/perception/object_detection.py b/GEMstack/onboard/perception/object_detection.py index b948ad233..3cc2619af 100644 --- a/GEMstack/onboard/perception/object_detection.py +++ b/GEMstack/onboard/perception/object_detection.py @@ -45,7 +45,7 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: # yolo model self.detector = YOLO(os.getcwd()+'/GEMstack/knowledge/detection/yolov8n.pt') - self.confidence = 0.7 + self.confidence = 0.1 self.classes_to_detect = 0 @@ -61,6 +61,18 @@ def initialize(self): def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: + print(f"VEHICLE State at time: {vehicle.pose.t}") + + print(f"x: {vehicle.pose.x}") + print(f"y: {vehicle.pose.y}") + + print(f"z: {vehicle.pose.z}") + print(f"roll: {vehicle.pose.roll}") + print(f"pitch: {vehicle.pose.pitch}") + print(f"yaw: {vehicle.pose.yaw}") + print(f"speed: {vehicle.v}") + + return {} @@ -73,19 +85,22 @@ def front_camera_callback(self, image: cv2.Mat): track_result = self.detector.track(source=image, persist=True, conf=self.confidence) - + class_names = self.detector.names label_text = "Object " font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.5 font_color = (255, 255, 255) # White text outline_color = (0, 0, 0) # Black outline line_type = 2 - text_thickness = 2 # Text thickness + text_thickness = 1 # Text thickness outline_thickness = 1 # Thickness of the text outline boxes = track_result[0].boxes for box in boxes: + + class_id = int(box.cls.item()) + label_text = class_names[class_id] xywh = box.xywh[0].tolist() x, y, w, h = xywh id = box.id.item() diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index ef9a24851..08392e9ed 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -5,6 +5,8 @@ import matplotlib.animation as animation import time from collections import deque +from ...state.agent import AgentEnum +import numpy as np class MPLVisualization(Component): """Runs a matplotlib visualization at 10Hz. @@ -21,8 +23,12 @@ def __init__(self, rate : float = 10.0, save_as : str = None): self.axs = None self.tstart = 0 self.plot_t_range = 10 - self.plot_values = {} - self.plot_events = {} + + # Separate vehicle and pedestrian tracking + self.vehicle_plot_values = {} + self.pedestrian_plot_values = {} + self.vehicle_plot_events = {} + self.pedestrian_plot_events = {} def rate(self) -> float: return self._rate @@ -41,7 +47,7 @@ def initialize(self): self.writer.setup(plt.gcf(), self.save_as, dpi=100) plt.ion() # to run GUI event loop - self.fig,self.axs = plt.subplots(1,2,figsize=(12,6)) + self.fig,self.axs = plt.subplots(1,3,figsize=(18,6)) self.fig.canvas.mpl_connect('close_event', self.on_close) plt.show(block=False) self.tstart = time.time() @@ -53,30 +59,48 @@ def on_close(self,event): def debug(self, source, item, value): t = time.time() - self.tstart item = source+'.'+item - if item not in self.plot_values: - self.plot_values[item] = deque() - plot = self.plot_values[item] - self.plot_values[item].append((t,value)) + # Determine which plot dict to use based on source + if source.startswith('ped_'): + target_dict = self.pedestrian_plot_values + else: + target_dict = self.vehicle_plot_values + + if item not in target_dict: + target_dict[item] = deque() + plot = target_dict[item] + plot.append((t,value)) while t - plot[0][0] > self.plot_t_range: plot.popleft() def debug_event(self, source, event): t = time.time() - self.tstart event = source+'.'+event - if event not in self.plot_events: - self.plot_events[event] = deque() - plot = self.plot_events[event] + target_dict = self.pedestrian_plot_events if source.startswith('ped_') else self.vehicle_plot_events + + if event not in target_dict: + target_dict[event] = deque() + plot = target_dict[event] plot.append(t) while t - plot[0] > self.plot_t_range: plot.popleft() def update(self, state): if not plt.fignum_exists(self.fig.number): - #plot closed return self.num_updates += 1 - self.debug("vehicle","velocity",state.vehicle.v) - self.debug("vehicle","front wheel angle",state.vehicle.front_wheel_angle) + + # Vehicle metrics + self.debug("vehicle", "velocity", state.vehicle.v) + self.debug("vehicle", "front_wheel_angle", state.vehicle.front_wheel_angle) + + # Pedestrian metrics + for agent_id, agent in state.agents.items(): + if agent.type == AgentEnum.PEDESTRIAN: + # self.debug(f"ped_{agent_id}", "x", agent.pose.x) + # self.debug(f"ped_{agent_id}", "y", agent.pose.y) + self.debug(f"ped_{agent_id}", "velocity", np.linalg.norm(agent.velocity)) # Magnitude of resultant velocity + self.debug(f"ped_{agent_id}", "yaw_rate", agent.yaw_rate) + time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(state.t)) #frame=ObjectFrameEnum.CURRENT #state = state.to_frame(frame) @@ -84,18 +108,27 @@ def update(self, state): yrange = [state.vehicle.pose.y - 10, state.vehicle.pose.y + 10] #plot main visualization mpl_visualization.plot(state,title="Scene %d at %s"%(self.num_updates,time_str),xrange=xrange,yrange=yrange,show=False,ax=self.axs[0]) - #plot figure on axs[1] + + # Vehicle plot (axs[1]) self.axs[1].clear() - for k,v in self.plot_values.items(): + for k,v in self.vehicle_plot_values.items(): t = [x[0] for x in v] y = [x[1] for x in v] self.axs[1].plot(t,y,label=k) - for i,(k,v) in enumerate(self.plot_events.items()): - for t in v: - self.axs[1].axvline(x=t,linestyle='--',color='C'+str(i),label=k) + self.axs[1].set_title('Vehicle Metrics') self.axs[1].set_xlabel('Time (s)') self.axs[1].legend() + # Pedestrian plot (axs[2]) + self.axs[2].clear() + for k,v in self.pedestrian_plot_values.items(): + t = [x[0] for x in v] + y = [x[1] for x in v] + self.axs[2].plot(t,y,label=k) + self.axs[2].set_title('Pedestrian Metrics') + self.axs[2].set_xlabel('Time (s)') + self.axs[2].legend() + self.fig.canvas.draw_idle() self.fig.canvas.flush_events() diff --git a/README.md b/README.md index 51f26e290..b66f78c3c 100644 --- a/README.md +++ b/README.md @@ -11,25 +11,103 @@ GEMstack uses Python 3.7+ and ROS Noetic. (It is possible to do some offline and simulation work without ROS, but it is highly recommended to install it if you are working on any onboard behavior or training for rosbag files.) You should also have the following Python dependencies installed, which you can install from this folder using `pip install -r requirements.txt`: +- GEMstack Dependencies + - numpy + - scipy + - matplotlib + - opencv-python + - torch + - klampt==0.9.2 + - shapely + - dacite + - pyyaml +- Perception Dependencies + - ultralytics -- numpy -- scipy -- matplotlib -- opencv-python -- torch -- klampt -- shapely -- dacite -- pyyaml +In order to interface with the actual GEM e2 vehicle, you will need [PACMOD2](https://github.com/astuff/pacmod2) - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's [sensor message packages](https://github.com/astuff/astuff_sensor_msgs). The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system. +## Running the stack on Ubuntu 20.04 without Docker +### Checking CUDA Version -In order to interface with the actual GEM e2 vehicle, you will need [PACMOD2](https://github.com/astuff/pacmod2) - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's [sensor message packages](https://github.com/astuff/astuff_sensor_msgs). The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system. +Before proceeding, check your Nvidia Driver and supported CUDA version: +```bash +nvidia-smi +``` +This will show your NVIDIA driver version and the maximum supported CUDA version. Make sure you have CUDA 11.8 or 12+ installed. + +From Ubuntu 20.04 install [CUDA 11.6](https://gist.github.com/ksopyla/bf74e8ce2683460d8de6e0dc389fc7f5) or [CUDA 12+](https://gist.github.com/ksopyla/ee744bf013c83e4aa3fc525634d893c9) based on your current Nvidia Driver versio. + +To check the currently installed CUDA version: +```bash +nvcc --version +``` +you can install the dependencies or GEMstack by running `setup/setup_this_machine.sh` from the top-level GEMstack folder. + +## Running the stack on Ubuntu 20.04 or 22.04 with Docker +> [!NOTE] +> Make sure to check the Nvidia Driver and supported CUDA version before proceeding by following the steps in the previous section. -From a fresh Ubuntu 20.04 with ROS Noetic and [CUDA 11.6 installed](https://gist.github.com/ksopyla/bf74e8ce2683460d8de6e0dc389fc7f5), you can install these dependencies by running `setup/setup_this_machine.sh` from the top-level GEMstack folder. +## Prerequisites +- Docker (In Linux - Make sure to follow the post-installation steps from [here](https://docs.docker.com/engine/install/linux-postinstall/)) +- Nvidia Container Toolkit -To build a Docker container with all these prerequisites, you can use the provided Dockerfile by running `docker build -t gem_stack setup/`. For GPU support you will need the NVidia Container Runtime (run `setup/get_nvidia_container.sh` from this directory to install, or see [this tutorial](https://collabnix.com/introducing-new-docker-cli-api-support-for-nvidia-gpus-under-docker-engine-19-03-0-beta-release/) to install) and run `docker run -it --gpus all gem_stack /bin/bash`. +Try running the sample workload from the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html) to check if your system is compatible. +```bash +sudo docker run --rm --runtime=nvidia --gpus all ubuntu nvidia-smi +``` +You should see the nvidia-smi output similar to [this](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html#:~:text=all%20ubuntu%20nvidia%2Dsmi-,Your%20output%20should%20resemble%20the%20following%20output%3A,-%2B%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2B%0A%7C%20NVIDIA%2DSMI%20535.86.10). + +If you see the output, you are good to go. Otherwise, you will need to install the Docker and NVidia Container Toolkit by following the instructions. +- For **Docker**, follow the instructions [here](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository). + +- For **Nvidia Container Toolkit**, run `setup/get_nvidia_container.sh` from this directory to install, or see [this](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html) for more details. + +## Building the Docker image + +To build a Docker image with all these prerequisites, you can use the provided Dockerfile by running. + +```bash +bash setup/build_docker_image.sh +``` + +## Running the Docker container + +To run the container, you can use the provided Docker Compose file by running. +> [!NOTE] +> If you want to open multiple terminals to run the container, you can use the same command. It will automatically start a new terminal inside the same container. +```bash +bash run_docker_container.sh +``` +## Usage Tips and Instructions + +### Using Host Volume + +You can use the host volume under the container's home directory inside the `` folder. This allows you to build and run files that are on the host machine. For example, if you have a file on the host machine at `/home//project`, you can access it inside the container at `/home//host/project`. + +### Using Dev Containers Extension in VSCode + +To have a good developer environment inside the Docker container, you can use the Dev Containers extension in VSCode. Follow these steps: + +1. Install the Dev Containers extension in VSCode. +2. Open the cloned repository in VSCode. +3. Press `ctrl+shift+p`(or select the remote explorer icon from the left bar) and select `Dev-Containers: Attach to Running Container...`. +4. Select the container name `gem_stack-container`. +5. Once attached, Select `File->Open Folder...`. +6. Select the folder/workspace you want to open in the container. + +This will set up the development environment inside the Docker container, allowing you to use VSCode features seamlessly. + +## Stopping the Docker container + +To stop the container, you can use the provided stop script by running. + +```bash +bash stop_docker_container.sh +``` +## Installing for Mac +To install Ubuntu and setup ROS for Mac, follow this [link](https://doc.clickup.com/9011960452/d/h/8cjf6m4-11191/e694fcfb47a015e) for in-depth instructions and troubleshooting. ## In this folder diff --git a/docs/Gazebo Simulation Documentation.md b/docs/Gazebo Simulation Documentation.md new file mode 100644 index 000000000..4e78bf920 --- /dev/null +++ b/docs/Gazebo Simulation Documentation.md @@ -0,0 +1,82 @@ +# Gazebo Simulation Documentation + +## Available Sensors in Gazebo + +The following sensors are currently available in the Gazebo simulation environment: + +- **Front Camera** +- **GNSS** +- **Lidar** + +--- + +## Gazebo Simulation Setup + +To run your code within the Gazebo simulator, you'll first need to set up and install necessary dependencies. + +### 1. Set Up the Simulator + +Go to this [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository to set up the Gazebo Docker container and environment for simulation. + +Follow the instructions in the linked repo to build and run the Docker container. It also provides a simulation environment of highbay. + +### 2. Install Dependencies + +Install the required ROS packages: + +```bash +sudo apt-get install -y ros-noetic-ackermann-msgs +``` + +--- + +## Configuring Your Code for Simulation + +To run your code with the Gazebo simulation, modify your launch file according to the following steps: + +1. **Add a new variant:** + Under the `variants` section, create a new variant called `"gazebo"`. + +2. **Set the vehicle interface:** + In the `"gazebo"` variant, set the `vehicle_interface` to use the Gazebo interface: + ```yaml + vehicle_interface: + type: gem_gazebo.GEMGazeboInterface + ``` + +3. **Add modifications (optional):** + You can add any other configuration or parameters needed for your testing under the `"gazebo"` variant. + +Setting the `vehicle_interface` to `gem_gazebo.GEMGazeboInterface` will automatically link the **GEMStack** sensor topics with the corresponding **Gazebo vehicle sensors**, allowing you to test your code in the simulation environment. + +## Example Launch File + +For a full example, refer to the [`launch/fixed_route.yaml`](launch/fixed_route.yaml) file, which includes a sample `gazebo` variant configuration. + +--- + +## Running Gazebo + +Follow these steps to run your GEMStack code with Gazebo: + +### 1. Launch the Gazebo Simulator + +In one terminal, run the Gazebo simulator (using the instructions provided in the [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository). This will load the simulation environment with the vehicle and sensors. + +### 2. Launch the GEM Stack + +Open a second terminal and launch GEMStack with your configured launch file. + +```bash +python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/{your_file}.yaml +``` +- Make sure to set the variant to `gazebo`. +- You can specify the vehicle type to be `e2_gazebo` or `e4_gazebo`. + +Example command launching the fixed route with e4 vehicle: + +```bash +python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/fixed_route.yaml +``` + +--- diff --git a/docs/Safety and Comfort Metrics Documentation.md b/docs/Safety and Comfort Metrics Documentation.md new file mode 100644 index 000000000..ea0e2f22f --- /dev/null +++ b/docs/Safety and Comfort Metrics Documentation.md @@ -0,0 +1,39 @@ +# Test Safety and Comfort Metrics Documentation + +## Files/Scripts +- `testing/test_comfort_metrics.py` + +## Purpose + +This script analyzes log files and reports vehicle comfort and pedestrian safety with plots. It extracts: + +- **Vehicle Data:** Time, acceleration, heading rate from `behavior.json`. +- **Pedestrian Data:** Time and pedestrian distance to car from `behavior.json`. +- **Pure Pursuit Tracker Data (Optional):** Vehicle time, cross-track error, and position (actual vs. desired) from `PurePursuitTrajectoryTracker_debug.csv`. + +The script will include 5 plots: +- Vehicle jerk vs. time. +- Vehicle heading acceleration vs. time. +- Vehicle cross-track error vs. time. +- Vehicle actual vs. desired position. +- Pedestrian distance vs. time. + +The script also prints key metrics: +- RMS Jerk +- RMS Heading acceleration +- RMS Cross track error +- RMS Position error +- Minimum pedestrian distance to car + +## Usage + +1. **Check log directory:** + - Ensure your log directory contains `behavior.json` (required). + - Optionally include `PurePursuitTrajectoryTracker_debug.csv` (if missing, some plots are skipped). + +2. **Run the script:** + + ```bash + python test_comfort_metrics.py + ``` + Replace `` with the path to the folder containing the log files. \ No newline at end of file diff --git a/launch/README.md b/launch/README.md index d90eabc06..4c9e18fc2 100644 --- a/launch/README.md +++ b/launch/README.md @@ -1 +1,48 @@ Contains launch scripts for use in the standard executor, e.g., `python main.py launch/fixed_route_sim.yaml`. + + +# Gazebo Simulation Guide lines + +## Gazebo Simulation Setup + +Head over [here](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) to setup gazebo docker container to run the simulation + +Install dependency packages + +``` +sudo apt-get install -y ros-noetic-ackermann-msgs + +``` + + + + +## Running GEM Stack with Gazebo + +- Open two terminals +- Launch the Gazebo simulator in the first terminal +- In the other terminal navigate to GEM Stack folder and run the following command to launch GEM Stack + +``` +Sample command: + +python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/gazebo_simulation.yaml + + +Other variants: + +python3 main.py --variant={variant_name} --vehicle={vehicle_name} launch/gazebo_simulation.yaml + +Variants: +sim + +Vehicle types: + +e2 +e4 +e2_gazebo +e4_gazebo + +``` + +- The gazebo_simulation.yaml can be editied to add your planning and perception code, more variants can be added on request. \ No newline at end of file diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index c05de8ff7..40e6b9a73 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -74,6 +74,15 @@ variants: agent_detection : OmniscientAgentDetector visualization: !include "klampt_visualization.yaml" #visualization: !include "mpl_visualization.yaml" + gazebo: + run: + mode: simulation + vehicle_interface: + type: gem_gazebo.GEMGazeboInterface + drive: + perception: + state_estimation: GNSSStateEstimator # Matches your Gazebo GNSS implementation + # visualization: !include "mpl_visualization.yaml" log_ros: log: ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file diff --git a/launch/gazebo_simulation.yaml b/launch/gazebo_simulation.yaml index 06c30dbfa..eb334fb2e 100644 --- a/launch/gazebo_simulation.yaml +++ b/launch/gazebo_simulation.yaml @@ -1,7 +1,7 @@ description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" mode: hardware vehicle_interface: gem_hardware.GEMHardwareInterface -# vehicle: !relative_path ../vehicle/gem_e2_gazebo.yaml +# !include "../GEMstack/knowledge/defaults/computation_graph.yaml" mission_execution: StandardExecutor # Recovery behavior after a component failure recovery: @@ -10,20 +10,21 @@ recovery: type: recovery.StopTrajectoryTracker print: False # Driving behavior for the GEM vehicle following a fixed route + +# Default settings drive: perception: state_estimation : GNSSStateEstimator - agent_detection : object_detection.ObjectDetection perception_normalization : StandardPerceptionNormalizer + planning: - # relations_estimation: pedestrian_yield_logic.PedestrianYielder - # route_planning: - # type: StaticRoutePlanner - # args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start'] - # motion_planning: longitudinal_planning.YieldTrajectoryPlanner - # trajectory_tracking: - # type: pure_pursuit.PurePursuitTrajectoryTracker - # print: False + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start'] + motion_planning: longitudinal_planning.YieldTrajectoryPlanner + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False log: # Specify the top-level folder to save the log files. Default is 'logs' #folder : 'logs' @@ -60,20 +61,34 @@ after: variants: #sim variant doesn't execute on the real vehicle #real variant executes on the real robot - sim: + gazebo: run: mode: simulation vehicle_interface: - type: gem_gazebo.GEMDoubleIntegratorSimulationInterface - args: - scene: !relative_path '../scenes/xyhead_demo.yaml' + type: gem_gazebo.GEMGazeboInterface + # args: + # scene: !relative_path '../scenes/gazebo.yaml' drive: - # perception: - # state_estimation : OmniscientStateEstimator - # agent_detection : OmniscientAgentDetector + perception: + state_estimation : GNSSStateEstimator + agent_detection : object_detection.ObjectDetection + perception_normalization : StandardPerceptionNormalizer + planning: - trajectory_tracking: + # Adding your custom relation estimation + # relations_estimation: pedestrian_yield_logic.PedestrianYielder + # Fixed route with pure pursuit + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # Fixed path + motion_planning: + 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: {desired_speed: 2.5} #approximately 5mph + print: True # visualization: !include "klampt_visualization.yaml" # visualization: !include "mpl_visualization.yaml" log_ros: diff --git a/launch/perception.yaml b/launch/perception.yaml index 623bc682c..3cbeb0ab2 100644 --- a/launch/perception.yaml +++ b/launch/perception.yaml @@ -57,9 +57,11 @@ variants: detector_only: run: description: "Run the pedestrian detection code" - drive: - planning: - trajectory_tracking: + drive: + planning: + trajectory_tracking: + visualization: !include "mpl_visualization.yaml" + real_sim: run: description: "Run the pedestrian detection code with real detection and fake simulation" diff --git a/main.py b/main.py index 64d6afadc..d374afd19 100644 --- a/main.py +++ b/main.py @@ -21,9 +21,9 @@ else: #set the run settings from command line run_config = config.load_config_recursive(launch_file) - #print(run_config) settings.set('run',run_config) if settings.get('run.name',None) is None: + print("yes") settings.set('run.name',launch_file) from GEMstack.onboard.execution import entrypoint diff --git a/requirements.txt b/requirements.txt index 94db8ba43..f8e808d90 100644 --- a/requirements.txt +++ b/requirements.txt @@ -4,6 +4,11 @@ scipy matplotlib torch shapely -klampt +klampt==0.9.2 pyyaml dacite + +# Perception +ultralytics +lap==0.5.12 +open3d \ No newline at end of file diff --git a/run_docker_container.sh b/run_docker_container.sh new file mode 100644 index 000000000..ac0f998b3 --- /dev/null +++ b/run_docker_container.sh @@ -0,0 +1,8 @@ +#!/bin/bash +# Check if container is already running +if [ "$(docker ps -q -f name=gem_stack-container)" ]; then + docker exec -it gem_stack-container bash +else + UID=$(id -u) GID=$(id -g) docker compose -f setup/docker-compose.yaml up -d + docker exec -it gem_stack-container bash +fi \ No newline at end of file diff --git a/setup/Dockerfile b/setup/Dockerfile deleted file mode 100644 index b35c3b85d..000000000 --- a/setup/Dockerfile +++ /dev/null @@ -1,49 +0,0 @@ -FROM nvidia/cuda:11.8.0-cudnn8-devel-ubuntu20.04 -#use bash instead of sh -RUN rm /bin/sh && ln -s /bin/bash /bin/sh -RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd -# Install ROS Noetic -RUN apt-get update && apt-get install -y lsb-release gnupg2 -RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list' -RUN wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc -RUN apt-key add ros.asc -RUN apt-get update -RUN DEBIAN_FRONTEND=noninteractive apt-get install -y ros-noetic-desktop -RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools -RUN rosdep init -RUN rosdep update - -#Install Cuda 11.8 -#RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin -#RUN sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600 -##add public keys -#RUN sudo apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub -#RUN sudo add-apt-repository "deb http://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/ /" -#RUN install cuda-toolkit-11-8 - - -# install Zed SDK -RUN wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run -RUN chmod +x zed_sdk.run -RUN ./zed_sdk.run -- silent - -# create ROS Catkin workspace -RUN mkdir -p /catkin_ws/src - -# install ROS dependencies and packages -RUN cd /catkin_ws/src && git clone https://github.com/krishauser/POLARIS_GEM_e2.git -RUN cd /catkin_ws/src && git clone --recurse-submodules https://github.com/stereolabs/zed-ros-wrapper.git -RUN cd /catkin_ws/src && git clone https://github.com/astuff/pacmod2.git - #for some reason the ibeo messages don't work? -RUN cd /catkin_ws/src && git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs -RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.git \ - && cd radar_msgs && git checkout noetic - -RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y -RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release - -# install GEMstack Python dependencies -RUN git clone https://github.com/krishauser/GEMstack.git -RUN cd GEMstack && pip3 install -r requirements.txt - -RUN echo /catkin_ws/devel/setup.sh diff --git a/setup/Dockerfile.cuda11.8 b/setup/Dockerfile.cuda11.8 new file mode 100644 index 000000000..464b3b29b --- /dev/null +++ b/setup/Dockerfile.cuda11.8 @@ -0,0 +1,82 @@ +FROM nvidia/cuda:11.8.0-cudnn8-devel-ubuntu20.04 + +ARG USER_UID=1000 +ARG USER_GID=1000 +ARG USER=$(whoami) + +ENV DEBIAN_FRONTEND=noninteractive + +#use bash instead of sh +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd + +# Set time zone non-interactively +ENV TZ=America/Chicago +RUN ln -fs /usr/share/zoneinfo/$TZ /etc/localtime \ + && echo $TZ > /etc/timezone \ + && apt-get update && apt-get install -y tzdata \ + && rm -rf /var/lib/apt/lists/* + +RUN wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda11.8_v4.2.4.zstd.run -O zed_sdk.run +RUN chmod +x zed_sdk.run +RUN ./zed_sdk.run -- silent + +# Install ROS Noetic +RUN apt-get update && apt-get install -y lsb-release gnupg2 +RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list' +RUN wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc +RUN apt-key add ros.asc +RUN apt-get update +RUN DEBIAN_FRONTEND=noninteractive apt-get install -y ros-noetic-desktop +RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools +RUN rosdep init +RUN rosdep update + +ARG USER +ARG USER_UID +ARG USER_GID + +# Create user more safely +RUN groupadd -g ${USER_GID} ${USER} || groupmod -n ${USER} $(getent group ${USER_GID} | cut -d: -f1) +RUN useradd -l -m -u ${USER_UID} -g ${USER_GID} ${USER} || usermod -l ${USER} -m -u ${USER_UID} -g ${USER_GID} $(getent passwd ${USER_UID} | cut -d: -f1) +RUN echo "${USER} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +# Fix permissions for Python packages +RUN chown -R ${USER}:${USER} /usr/local/lib/python3.8/dist-packages/ \ + && chmod -R u+rw /usr/local/lib/python3.8/dist-packages/ + +# create ROS Catkin workspace +RUN mkdir -p /catkin_ws/src + +# install ROS dependencies and packages +RUN cd /catkin_ws/src && git clone https://github.com/krishauser/POLARIS_GEM_e2.git +RUN cd /catkin_ws/src && git clone --recurse-submodules https://github.com/stereolabs/zed-ros-wrapper.git +RUN cd /catkin_ws/src && git clone https://github.com/astuff/pacmod2.git + #for some reason the ibeo messages don't work? +RUN cd /catkin_ws/src && git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs +RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.git \ + && cd radar_msgs && git checkout noetic + +RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y +RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release -j1 + +# Copy requirements.txt from host (now relative to parent directory) +COPY requirements.txt /tmp/requirements.txt + +# Install Python dependencies +RUN pip3 install -r /tmp/requirements.txt + +# Install other Dependencies +RUN apt-get install -y ros-noetic-septentrio-gnss-driver + +USER ${USER} + +# Add ROS and GEMstack paths to bashrc +RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USER}/.bashrc +RUN echo "source /catkin_ws/devel/setup.bash" >> /home/${USER}/.bashrc + +# BASE END CONFIG +WORKDIR /home/${USER} + +ENTRYPOINT [ "/bin/bash", "-l" ] diff --git a/setup/Dockerfile.cuda12 b/setup/Dockerfile.cuda12 new file mode 100644 index 000000000..32005202f --- /dev/null +++ b/setup/Dockerfile.cuda12 @@ -0,0 +1,83 @@ +FROM nvidia/cuda:12.0.0-cudnn8-devel-ubuntu20.04 + +ARG USER_UID=1000 +ARG USER_GID=1000 +ARG USER=$(whoami) + +ENV DEBIAN_FRONTEND=noninteractive + +#use bash instead of sh +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd + +# Set time zone non-interactively +ENV TZ=America/Chicago +RUN ln -fs /usr/share/zoneinfo/$TZ /etc/localtime \ + && echo $TZ > /etc/timezone \ + && apt-get update && apt-get install -y tzdata \ + && rm -rf /var/lib/apt/lists/* + +# install Zed SDK. If you want to install a different version, change to this https://download.stereolabs.com/zedsdk/4.2/cu12/ubuntu20 +RUN wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda12.1_v4.2.4.zstd.run -O zed_sdk.run +RUN chmod +x zed_sdk.run +RUN ./zed_sdk.run -- silent + +# Install ROS Noetic +RUN apt-get update && apt-get install -y lsb-release gnupg2 +RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list' +RUN wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc +RUN apt-key add ros.asc +RUN apt-get update +RUN DEBIAN_FRONTEND=noninteractive apt-get install -y ros-noetic-desktop +RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools +RUN rosdep init +RUN rosdep update + +ARG USER +ARG USER_UID +ARG USER_GID + +# Create user more safely +RUN groupadd -g ${USER_GID} ${USER} || groupmod -n ${USER} $(getent group ${USER_GID} | cut -d: -f1) +RUN useradd -l -m -u ${USER_UID} -g ${USER_GID} ${USER} || usermod -l ${USER} -m -u ${USER_UID} -g ${USER_GID} $(getent passwd ${USER_UID} | cut -d: -f1) +RUN echo "${USER} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +# Fix permissions for Python packages +RUN chown -R ${USER}:${USER} /usr/local/lib/python3.8/dist-packages/ \ + && chmod -R u+rw /usr/local/lib/python3.8/dist-packages/ + +# create ROS Catkin workspace +RUN mkdir -p /catkin_ws/src + +# install ROS dependencies and packages +RUN cd /catkin_ws/src && git clone https://github.com/krishauser/POLARIS_GEM_e2.git +RUN cd /catkin_ws/src && git clone --recurse-submodules https://github.com/stereolabs/zed-ros-wrapper.git +RUN cd /catkin_ws/src && git clone https://github.com/astuff/pacmod2.git + #for some reason the ibeo messages don't work? +RUN cd /catkin_ws/src && git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs +RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.git \ + && cd radar_msgs && git checkout noetic + +RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y +RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release + +# Copy requirements.txt from host (now relative to parent directory) +COPY requirements.txt /tmp/requirements.txt + +# Install Python dependencies +RUN pip3 install -r /tmp/requirements.txt + +# Install other Dependencies +RUN apt-get install -y ros-noetic-septentrio-gnss-driver + +USER ${USER} + +# Add ROS and GEMstack paths to bashrc +RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USER}/.bashrc +RUN echo "source /catkin_ws/devel/setup.bash" >> /home/${USER}/.bashrc + +# BASE END CONFIG +WORKDIR /home/${USER} + +ENTRYPOINT ["/bin/bash"] \ No newline at end of file diff --git a/setup/build_docker_image.sh b/setup/build_docker_image.sh new file mode 100644 index 000000000..0852f27ab --- /dev/null +++ b/setup/build_docker_image.sh @@ -0,0 +1,22 @@ +#!/bin/bash + +echo "Select CUDA version:" +echo "1) CUDA 11.8" +echo "2) CUDA 12+" +read -p "Enter choice [1-2]: " choice + +case $choice in + 1) + DOCKERFILE=setup/Dockerfile.cuda11.8 + ;; + 2) + DOCKERFILE=setup/Dockerfile.cuda12 + ;; + *) + echo "Invalid choice" + exit 1 + ;; +esac + +export DOCKERFILE +UID=$(id -u) GID=$(id -g) docker compose -f setup/docker-compose.yaml build \ No newline at end of file diff --git a/setup/docker-compose.yaml b/setup/docker-compose.yaml new file mode 100644 index 000000000..f0c7703b8 --- /dev/null +++ b/setup/docker-compose.yaml @@ -0,0 +1,41 @@ +version: '3.9' + +services: + gem-stack-ubuntu-20.04-CUDA: + image: gem_stack + container_name: gem_stack-container + build: + context: .. + dockerfile: ${DOCKERFILE:-setup/Dockerfile.cuda11.8} # Default to cuda11.8 if not specified + args: + USER: ${USER} + USER_UID: ${UID:-1000} # Pass host UID + USER_GID: ${GID:-1000} # Pass host GID + stdin_open: true + tty: true + volumes: + # - "/etc/group:/etc/group:ro" + # - "/etc/passwd:/etc/passwd:ro" + # - "/etc/shadow:/etc/shadow:ro" + # - "/etc/sudoers.d:/etc/sudoers.d:ro" + - "~:/home/${USER}/host" + - "/tmp/.X11-unix:/tmp/.X11-unix:rw" + environment: + - DISPLAY=${DISPLAY} + - XDG_RUNTIME_DIR=/tmp/runtime-${USER} + - DBUS_SYSTEM_BUS_ADDRESS=unix:path=/var/run/dbus/system_bus_socket + - DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/${UID}/bus + - NVIDIA_DRIVER_CAPABILITIES=all + - NVIDIA_VISIBLE_DEVICES=all + # - LIBGL_ALWAYS_SOFTWARE=1 # Uncomment if you want to use software rendering (No GPU) + network_mode: host + ipc: host + user: "${USER}:${USER}" + # Un-Comment the following lines if you want to use Nvidia GPU + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all # alternatively, use `count: all` for all GPUs + capabilities: [gpu] diff --git a/setup/get_nvidia_container.sh b/setup/get_nvidia_container.sh index 466b1989e..4f738e2c8 100755 --- a/setup/get_nvidia_container.sh +++ b/setup/get_nvidia_container.sh @@ -1,9 +1,14 @@ #!/bin/bash -curl -s -L https://nvidia.github.io/nvidia-container-runtime/gpgkey | \ - sudo apt-key add - -distribution=$(. /etc/os-release;echo $ID$VERSION_ID) -curl -s -L https://nvidia.github.io/nvidia-container-runtime/$distribution/nvidia-container-runtime.list | \ - sudo tee /etc/apt/sources.list.d/nvidia-container-runtime.list +curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \ + && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \ + sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \ + sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list + +sed -i -e '/experimental/ s/^#//g' /etc/apt/sources.list.d/nvidia-container-toolkit.list + sudo apt-get update -sudo apt-get install nvidia-container-runtime \ No newline at end of file +sudo apt-get install -y nvidia-container-toolkit + +sudo nvidia-ctk runtime configure --runtime=docker +sudo systemctl restart docker \ No newline at end of file diff --git a/setup/setup_this_machine.sh b/setup/setup_this_machine.sh index e30b3aa23..36690f1d4 100755 --- a/setup/setup_this_machine.sh +++ b/setup/setup_this_machine.sh @@ -1,30 +1,88 @@ #!/bin/bash sudo apt update -sudo apt-get install git python3 python3-pip wget zstd +sudo apt-get install -y git python3 python3-pip wget zstd + +if [ ! -f /opt/ros/noetic/setup.bash ]; then + echo "ROS Noetic not found. Installing ROS Noetic..." + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc + sudo apt-key add ros.asc + sudo apt update + sudo DEBIAN_FRONTEND=noninteractive apt install -y ros-noetic-desktop + sudo apt install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools + sudo rosdep init + rosdep update +fi source /opt/ros/noetic/setup.bash #install Zed SDK -wget https://download.stereolabs.com/zedsdk/4.0/cu121/ubuntu20 -O ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -chmod +x ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -./ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -- silent +echo "To install the ZED SDK, select the CUDA version:" +echo "1) CUDA 11.8" +echo "2) CUDA 12+" +echo "3) No GPU (Skip ZED SDK installation)" +read -p "Enter choice [1-3]: " choice + +case $choice in + 1) + wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run + chmod +x zed_sdk.run + ./zed_sdk.run -- silent + ;; + 2) + wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda12.1_v4.2.4.zstd.run -O zed_sdk.run + chmod +x zed_sdk.run + ./zed_sdk.run -- silent + ;; + 3) + echo "Skipping ZED SDK installation..." + ;; + *) + echo "Invalid choice" + exit 1 + ;; +esac #create ROS Catkin workspace -mkdir catkin_ws -mkdir catkin_ws/src +mkdir -p ~/catkin_ws/src + +# Store current working directory +CURRENT_DIR=$(pwd) +echo "CURRENT_DIR: $CURRENT_DIR" #install ROS dependencies and packages -cd catkin_ws/src +cd ~/catkin_ws/src git clone https://github.com/krishauser/POLARIS_GEM_e2.git git clone https://github.com/astuff/pacmod2.git -git clone https://github.com/astuff/astuff_sensor_msgs.git -git clone https://github.com/ros-perception/radar_msgs.git -cd radar_msgs; git checkout noetic; cd .. +git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs +git clone https://github.com/ros-perception/radar_msgs.git && cd radar_msgs; git checkout noetic; cd .. cd .. #back to catkin_ws +rosdep update rosdep install --from-paths src --ignore-src -r -y catkin_make -DCMAKE_BUILD_TYPE=Release source devel/setup.bash -cd .. #back to GEMstack +cd $CURRENT_DIR #install GEMstack Python dependencies -python3 -m pip install -r requirements.txt \ No newline at end of file + +# Ask the user if they want to install ultralytics +read -p "Do you want to install ultralytics? (y/n): " install_ultralytics + +# Create a temporary requirements file +temp_requirements="temp_requirements.txt" + +# Copy all lines except ultralytics if the user chooses to skip it +if [ "$install_ultralytics" == "y" ]; then + cp requirements.txt $temp_requirements +else + grep -v "ultralytics" requirements.txt > $temp_requirements +fi + +# Install the packages from the temporary requirements file +pip3 install -r $temp_requirements + +# Clean up the temporary file +rm $temp_requirements + +#install other dependencies +sudo apt-get install -y ros-noetic-septentrio-gnss-driver \ No newline at end of file diff --git a/stop_docker_container.sh b/stop_docker_container.sh new file mode 100644 index 000000000..1b1bad2c8 --- /dev/null +++ b/stop_docker_container.sh @@ -0,0 +1,3 @@ +#! /bin/bash + +docker compose -f setup/docker-compose.yaml down \ No newline at end of file diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py new file mode 100644 index 000000000..78370b88f --- /dev/null +++ b/testing/test_comfort_metrics.py @@ -0,0 +1,377 @@ +import sys +import os +sys.path.append(os.getcwd()) + +from GEMstack.state import AgentEnum +import json +import matplotlib.pyplot as plt +import numpy as np +from matplotlib.collections import LineCollection +from matplotlib.colors import Normalize +from matplotlib import cm +import os + +CMAP = "RdYlGn" + +def compute_safety_factor(value, safe_thresh, unsafe_thresh, flip=False): + """ + Computes a safety factor between 0(unsafe) and 1(safe) + If flip is True, the threshold bounds are reversed. + """ + abs_val = abs(value) + if abs_val <= safe_thresh: + factor = 1.0 + elif abs_val >= unsafe_thresh: + factor = 0.0 + else: + factor = 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) + + if flip: + return 1.0 - factor + return factor + +def parse_behavior_log(filename): + """ + Parses the behavior log file and extracts the following data: + - vehicle time + - vehicle acceleration + - vehicle heading rate + - pedestrian time + - pedestrian distance + """ + times = [] + accelerations = [] + heading_rates = [] + pedestrian_times = [] + pedestrian_distances = [] + + with open(filename, 'r') as f: + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + # Process vehicle state data + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + # Only add if all fields are available + if None not in (t, acceleration, heading_rate): + times.append(t) + accelerations.append(acceleration) + heading_rates.append(heading_rate) + # Process agent state data + if "agents" in entry: + for agent in entry["agents"].values(): + agent_data = agent.get("data", {}) + # Check if the agent is a pedestrian + if agent_data.get("type") == AgentEnum.PEDESTRIAN.value: + t = entry.get("time") + pose = agent_data.get("pose", {}) + x_agent = pose.get("x") + y_agent = pose.get("y") + if None not in (t, x_agent, y_agent): + pedestrian_times.append(t) + dist = np.sqrt(x_agent**2 + y_agent**2) + pedestrian_distances.append(dist) + + return (np.array(times), np.array(accelerations), np.array(heading_rates), + np.array(pedestrian_times), np.array(pedestrian_distances)) + +def parse_tracker_csv(filename): + """ + Parses the pure pursuit tracker log file and extracts the following data: + - vehicle time (from column index 18) + - Crosstrack error (from column index 20) + - X position actual (from column index 2) + - Y position actual (from column index 5) + - X position desired (from column index 11) + - Y position desired (from column index 14) + """ + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + vehicle_time = data[:, 18] + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 11], data[:, 14] + return vehicle_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + """ + Computes the derivative of array with respect to time. + Returns the time array and derivative values. + """ + dt = np.diff(times) + dv = np.diff(values) + + # Avoid division by zero or very small values + mask = np.abs(dt) > 1e-10 + derivative = np.zeros_like(dt) + derivative[mask] = dv[mask] / dt[mask] + + return times[1:], derivative + +def add_safety_colorbar(figure): + """Adds a colorbar to the right of the figure""" + cbar_ax = figure.add_axes([0.92, 0.2, 0.02, 0.6]) + sm = cm.ScalarMappable(cmap=CMAP) + cbar = figure.colorbar(sm, cax=cbar_ax) + cbar.set_label("Comfort/Safety Level") + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, time_accel, accel, vehicle_time, cte, + x_actual, y_actual, x_desired, y_desired, pedestrian_times, pedestrian_distances): + """Plots all metrics in 2x3 grid""" + fig, axs = plt.subplots(2, 3, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.375, wspace=0.35) + # axs[1,2].axis('off') + + plot_jerk(axs[0,0], time_jerk, jerk) + plot_heading_acceleration(axs[0,1], time_heading_acc, heading_acc) + plot_acceleration(axs[0,2], time_accel, accel) + plot_crosstrack_error(axs[1,0], vehicle_time, cte) + plot_position(axs[1,1], x_actual, y_actual, x_desired, y_desired) + plot_pedestrian_dist(axs[1,2], pedestrian_times, pedestrian_distances) + + # Colorbar on the right side + add_safety_colorbar(fig) + plt.show() + +def plot_jerk(axis, time, jerk, safe_thresh=1.0, unsafe_thresh=2.5): + """ + Plots vehicle jerk (rate of acceleration) vs. time as a colored line. + """ + # compute safety scores + safety = np.vectorize(compute_safety_factor)(jerk, safe_thresh, unsafe_thresh) + + # build line segments + points = np.vstack([time, jerk]).T.reshape(-1,1,2) + segments = np.concatenate([points[:-1], points[1:]], axis=1) + + # create colored LineCollection + norm = Normalize(vmin=0, vmax=1) + lc = LineCollection(segments, cmap=CMAP, norm=norm, linewidth=1.5) + lc.set_array(safety[:-1]) + axis.add_collection(lc) + + # Set axis limits safely, avoiding NaN/Inf values + if len(jerk) > 0: + valid_jerk = jerk[~np.isnan(jerk) & ~np.isinf(jerk)] + if len(valid_jerk) > 0: + ymin, ymax = valid_jerk.min(), valid_jerk.max() + # Add small margin if min equals max + if ymin == ymax: + ymin -= 0.1 + ymax += 0.1 + axis.set_ylim(ymin, ymax) + + axis.set_xlim(time.min(), time.max()) + axis.set_xlabel("Time (s)") + axis.set_ylabel("Jerk (m/s³)") + axis.set_title("Vehicle Jerk Over Time") + axis.grid(True) + +def plot_acceleration(axis, time, acceleration, safe_thresh=0.5, unsafe_thresh=1.5): + """ + Plots vehicle acceleration vs. time as a colored line. + """ + safety = np.vectorize(compute_safety_factor)(acceleration, safe_thresh, unsafe_thresh) + + points = np.vstack([time, acceleration]).T.reshape(-1,1,2) + segments = np.concatenate([points[:-1], points[1:]], axis=1) + + norm = Normalize(vmin=0, vmax=1) + lc = LineCollection(segments, cmap=CMAP, norm=norm, linewidth=1.5) + lc.set_array(safety[:-1]) + axis.add_collection(lc) + + # Set axis limits safely + if len(acceleration) > 0: + valid_accel = acceleration[~np.isnan(acceleration) & ~np.isinf(acceleration)] + if len(valid_accel) > 0: + ymin, ymax = valid_accel.min(), valid_accel.max() + if ymin == ymax: + ymin -= 0.1 + ymax += 0.1 + axis.set_ylim(ymin, ymax) + + axis.set_xlim(time.min(), time.max()) + axis.set_xlabel("Time (s)") + axis.set_ylabel("Acceleration (m/s²)") + axis.set_title("Vehicle Acceleration Over Time") + axis.grid(True) + +def plot_heading_acceleration(axis, time, heading_acc, safe_thresh=0.0075, unsafe_thresh=0.25): + """ + Plots vehicle heading acceleration vs. time as a colored line. + """ + safety = np.vectorize(compute_safety_factor)(heading_acc, safe_thresh, unsafe_thresh) + + points = np.vstack([time, heading_acc]).T.reshape(-1,1,2) + segments = np.concatenate([points[:-1], points[1:]], axis=1) + + norm = Normalize(vmin=0, vmax=1) + lc = LineCollection(segments, cmap=CMAP, norm=norm, linewidth=1.5) + lc.set_array(safety[:-1]) + axis.add_collection(lc) + + # Set axis limits safely + if len(heading_acc) > 0: + valid_heading = heading_acc[~np.isnan(heading_acc) & ~np.isinf(heading_acc)] + if len(valid_heading) > 0: + ymin, ymax = valid_heading.min(), valid_heading.max() + if ymin == ymax: + ymin -= 0.1 + ymax += 0.1 + axis.set_ylim(ymin, ymax) + + axis.set_xlim(time.min(), time.max()) + axis.set_xlabel("Time (s)") + axis.set_ylabel("Heading Acceleration (rad/s²)") + axis.set_title("Vehicle Heading Acceleration Over Time") + axis.grid(True) + +def plot_crosstrack_error(axis, time, cte, safe_thresh=0.1, unsafe_thresh=0.4): + """ + Plots vehicle cross track error vs. time as a colored line. + """ + # compute safety scores for each point + safety = np.vectorize(compute_safety_factor)(cte, safe_thresh, unsafe_thresh) + + points = np.vstack([time, cte]).T.reshape(-1,1,2) + segments = np.concatenate([points[:-1], points[1:]], axis=1) + + lc = LineCollection(segments, cmap=CMAP, norm=Normalize(0,1)) + lc.set_array(safety[:-1]) + lc.set_linewidth(2.0) + axis.add_collection(lc) + + # Set axis limits safely + if len(cte) > 0: + valid_cte = cte[~np.isnan(cte) & ~np.isinf(cte)] + if len(valid_cte) > 0: + ymin, ymax = valid_cte.min(), valid_cte.max() + if ymin == ymax: + ymin -= 0.1 + ymax += 0.1 + axis.set_ylim(ymin, ymax) + + axis.set_xlim(time.min(), time.max()) + axis.set_xlabel("Time (s)") + axis.set_ylabel("Cross Track Error") + axis.set_title("Vehicle Cross Track Error Over Time") + axis.grid(True) + +def plot_position(axis, x_actual, y_actual, x_desired, y_desired, + safe_thresh=1.0, unsafe_thresh=2.5): + """ + Plots vehicle actual and desired positions + """ + # compute positional error and safety at each point + pos_error = np.sqrt((x_desired - x_actual)**2 + (y_desired - y_actual)**2) + safety = np.vectorize(compute_safety_factor)(pos_error, safe_thresh, unsafe_thresh) + + # actual path segments + actual_pts = np.vstack([y_actual, x_actual]).T.reshape(-1,1,2) + actual_segs = np.concatenate([actual_pts[:-1], actual_pts[1:]], axis=1) + norm = Normalize(vmin=0, vmax=1) + lc_actual = LineCollection(actual_segs, cmap=CMAP, norm=norm) + lc_actual.set_array(safety[:-1]) + lc_actual.set_linewidth(2.0) + axis.add_collection(lc_actual) + + # desired path as dashed gray line + axis.plot(y_desired, x_desired, + linestyle='--', linewidth=1.5, color='gray', label='Desired') + + axis.set_xlabel("Y Position (m)") + axis.set_ylabel("X Position (m)") + axis.set_title("Vehicle Position vs. Desired Position") + axis.legend() + axis.grid(True) + +def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thresh=5.0, unsafe_thresh=2.0): + """Plots pedestrian distance to vehicle vs. time""" + if len(pedestrian_times) > 0: + safety_scores = np.vectorize(compute_safety_factor)(pedestrian_distances, safe_thresh, unsafe_thresh, flip=True) + axis.plot(pedestrian_times, pedestrian_distances, color="black", linewidth=0.8, alpha=0.5) + axis.scatter(pedestrian_times, pedestrian_distances, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + + axis.set_xlabel("Time (s)") + axis.set_ylabel("Pedestrian Distance (m)") + axis.set_title("Pedestrian Distance Over Time") + axis.grid(True) + +if __name__=='__main__': + if len(sys.argv) == 2: + log_dir = "logs/" + sys.argv[1] + else: + + logs_root = "logs/" + # Get all subdirectories inside "logs/" + subdirs = [os.path.join(logs_root, d) for d in os.listdir(logs_root) if os.path.isdir(os.path.join(logs_root, d))] + + # Find the latest directory based on modification time + log_dir = max(subdirs, key=os.path.getmtime) + + print(f"Using latest log directory: {log_dir}") + behavior_file = os.path.join(log_dir, "behavior.json") + tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + + # if behavior.json doesn't exist, print error and exit + if not os.path.exists(behavior_file): + print("Error: behavior.json file is missing in log folder.") + sys.exit(1) + + # Parse behavior log file and compute metrics + times, accelerations, heading_rates, ped_times, ped_distances = parse_behavior_log(behavior_file) + + # Ensure we have valid data before computing derivatives + if len(times) > 1 and len(accelerations) > 1 and len(heading_rates) > 1: + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + else: + print("Warning: Not enough data points to compute derivatives.") + time_jerk, jerk = np.array([]), np.array([]) + time_heading_acc, heading_acc = np.array([]), np.array([]) + + # Pure pursuit tracker file exists: parse and plot all metrics + if os.path.exists(tracker_file): + vehicle_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, times, accelerations, vehicle_time, cte, + x_actual, y_actual, x_desired, y_desired, ped_times, ped_distances) + + # Calculate RMS values only for valid data points + valid_cte = cte[~np.isnan(cte) & ~np.isinf(cte)] + if len(valid_cte) > 0: + print("RMS Cross Track Error:", np.sqrt(np.mean(valid_cte**2)), "m") + + valid_pos_error = np.sqrt((x_actual - x_desired)**2 + (y_actual - y_desired)**2) + valid_pos_error = valid_pos_error[~np.isnan(valid_pos_error) & ~np.isinf(valid_pos_error)] + if len(valid_pos_error) > 0: + print("RMS Position Error:", np.sqrt(np.mean(valid_pos_error**2)), 'm') + # Pure pursuit tracker file is missing: plot only behavior.json metrics + else: + print("Tracker file is missing. Skipping cross track error and vehicle position plots.") + # Plot only jerk, heading acceleration, and pedestrian distance + fig, axs = plt.subplots(2, 2, figsize=(12, 4)) + plot_jerk(axs[0, 0], time_jerk, jerk) + plot_heading_acceleration(axs[0, 1], time_heading_acc, heading_acc) + plot_acceleration(axs[1, 0], times, accelerations) + plot_pedestrian_dist(axs[1, 1], ped_times, ped_distances) + + add_safety_colorbar(fig) + plt.show() + + # Calculate RMS values only for valid data points + valid_jerk = jerk[~np.isnan(jerk) & ~np.isinf(jerk)] + if len(valid_jerk) > 0: + print("RMS Jerk:", np.sqrt(np.mean(valid_jerk**2)), "m/s³") + + valid_heading_acc = heading_acc[~np.isnan(heading_acc) & ~np.isinf(heading_acc)] + if len(valid_heading_acc) > 0: + print("RMS Heading Acceleration:", np.sqrt(np.mean(valid_heading_acc**2)), "rad/s²") + + if len(ped_distances) > 0: + print("Minimum Pedestrian Distance:", np.min(ped_distances), "m")