class WAChronoSensorManager(WASensorManager):
"""Derived SensorManager class that essentially wraps a ChSensorManager. Used to maintain sensors.
Args:
system (WAChronoSystem): The system for the simulation
vehicle (WAVehicle): The vehicle each sensor is attached to
filename (str): A json file to load a scene from. Defaults to None (does nothing).
"""
# Global filenames for environment models
_EGP_SENSOR_SCENE_FILE = "sensors/scenes/ev_grand_prix.json"
EGP_SENSOR_SCENE_FILE = _WAStaticAttribute('_EGP_SENSOR_SCENE_FILE',
chrono.GetChronoDataFile)
def __init__(self, system: 'WAChronoSystem', filename: str = None):
if missing_chrono_sensor:
sens_import_error('WAChronoSensorManager.__init__')
_check_type(system, WAChronoSystem, 'system',
'WAChronoSensorManager.__init__') # noqa
super().__init__(system)
self._manager = sens.ChSensorManager(system._system)
if filename is not None:
load_chrono_sensor_scene_from_json(self, filename)
def add_sensor(self, sensor: "WAChronoSensor"):
"""Add a sensor to the sensor manager"""
if isinstance(sensor, WAChronoSensor):
self._manager.AddSensor(sensor._sensor)
elif isinstance(sensor, WASensor):
super().add_sensor(sensor)
else:
raise TypeError(
f"WAChronoSensorManager.add_sensor: sensor has unknown type '{type(sensor)}'. Must be 'WAChronoSensor' or 'WASensor."
)
def advance(self, step):
"""Advance the state of the sensor by the specified time step
Args:
step(float): the step to update the sensor by
"""
self._manager.Update()
super().advance(step)
class WASensorManager(WABase):
"""Base class that manages sensors.
Args:
system (WASystem): The system for the simulation
filename (str, optional): A json file to load a scene from. Defaults to None (does nothing).
"""
# Global filenames for sensor suites
_EGP_SENSOR_SUITE_FILE = "sensors/suites/ev_grand_prix.json"
EGP_SENSOR_SUITE_FILE = _WAStaticAttribute('_EGP_SENSOR_SUITE_FILE',
get_wa_data_file)
def __init__(self, system: 'WASystem', filename: str = None):
self._system = system
self._sensors = []
if filename is not None:
load_sensor_suite_from_json(filename, self)
def add_sensor(self, sensor: "WASensor"):
"""Add a sensor to the sensor manager"""
self._sensors.append(sensor)
def synchronize(self, time):
"""Synchronize each sensor at the specified time
Args:
time (float): the time at which the sensors are synchronized to
"""
for sensor in self._sensors:
sensor.synchronize(time)
def advance(self, step):
"""Advance the state of each sensor by the specified time step
Args:
step (float): the step to update the sensor by
"""
for sensor in self._sensors:
sensor.advance(step)
def is_ok(self) -> bool:
for sensor in self._sensors:
if not sensor.is_ok():
return False
return True
class WASimpleEnvironment(WAEnvironment):
"""Simple environment.
Args:
system (WASystem): The system used to handle simulation meta data
filename (str): The json specification file used to generate the terrain
"""
_EGP_ENV_MODEL_FILE = "environments/ev_grand_prix.json"
EGP_ENV_MODEL_FILE = _WAStaticAttribute('_EGP_ENV_MODEL_FILE',
get_wa_data_file)
def __init__(self, system: 'WASystem', filename: str):
super().__init__()
self._system = system
load_environment_from_json(self, filename)
def synchronize(self, time: float):
"""Synchronize the environment with the rest of the world at the specified time
Simple environment doesn't actually do anything for now.
Args:
time(float): the time at which the enviornment should be synchronized to
"""
pass
def advance(self, step: float):
"""Advance the state of the environment
Simple environment doesn't actually do anything for now.
Args:
step(float): the time step at which the enviornment should be advanced
"""
pass
class WAChronoVehicle(WAVehicle):
"""Chrono vehicle wrapper
Args:
system (WAChronoSystem): the system used to run the simulation
env (WAEnvironment): the environment with a terrain
vehicle_inputs (WAVehicleInputs): the vehicle inputs
filename (str): json file specification file
init_loc (WAVector, optional): the inital location of the vehicle. Defaults to WAVector([0, 0, 0.5]).
init_rot (WAQuaternion, optional): the initial orientation of the vehicle. Defaults to WAQuaternion([1, 0, 0, 0]).
init_speed (WAVector, optional): the initial forward speed of the vehicle. Defaults to 0.0.
"""
# Global filenames for vehicle models
_GO_KART_MODEL_FILE = "GoKart/GoKart.json"
GO_KART_MODEL_FILE = _WAStaticAttribute('_GO_KART_MODEL_FILE',
get_chrono_vehicle_data_file)
def __init__(self,
system: 'WAChronoSystem',
vehicle_inputs: 'WAVehicleInputs',
env: 'WAEnvironment',
filename: str,
init_loc: WAVector = WAVector([0, 0, 0.5]),
init_rot: WAQuaternion = WAQuaternion([1, 0, 0, 0]),
init_speed: float = 0.0):
super().__init__(
system, vehicle_inputs,
get_wa_data_file("vehicles/GoKart/GoKart_KinematicBicycle.json"))
# Get the filenames
vehicle_file, powertrain_file, tire_file = read_vehicle_model_file(
filename)
# Create the vehicle
vehicle = veh.WheeledVehicle(system._system, vehicle_file)
# Initialize the vehicle
init_loc = WAVector_to_ChVector(init_loc)
init_rot = WAQuaternion_to_ChQuaternion(init_rot)
vehicle.Initialize(chrono.ChCoordsysD(init_loc, init_rot), init_speed)
# Set the visualization components for the vehicle
vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
vehicle.SetSuspensionVisualizationType(veh.VisualizationType_NONE)
vehicle.SetSteeringVisualizationType(veh.VisualizationType_NONE)
vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
# Create the powertrain
# Assumes a SimplePowertrain
powertrain = veh.SimplePowertrain(powertrain_file)
vehicle.InitializePowertrain(powertrain)
# Create and initialize the tires
for axle in vehicle.GetAxles():
tireL = create_tire_from_json(tire_file)
vehicle.InitializeTire(tireL, axle.m_wheels[0],
veh.VisualizationType_MESH)
tireR = create_tire_from_json(tire_file)
vehicle.InitializeTire(tireR, axle.m_wheels[1],
veh.VisualizationType_MESH)
self._vehicle = vehicle
self._terrain = env._terrain
def synchronize(self, time: float):
d = veh.Inputs()
d.m_steering = self._vehicle_inputs.steering
d.m_throttle = self._vehicle_inputs.throttle
d.m_braking = self._vehicle_inputs.braking
self._vehicle.Synchronize(time, d, self._terrain)
def advance(self, step: float):
self._vehicle.Advance(step)
def get_tire_radius(self, axle: str) -> float:
axle = axle.lower()
assert axle == 'front' or axle == 'rear'
axle = 0 if axle == 'front' else 1
return self._vehicle.GetTire(axle, 0).GetRadius()
def get_pos(self) -> WAVector:
return ChVector_to_WAVector(self._vehicle.GetVehiclePos())
def get_rot(self) -> WAQuaternion:
return ChQuaternion_to_WAQuaternion(self._vehicle.GetVehicleRot())
def get_pos_dt(self) -> WAVector:
vel = self._vehicle.GetChassisBody().PointSpeedLocalToParent(
self._vehicle.GetChassis().GetLocalDriverCoordsys().pos)
vel = self._vehicle.GetChassisBody().GetRot().Rotate(vel)
return ChVector_to_WAVector(vel)
def get_rot_dt(self) -> WAQuaternion:
return ChVector_to_WAVector(
self._vehicle.GetChassisBody().GetWvel_loc())
def get_pos_dtdt(self) -> WAVector:
acc = self._vehicle.GetChassisBody().PointAccelerationLocalToParent(
self._vehicle.GetChassis().GetLocalDriverCoordsys().pos)
acc = self._vehicle.GetChassisBody().GetRot().Rotate(acc) - \
self._vehicle.GetSystem().Get_G_acc()
return ChVector_to_WAVector(acc)
def get_rot_dtdt(self) -> WAQuaternion:
return ChVector_to_WAVector(
self._vehicle.GetChassisBody().GetWacc_loc())
class WAWheelEncoderSensor(WASensor):
"""Derived sensor class that implements an wheel encoder model
Wheel encoders are common sensors used by robots to measure the rotational speed of rotational objects. For the application
of autonomous vehicles, wheel encoders can be placed on the front or rear axles to measure the angular rotation speed of the wheels.
Further, a wheel encoder can be placed on the steering column, to again, measure the angular speed. With the angular speed, one can
simply integrate to find the angular position (i.e. steering angle).
Args:
system (WASystem): The system for the simulation
filename (str): a json specification file that describes a wheel encoder sensor model
vehicle (WAVehicle, optional): The vehicle to attach to. If not passed, body must be passed.
body (WABody, optional): The body to attach to. If not passed, vehicle must be passed.
"""
# Global filenames for gps sensors
_WHEEL_ENCODER_SENSOR_FILE = "sensors/models/wheel_encoder.json"
WHEEL_ENCODER_SENSOR_FILE = _WAStaticAttribute(
'_WHEEL_ENCODER_SENSOR_FILE', get_wa_data_file)
def __init__(self,
system: 'WASystem',
filename: str,
vehicle: 'WAVehicle' = None,
body: 'WABody' = None):
super().__init__(vehicle, body)
if body is not None:
raise NotImplementedError(
"Setting 'body' is currently not supported. Please pass a vehicle instead"
)
self._vehicle = vehicle
j = _load_json(filename)
# Validate the json file
_check_field(j, 'Type', value='Sensor')
_check_field(j, 'Template', value='Wheel Encoder')
_check_field(j, 'Properties', field_type=dict)
p = j['Properties']
_check_field(p, 'Update Rate', field_type=int)
_check_field(p,
'Axle',
field_type=str,
allowed_values=['Front', 'Rear', 'Steering'])
_check_field(p, 'Noise Model', field_type=dict, optional=True)
if 'Noise Model' in p:
_check_field(p['Noise Model'],
'Noise Type',
allowed_values=["Normal", "Normal Drift"])
self._load_properties(p)
def _load_properties(self, p: dict):
"""Private function that loads properties and sets them to class variables
Args:
p (dict): Properties dictionary
"""
self._update_rate = p['Update Rate']
self._axle = p['Axle']
self._noise_model = WANoNoiseModel()
if 'Noise Model' in p:
n = p['Noise Model']
if n['Noise Type'] == 'Normal Drift':
self._noise_model = WANormalDriftNoiseModel(n)
elif n['Noise Type'] == 'Normal':
self._noise_model = WANormalNoiseModel(n)
else:
raise TypeError(
f"{p['Noise Type']} is not an implemented model type")
self._vel = WAVector()
self._angular_speed = 0
self._tire_radius = self._vehicle.get_tire_radius(self._axle)
def synchronize(self, time):
"""Synchronize the sensor at the specified time
Args:
time (float): the time at which the sensors are synchronized to
"""
pass
def advance(self, step):
"""Advance the state of the sensor by the specified time step
Args:
step (float): the step to update the sensor by
"""
self._vel = self._vehicle.get_pos_dt()
self._noise_model.add_noise(self._vel)
self._angular_speed = self._vel.length / self._tire_radius
def get_data(self):
"""Get the sensor data
Returns:
WAVector: The coordinate location of the vehicle in the form of [longitude, latitude, altitude]
"""
return self._angular_speed
class WAGPSSensor(WASensor):
"""Derived sensor class that implements a GPS model
GPS stands for Global Positioning System. GPS's are everyday sensors you can find in your computer, watch, phone, etc.
They essentially ping satilites orbiting earth for positional information. This information, in real life, is not very
precise. IMU's and GPS's are commonly "fused" together to achieve highly accurate and reliable position information.
Args:
system (WASystem): The system for the simulation
filename (str): a json specification file that describes a GPS sensor model
vehicle (WAVehicle, optional): The vehicle to attach to. If not passed, body must be passed.
body (WABody, optional): The body to attach to. If not passed, vehicle must be passed.
"""
# Global filenames for gps sensors
_SBG_GPS_SENSOR_FILE = "sensors/models/SBG_GPS.json"
SBG_GPS_SENSOR_FILE = _WAStaticAttribute('_SBG_GPS_SENSOR_FILE',
get_wa_data_file)
def __init__(self,
system: 'WASystem',
filename: str,
vehicle: 'WAVehicle' = None,
body: 'WABody' = None):
super().__init__(vehicle, body)
if body is not None:
raise NotImplementedError(
"Setting 'body' is currently not supported. Please pass a vehicle instead"
)
self._vehicle = vehicle
self._coord = None
j = _load_json(filename)
# Validate the json file
_check_field(j, 'Type', value='Sensor')
_check_field(j, 'Template', value='GPS')
_check_field(j, 'Properties', field_type=dict)
p = j['Properties']
_check_field(p, 'Update Rate', field_type=int)
_check_field(p, 'GPS Reference', field_type=list)
_check_field(p, 'Noise Model', field_type=dict, optional=True)
if 'Noise Model' in p:
_check_field(p['Noise Model'],
'Noise Type',
allowed_values=["Normal", "Normal Drift"])
self._load_properties(p)
def _load_properties(self, p: dict):
"""Private function that loads properties and sets them to class variables
Args:
p (dict): Properties dictionary
"""
self._update_rate = p['Update Rate']
self._reference = WAVector(p['GPS Reference'])
self._noise_model = WANoNoiseModel()
if 'Noise Model' in p:
n = p['Noise Model']
if n['Noise Type'] == 'Normal Drift':
self._noise_model = WANormalDriftNoiseModel(n)
elif n['Noise Type'] == 'Normal':
self._noise_model = WANormalNoiseModel(n)
else:
raise TypeError(
f"{p['Noise Type']} is not an implemented model type")
self._pos = WAVector()
def synchronize(self, time):
"""Synchronize the sensor at the specified time
Args:
time (float): the time at which the sensors are synchronized to
"""
pass
def advance(self, step):
"""Advance the state of the sensor by the specified time step
Args:
step (float): the step to update the sensor by
"""
self._pos = self._vehicle.get_pos()
self._noise_model.add_noise(self._pos)
self._coord = WAGPSSensor.cartesian_to_gps(self._pos, self._reference)
def get_data(self) -> WAVector:
"""Get the sensor data
Returns:
WAVector: The coordinate location of the vehicle in the form of [longitude, latitude, altitude]
"""
return self._coord
@staticmethod
def cartesian_to_gps(coords: WAVector, ref: WAVector):
"""Convert a point from cartesian to gps
Args:
coords(WAVector): The coordinate to convert
ref(WAVector): The "origin" or reference point
Returns:
WAVector: The coordinate in the form of[longitude, latitude, altitude]
"""
lat = (coords.y / WA_EARTH_RADIUS) * 180.0 / WA_PI + ref.y
lon = (coords.x / (WA_EARTH_RADIUS * np.cos(lat * WA_PI / 180.0))
) * 180.0 / WA_PI + ref.x # noqa
alt = coords.z + ref.z
lon = lon + 360.0 if lon < -180.0 else lon - 360.0 if lon > 180.0 else lon
return WAVector([lon, lat, alt])
@staticmethod
def gps_to_cartesian(coords: WAVector, ref: WAVector):
"""Convert a gps coordinate to cartesian given some reference
Args:
coords(WAVector): The coordinate to convert
ref(WAVector): The "origin" or reference point
Returns:
WAVector: The x, y, z point in cartesian
"""
lon = coords.x
lat = coords.y
alt = coords.z
x = ((lon - ref.x) * WA_PI / 180.0) * (
WA_EARTH_RADIUS * np.cos(lat * WA_PI / 180.0)) # noqa
y = ((lat - ref.y) * WA_PI / 180.0) * WA_EARTH_RADIUS
z = alt - ref.z
return WAVector([x, y, z])
class WAIMUSensor(WASensor):
"""Derived sensor class that implements an IMU model
IMU stands for Inertial Measurement Unit. They come in all shapes and sizes. Typically, an IMU is used to
measure position and orientation of whatever it is attached to. To do this, it is actually made up of more sensors,
which when combined, produce a localizable reading. Common sub-sensors include Gyroscopes, Accelerometers, Altimeters and
Magnetometer.
This implementation combines three sensors to produce the expected output: Gyroscope, Accelerometer and Magnetometer. A good
description of the underyling implementation can be found in `these slides <https://stanford.edu/class/ee267/lectures/lecture9.pdf>`_.
Args:
system (WASystem): The system for the simulation
filename (str): a json specification file that describes an IMU sensor model
vehicle (WAVehicle, optional): The vehicle to attach to. If not passed, body must be passed.
body (WABody, optional): The body to attach to. If not passed, vehicle must be passed.
"""
# Global filenames for imu sensors
_SBG_IMU_SENSOR_FILE = "sensors/models/SBG_IMU.json"
SBG_IMU_SENSOR_FILE = _WAStaticAttribute('_SBG_IMU_SENSOR_FILE',
get_wa_data_file)
def __init__(self,
system: 'WASystem',
filename: str,
vehicle: 'WAVehicle' = None,
body: 'WABody' = None):
super().__init__(vehicle, body)
if body is not None:
raise NotImplementedError(
"Setting 'body' is currently not supported. Please pass a vehicle instead"
)
self._vehicle = vehicle
self._pos_dtdt = None
self._rot = None
self._rot_dt = None
j = _load_json(filename)
# Validate the json file
_check_field(j, 'Type', value='Sensor')
_check_field(j, 'Template', value='IMU')
_check_field(j, 'Properties', field_type=dict)
p = j['Properties']
_check_field(p, 'Update Rate', field_type=int)
_check_field(p, 'Noise Model', field_type=dict, optional=True)
if 'Noise Model' in p:
_check_field(p['Noise Model'],
'Noise Type',
allowed_values=["Normal", "Normal Drift"])
self._load_properties(p)
self._acc = WAVector()
self._omega = WAVector()
self._orientation = WAVector()
def _load_properties(self, p: dict):
"""Private function that loads properties and sets them to class variables
Args:
p (dict): Properties dictionary
"""
self._update_rate = p['Update Rate']
self._noise_model = WANoNoiseModel()
if 'Noise Model' in p:
n = p['Noise Model']
if n['Noise Type'] == 'Normal Drift':
self._noise_model = WANormalDriftNoiseModel(n)
elif n['Noise Type'] == 'Normal':
self._noise_model = WANormalNoiseModel(n)
else:
raise TypeError(
f"{p['Noise Type']} is not an implemented model type")
def synchronize(self, time):
"""Synchronize the sensor at the specified time
Args:
time (float): the time at which the sensors are synchronized to
"""
self._noise_model.add_noise(self._acc) # Acceleration (accelerometer)
# Angular velocity (gyroscope)
self._noise_model.add_noise(self._omega)
# self.noise_model.add_noise(self.orientation) # Orientation ("Magnometer") # noqa
def advance(self, step):
"""Advance the state of the sensor by the specified time step
Args:
step (float): the step to update the sensor by
"""
self._pos_dtdt = self._vehicle.get_pos_dtdt()
self._rot = self._vehicle.get_rot()
self._rot_dt = self._vehicle.get_rot_dt()
def get_data(self):
"""Get the sensor data
Returns:
(WAVector, WAQuaternion, WAQuaternion): Tuple in the form of (acceleration, angular_velocity, orientation)
"""
return self._pos_dtdt, self._rot_dt, self._rot
class WALinearKinematicBicycle(WAVehicle):
"""A linear, kinematic bicycle model
Args:
system (WASystem): the system used to run the simulation
vehicle_inputs (WAVehicleInputs): The vehicle inputs
filename (str): json specification file used to describe the parameters for the vehicle
init_pos (WAVector): initial position
init_rot (WAQuaternion): initial rotation
init_pos_dt (WAVector): initial velocity
"""
# Global filenames for vehicle models
_GO_KART_MODEL_FILE = "vehicles/GoKart/GoKart_KinematicBicycle.json"
GO_KART_MODEL_FILE = _WAStaticAttribute('_GO_KART_MODEL_FILE', get_wa_data_file)
def __init__(self, system: 'WASystem', vehicle_inputs: 'WAVehicleInputs', filename: str, init_pos: WAVector = WAVector(), init_rot: WAQuaternion = WAQuaternion.from_z_rotation(0), init_pos_dt: WAVector = WAVector()):
super().__init__(system, vehicle_inputs, filename)
# Simple state variables
self._x = init_pos.x
self._y = init_pos.y
self._yaw = init_rot.to_euler_yaw()
self._v = init_pos_dt.length
self._acc = 0.0
self._steering = 0
self._throttle = 0
self._braking = 0
# Wheel angular velocity
self._omega = 0.0
self._omega_dot = 0.0
self._yaw_dt = 0.0
self._yaw_dtdt = 0.0
self._last_yaw = 0.0
properties = load_properties_from_json(filename, "Vehicle Properties")
self._initialize(properties)
def _initialize(self, vp):
"""Initialize the vehicle parameters
Expected properties:
"Wheelbase" (float): distance between the front and back tire
"Mass" (float): mass of the vehicle
"Gear Ratio" (float): gear ratio of the simulated powertrain
"Effective Radius" (float):
"Inertia" (float): Effective inertia for the vehicle
"Tire Radius" (float): The radius of the tires
"Aerodynamic Coefficient" (float):
"Friction Coefficient" (float):
"C" (float)
"Max Force" (float)
"Torque Coefficients" (list)
"Steering" ([min, max]): Min and max throttle values
"Throttle" ([min, max]): Min and max throttle values
"Braking" ([min, max]): Min and max braking values
Args:
vp (dict): Vehicle preoperties (see above for expected keys)
"""
# Initialize vehicle properties
self._mass = vp["Mass"]
# Torque coefficients
self._a = vp["Torque Coefficients"]
# Gear ratio, effective radius and inertia
self._GR = vp["Gear Ratio"]
self._r_eff = vp["Effective Radius"]
self._J_e = vp["Inertia"]
self._R_tire = vp["Tire Radius"]
# Aerodynamic and friction coefficients
self._c_a = vp["Aerodynamic Coefficient"]
self._c_rl = vp["Friction Coefficient"]
# Tire forces
self._c = vp["C"]
self._F_max = vp["Max Force"]
self._L = vp["Wheelbase"]
(self._min_steering, self._max_steering) = vp["Steering"]
(self._min_throttle, self._max_throttle) = vp["Throttle"]
(self._min_braking, self._max_braking) = vp["Braking"]
def synchronize(self, time):
s = self._vehicle_inputs.steering
t = self._vehicle_inputs.throttle
b = self._vehicle_inputs.braking
self._steering = np.clip(s, self._min_steering, self._max_steering)
self._throttle = np.clip(t, self._min_throttle, self._max_throttle)
self._braking = np.clip(b, self._min_braking, self._max_braking)
if self._braking > 1e-1:
self._throttle = -self._braking
def advance(self, step):
if self._v == 0 and self._throttle == 0:
F_x = 0
F_load = 0
T_e = 0
else:
if self._v == 0:
# Remove possibity of divide by zero error
self._v = 1e-8
self._omega = 1e-8
# Update engine and tire forces
F_aero = self._c_a * self._v ** 2
R_x = self._c_rl * self._v
F_g = self._mass * WA_GRAVITY * np.sin(0)
F_load = F_aero + R_x + F_g
T_e = self._throttle * (
self._a[0] + self._a[1] * self._omega +
self._a[2] * self._omega ** 2
)
omega_w = self._GR * self._omega # Wheel angular velocity
s = (omega_w * self._r_eff - self._v) / self._v # slip ratio
cs = self._c * s
if abs(s) < 1:
F_x = cs
else:
F_x = self._F_max
if self._throttle < 0:
if self._v <= 0:
F_x = 0
else:
F_x = -abs(F_x)
# Update state information
self._x += self._v * np.cos(self._yaw) * step
self._y += self._v * np.sin(self._yaw) * step
self._yaw += self._v / self._L * np.tan(self._steering) * step
self._v += self._acc * step
self._acc = (F_x - F_load) / self._mass
self._omega += self._omega_dot * step
self._omega_dot = (T_e - self._GR * self._r_eff * F_load) / self._J_e
v_epsilon = abs(self._v) > 0.1
self._yaw_dt = (self._last_yaw - self._yaw) / step if v_epsilon else 0
self._yaw_dtdt = (self._last_yaw - self._yaw) / step if v_epsilon else 0 # noqa
self._last_yaw = self._yaw
def get_tire_radius(self, axle : str) -> float:
axle = axle.lower()
assert axle == 'front' or axle == 'rear'
return self._R_tire
def get_pos(self) -> WAVector:
return WAVector([self._x, self._y, 0.0])
def get_rot(self) -> WAQuaternion:
return WAQuaternion.from_z_rotation(self._yaw)
def get_pos_dt(self) -> WAVector:
return WAVector([self._v * np.cos(self._yaw), self._v * np.sin(self._yaw), 0.0])
def get_rot_dt(self) -> WAQuaternion:
return WAQuaternion.from_z_rotation(self._yaw_dt)
def get_pos_dtdt(self) -> WAVector:
angle = np.tan(np.interp(self._steering, [-1, 1], [self._min_steering, self._max_steering])) # noqa
angle = angle if angle != 0 else 1e-3
tr = self._L / angle
tr = tr if tr != 0 else 1e-3
return WAVector([self._acc, self._v ** 2 / tr, WA_GRAVITY])
def get_rot_dtdt(self) -> WAQuaternion:
return WAQuaternion.from_z_rotation(self._yaw_dtdt)
class WAChronoSensor(WASensor):
"""Derived Sensor class that is still abstract that essentially wraps a ChSensor.
A ChSensor has to be attached to a body that's present in the Chrono world. That body can then be moved
which then moves the ChSensor. If a vehicle is passed, the sensor is automatically attached to the chassis
body with some offset. If a vehicle is not passed, a :class:`~WABody` must be passed and the sensor
will be attached to that object. If both are passed, an exception is raised.
If body is passed, it must have one attribute: position.
Args:
system (WAChronoSystem): The system for the simulation
filename (str): The json file that describes this sensor
vehicle (WAChronoVehicle, optional): The vehicle to attach to. If not passed, body must be passed.
body (WABody, optional): The body to attach to. If not passed, vehicle must be passed.
"""
# Global filenames for various sensors
_MONO_CAM_SENSOR_FILE = "sensors/models/generic_monocamera.json"
_LDMRS_LIDAR_SENSOR_FILE = "sensors/models/ldmrs.json"
MONO_CAM_SENSOR_FILE = _WAStaticAttribute('_MONO_CAM_SENSOR_FILE',
get_chrono_data_file)
LDMRS_LIDAR_SENSOR_FILE = _WAStaticAttribute('_LDMRS_LIDAR_SENSOR_FILE',
get_chrono_data_file)
def __init__(self,
system: 'WAChronoSystem',
filename: str,
vehicle: 'WAChronoVehicle' = None,
body: 'WABody' = None):
if missing_chrono_sensor:
sens_import_error('WAChronoSensor.__init__')
super().__init__(vehicle, body)
j = _load_json(filename)
# Validate the json file
_check_field(j, 'Type', value='Sensor')
_check_field(j,
'Template',
allowed_values=['Camera', 'Lidar', 'IMU', 'GPS']) # noqa
_check_field(j, 'Offset Pose', field_type=dict)
offset_pose = ChFrame_from_json(j['Offset Pose'])
if vehicle is not None:
body = vehicle._vehicle.GetChassisBody()
else:
asset = body
body = chrono.ChBody()
body.SetPos(WAVector_to_ChVector(asset.position))
body.SetBodyFixed(True)
system._system.AddBody(body)
# Create the chrono sensor through the Sensor chrono class
self._sensor = sens.Sensor.CreateFromJSON(filename, body,
offset_pose) # noqa
def synchronize(self, time):
"""Synchronize the sensor at the specified time
Args:
time (float): the time at which the sensors are synchronized to
"""
pass
def advance(self, step):
"""Advance the state of the sensor by the specified time step
Args:
step (float): the step to update the sensor by
"""
pass
def get_data(self):
"""Get the sensor data
Returns:
np.ndarray: The sensor data. Type depends on the actual sensor
"""
name = self._sensor.GetName().lower()
if 'camera' in name:
buff = self._sensor.GetMostRecentRGBA8Buffer()
return buff.GetRGBA8Data()
if 'lidar' in name:
buff = self._sensor.GetMostRecentXYZIBuffer()
return buff.GetXYZIData()
else:
raise TypeError(
f"WAChronoSensorManager.get_data: can't determine sensor type from name: '{name}'."
)
class WAChronoEnvironment(WAEnvironment):
"""The environment wrapper that's responsible for holding Chrono assets and the terrain
TODO: Add assets from file
Args:
system (WASystem): the wa system that wraps a ChSystem
filename (str): the json specification file describing the environment
"""
# Global filenames for environment models
_EGP_ENV_MODEL_FILE = "environments/ev_grand_prix.json"
EGP_ENV_MODEL_FILE = _WAStaticAttribute(
'_EGP_ENV_MODEL_FILE', get_chrono_data_file)
def __init__(self, system: 'WAChronoSystem', filename: str):
super().__init__()
self._system = system
self._terrain = load_chrono_terrain_from_json(system, filename)
load_environment_from_json(self, filename)
def synchronize(self, time):
self._terrain.Synchronize(time)
def advance(self, step):
self._terrain.Advance(step)
for asset in self._updating_assets:
asset.chrono_body.SetPos(WAVector_to_ChVector(asset.position))
def add_asset(self, asset: 'Any'):
if isinstance(asset, chrono.ChBody):
self._system._system.AddBody(asset)
return
if isinstance(asset, WABody):
if not hasattr(asset, 'size') or not hasattr(asset, 'position'):
raise AttributeError(
"Body must have 'size', and 'position' fields")
position = asset.position
yaw = 0 if not hasattr(asset, 'yaw') else asset.yaw
size = asset.size
body_type = 'box'
if hasattr(asset, 'body_type'):
body_type = asset.body_type
if body_type == 'sphere':
body = chrono.ChBodyEasySphere(size.length, 1000, True, False)
body.SetBodyFixed(True)
elif body_type == 'box':
body = chrono.ChBodyEasyBox(
size.x, size.y, size.z, 1000, True, False)
body.SetBodyFixed(True)
else:
raise ValueError(
f"'{asset.body_type}' not a supported body type.")
body.SetPos(WAVector_to_ChVector(position))
body.SetRot(chrono.Q_from_AngZ(-yaw + WA_PI / 2))
if hasattr(asset, 'color'):
color = asset.color
body.AddAsset(chrono.ChColorAsset(
chrono.ChColor(color.x, color.y, color.z)))
texture = chrono.ChVisualMaterial()
texture.SetDiffuseColor(
chrono.ChVectorF(color.x, color.y, color.z))
chrono.CastToChVisualization(
body.GetAssets()[0]).material_list.append(texture)
if hasattr(asset, 'texture'):
texture = chrono.ChVisualMaterial()
texture.SetKdTexture(get_wa_data_file(asset.texture))
chrono.CastToChVisualization(
body.GetAssets()[0]).material_list.append(texture)
self._system._system.AddBody(body)
asset.chrono_body = body
super().add_asset(asset)
