def convert_data(self):
try:
rear_rgb = None
if self.ios_config.ar_mode and self.world_cam_streamer.curr_image is not None:
front_rgb = cv2.rotate(self.world_cam_streamer.curr_image,
cv2.ROTATE_90_CLOCKWISE)
else:
front_rgb = cv2.rotate(self.world_cam_streamer.curr_image,
cv2.ROTATE_90_CLOCKWISE)
sensor_data: SensorsData = \
self.ios_bridge.convert_sensor_data_from_source_to_agent(
{
"front_rgb": front_rgb,
"front_depth": self.depth_cam_streamer.curr_image,
"rear_rgb": rear_rgb
}
)
vehicle = self.ios_bridge.convert_vehicle_from_source_to_agent({
"transform":
self.veh_state_streamer.transform,
"velocity":
self.veh_state_streamer.velocity,
"acceleration":
self.veh_state_streamer.acceleration
})
vehicle.control = self.control_streamer.control_tx
if self.depth_cam_streamer.intrinsics is not None:
self.agent.front_depth_camera.intrinsics_matrix = self.depth_cam_streamer.intrinsics
if self.world_cam_streamer.intrinsics is not None:
self.agent.front_rgb_camera.intrinsics_matrix = self.world_cam_streamer.intrinsics
return sensor_data, vehicle
except Exception as e:
self.logger.error(f"Cannot convert data: {e}")
return SensorsData(), Vehicle()
def execute_npcs_step(self):
# TODO this can be parallelized
try:
for agent, actor in self.npc_agents.items():
new_vehicle = self.carla_bridge.convert_vehicle_from_source_to_agent(actor)
curr_control: VehicleControl = agent.run_step(sensors_data=SensorsData(), vehicle=new_vehicle)
carla_control = self.carla_bridge.convert_control_from_agent_to_source(curr_control)
actor.apply_control(carla_control)
except Exception as e:
self.logger.error(f"Failed to execute step for NPC. "
f"Error: {e}")
def convert_sensor_data_from_source_to_agent(self, source) -> SensorsData:
return SensorsData(
front_rgb=self.convert_rgb_from_source_to_agent(
source=source.get("front_rgb", None)),
rear_rgb=self.convert_rgb_from_source_to_agent(
source=source.get("rear_rgb", None)),
front_depth=self.convert_depth_from_source_to_agent(
source=source.get("front_depth", None)),
imu_data=self.convert_imu_from_source_to_agent(
source=source.get("imu", None)),
)
def convert_sensor_data_from_source_to_agent(self,
source: dict) -> SensorsData:
front_rgb = source.get("front_rgb", None)
front_depth = source.get("front_depth", None)
sensor_data = SensorsData()
if front_rgb is not None:
sensor_data.front_rgb = self.convert_rgb_from_source_to_agent(
front_rgb)
if front_depth is not None:
sensor_data.front_depth = self.convert_depth_from_source_to_agent(
front_depth)
return sensor_data
def convert_sensor_data_from_source_to_agent(self,
source: dict) -> SensorsData:
"""Returns CARLA Sensors Data from raw front RGB, rear RGB, front depth, and IMU Data."""
return SensorsData(
front_rgb=self.convert_rgb_from_source_to_agent(
source=source.get("front_rgb", None)),
rear_rgb=self.convert_rgb_from_source_to_agent(
source=source.get("rear_rgb", None)),
front_depth=self.convert_depth_from_source_to_agent(
source=source.get("front_depth", None)),
imu_data=self.convert_imu_from_source_to_agent(
source=source.get("imu", None)),
)
def __init__(self,
carla_settings: CarlaConfig,
agent_settings: AgentConfig,
npc_agent_class,
competition_mode=False,
start_bbox: np.ndarray = np.array([5, -5, 0, 13, 5, 50]),
lap_count=10):
"""
Args:
carla_settings: CarlaConfig instance
agent_settings: AgentConfig instance
npc_agent_class: an agent class
competition_mode: [Optional] True/False
start_bbox: [Optional] array of [minx, miny, minz, maxx, maxy, maxz].
[5, -5, 0, 13, 5, 50] is the bbox for easy_map.
[-815, 20, -760, -770, 120, -600] is the bbox for berkeley_minor_map
lap_count: [Optional] total lap count
"""
self.carla_settings = carla_settings
self.agent_settings = agent_settings
self.carla_bridge = CarlaBridge()
self.npc_agent_class = npc_agent_class
self.world = None
self.client = None
self.controller = None
self.display = None
self.agent = None
self.npc_agents: Dict[npc_agent_class, Any] = {}
self.agent_collision_counter = 0
self.competition_mode = competition_mode
self.start_bbox = start_bbox
self.lap_count = lap_count
self.completed_lap_count = 0
self.sensor_data = SensorsData()
self.vehicle_state = Vehicle()
self.start_simulation_time: Optional[float] = None
self.start_vehicle_position: Optional[np.array] = None
self.end_simulation_time: Optional[float] = None
self.end_vehicle_position: Optional[np.array] = None
self.logger = logging.getLogger(__name__)
self.timestep_counter = 0
def convert_sensor_data_from_source_to_agent(self, source) -> SensorsData:
"""
Returns Jetson Sensors Data from raw front RGB, rear RGB, front depth, and IMU Data.
Args:
source ():
Returns:
SensorData(front_RGB, rear_RGB, front_depth, IMU_Data)
"""
return SensorsData(front_rgb=self.convert_rgb_from_source_to_agent(
source=source.get("front_rgb", None)),
rear_rgb=self.convert_rgb_from_source_to_agent(
source=source.get("rear_rgb", None)),
front_depth=self.convert_depth_from_source_to_agent(
source=source.get("front_depth", None)),
imu_data=self.convert_imu_from_source_to_agent(
source=source.get("imu", None)),
location=self.convert_location_from_source_to_agent(
source=source.get("location", None)),
rotation=self.convert_rotation_from_source_to_agent(
source=source.get("rotation", None)),
velocity=self.convert_location_from_source_to_agent(
source=source.get("velocity", None)))