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)