def convert_imu_from_source_to_agent(self, source: IMUSensor) -> IMUData:
return IMUData(
accelerometer=Vector3D(
x=source.accelerometer[0],
y=source.accelerometer[1],
z=source.accelerometer[2],
),
gyroscope=Vector3D(
x=source.gyroscope[0], y=source.gyroscope[1], z=source.gyroscope[2]
),
)
def convert_imu_from_source_to_agent(self, source) -> IMUData:
"""
Convert Jetson raw IMUData to IMUData(accelerometer, gyroscope).
Args:
source ():
Returns:
IMUData(accelerometer, gyroscope)
"""
# TODO fill in data here
return IMUData(
accelerometer=Vector3D(x=0, y=0, z=0),
gyroscope=Vector3D(x=0, y=0, z=0),
)
class Vehicle(BaseModel):
"""
Encodes the Vehicle's state at the last tick
"""
velocity: Optional[Vector3D] = Field(default=Vector3D(x=0, y=0, z=0))
transform: Optional[Transform] = Field(default=Transform())
control: VehicleControl = Field(default=VehicleControl()) # ?
wheel_base: float = Field(
default=2.875,
title="Wheel Base length of the vehilce in meters",
description="Default to tesla model 3's wheel base",
)
@staticmethod
def get_speed(vehicle):
# TODO consider the jetson case
"""
Compute speed of a vehicle in Km/h.
:param vehicle: the vehicle for which speed is calculated
:return: speed as a float in Km/h
"""
vel = vehicle.velocity
return 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
def convert_vehicle_from_source_to_agent(self, source: JetsonVehicle) -> Vehicle:
return Vehicle(transform=Transform(
location=Location(x=-source.location[0], y=source.location[1], z=-source.location[2]),
rotation=Rotation(roll=-source.rotation[2], pitch=source.rotation[0], yaw=-source.rotation[1]),
),
velocity=Vector3D(x=-source.velocity[0], y=source.velocity[1], z=-source.velocity[2]),
wheel_base=0.26,
control=self.convert_control_from_source_to_agent(source=source)
)
def convert_vector3d_from_source_to_agent(self, source) -> Vector3D:
"""
Convert Jetson raw Vector3d Data to a Vector3D Object.
Args:
source ():
Returns:
Vector3D(x, y, z)
"""
return Vector3D(x=source.x, y=source.y, z=source.z)
def convert_vive_tracker_data_from_source_to_agent(self, source: Optional[ViveTrackerMessage]) -> \
Optional[ViveTrackerData]:
if source is not None:
vive_tracker_data = ViveTrackerData(
location=Location(x=-source.x, y=source.y, z=-source.z),
rotation=Rotation(roll=-source.roll,
pitch=source.pitch + 60,
yaw=-source.yaw),
velocity=Vector3D(x=-source.vel_x,
y=source.vel_y,
z=-source.vel_z))
return vive_tracker_data
return None
class Vehicle(BaseModel):
"""
Encodes the Vehicle's state at the last tick
"""
velocity: Optional[Vector3D] = Field(default=Vector3D(x=0, y=0, z=0))
transform: Optional[Transform] = Field(default=Transform())
acceleration: Optional[Vector3D] = Field(default=Vector3D(x=0, y=0, z=0))
control: VehicleControl = Field(default=VehicleControl()) # ?
wheel_base: float = Field(
default=2.875,
title="Wheel Base length of the vehilce in meters",
description="Default to tesla model 3's wheel base",
)
@staticmethod
def get_speed(vehicle):
# TODO consider the jetson case
"""
Compute speed of a vehicle in Km/h.
:param vehicle: the vehicle for which speed is calculated
:return: speed as a float in Km/h
"""
vel = vehicle.velocity
return 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
def to_array(self) -> np.ndarray:
return np.concatenate([
self.transform.to_array(),
self.velocity.to_array(),
self.acceleration.to_array(),
self.control.to_array()
])
def __repr__(self):
return f"Location: {self.transform.location.__str__()} | Rotation: {self.transform.rotation.__str__()} | " \
f"Velocity: {self.velocity.__str__()} | Acceleration: {self.acceleration.__str__()}"
def convert_vehicle_from_source_to_agent(self, source) -> Vehicle:
transform = source.get("transform", None)
velocity = source.get("velocity", None)
control = source.get("control", None)
acceleration = source.get("acceleration", None)
vehicle = Vehicle()
if transform is not None:
vehicle.transform = self.convert_transform_from_source_to_agent(
transform)
if velocity is not None:
vehicle.velocity = Vector3D(
x=velocity.x,
y=-velocity.z,
z=velocity.y,
)
if control is not None:
vehicle.control = self.convert_control_from_source_to_agent(
control)
if acceleration is not None:
vehicle.acceleration = acceleration
return vehicle
def convert_vive_tracker_data_from_source_to_agent(self, source: Optional[ViveTrackerMessage]) -> \
Optional[ViveTrackerData]:
"""
Converts raw Vive Tracker data to ViveTrackerData(Location, Rotation, Velocity).
Args:
source ():
Returns:
ViveTrackerData(Location, Rotation, Velocity)
"""
if source is not None:
vive_tracker_data = ViveTrackerData(
location=Location(x=-source.x, y=source.y, z=-source.z),
rotation=Rotation(
roll=-source.roll,
pitch=source.pitch - 90, # 不知道为什么有60度的误差
yaw=-source.yaw),
velocity=Vector3D(x=source.vel_x,
y=source.vel_y,
z=source.vel_z))
return vive_tracker_data
return None
def convert_vector3d_from_source_to_agent(
self, source: carla.Vector3D) -> Vector3D:
"""Convert CARLA raw Vector3d Data to a Vector3D Object."""
return Vector3D(x=source.x, y=source.y, z=source.z)
def convert_imu_from_source_to_agent(self, source) -> IMUData:
# TODO fill in data here
return IMUData(
accelerometer=Vector3D(x=0, y=0, z=0),
gyroscope=Vector3D(x=0, y=0, z=0),
)
def convert_vector3d_from_source_to_agent(self, source) -> Vector3D:
return Vector3D(x=source.x, y=source.y, z=source.z)