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)