diff --git a/LibCarla/source/carla/sensor/data/LibITS.h b/LibCarla/source/carla/sensor/data/LibITS.h index 6fbb32bd7..b3f419b6a 100644 --- a/LibCarla/source/carla/sensor/data/LibITS.h +++ b/LibCarla/source/carla/sensor/data/LibITS.h @@ -386,17 +386,6 @@ public: AccelerationControl_cruiseControlEngaged = 5, AccelerationControl_speedLimiterEngaged = 6 } e_AccelerationControl; - - /* BIT_STRING_s*/ - // struct BIT_STRING_s { - // // uint8_t *buf; /* BIT STRING body */ - // //note: we cant use pointers - // uint8_t buf; /* BIT STRING body */ - // int size; /* Size of the above buffer */ - - // int bits_unused;/* Unused trailing bits in the last octet (0..7) */ - - // }; /* AccelerationControl */ typedef uint8_t AccelerationControl_t; @@ -516,7 +505,6 @@ public: /* TimestampIts */ - // typedef BIT_STRING_s TimestampIts_t; typedef long TimestampIts_t; /* ProtectedZoneRadius Dependencies */ @@ -582,7 +570,6 @@ public: } e_ExteriorLights; /* ExteriorLights */ - // typedef BIT_STRING_s ExteriorLights_t; typedef uint8_t ExteriorLights_t; /* DeltaLatitude Dependencies */ typedef enum DeltaLatitude { @@ -718,15 +705,10 @@ public: /* HighFrequencyContainer */ typedef struct HighFrequencyContainer { - // HighFrequencyContainer() {}; - // HighFrequencyContainer(HighFrequencyContainer &hfc) {}; - // ~HighFrequencyContainer() {}; HighFrequencyContainer_PR present; - // union - // { - BasicVehicleContainerHighFrequency_t basicVehicleContainerHighFrequency; - RSUContainerHighFrequency_t rsuContainerHighFrequency; - // }; + + BasicVehicleContainerHighFrequency_t basicVehicleContainerHighFrequency; + RSUContainerHighFrequency_t rsuContainerHighFrequency; } HighFrequencyContainer_t; @@ -749,17 +731,10 @@ public: /* LowFrequencyContainer */ typedef struct LowFrequencyContainer { - // LowFrequencyContainer() {}; - // LowFrequencyContainer(LowFrequencyContainer &lfc) {}; - // ~LowFrequencyContainer() {}; LowFrequencyContainer_PR present; - // union - // { - BasicVehicleContainerLowFrequency_t basicVehicleContainerLowFrequency; + // Since only option is available + BasicVehicleContainerLowFrequency_t basicVehicleContainerLowFrequency; - // }; - //Since only option is available - // BasicVehicleContainerLowFrequency_t basicVehicleContainerLowFrequency; } LowFrequencyContainer_t; /* CamParameters */ @@ -768,7 +743,7 @@ public: BasicContainer_t basicContainer; HighFrequencyContainer_t highFrequencyContainer; LowFrequencyContainer_t lowFrequencyContainer; /* OPTIONAL */ - // SpecialVehicleContainer *specialVehicleContainer; /* OPTIONAL */ + // Optional TODO: SpecialVehicleContainer *specialVehicleContainer } CamParameters_t; /* CoopAwareness*/ diff --git a/LibCarla/source/carla/sensor/data/V2XData.h b/LibCarla/source/carla/sensor/data/V2XData.h index 18e23e4b1..4747b5ef8 100644 --- a/LibCarla/source/carla/sensor/data/V2XData.h +++ b/LibCarla/source/carla/sensor/data/V2XData.h @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -45,19 +45,19 @@ namespace carla CAMDataS &operator=(CAMDataS &&) = default; - // /// Returns the number of current received messages. + // Returns the number of current received messages. size_t GetMessageCount() const { return MessageList.size(); } - /// Deletes the current messages. + // Deletes the current messages. void Reset() { MessageList.clear(); } - /// Adds a new detection. + // Adds a new detection. void WriteMessage(CAMData message) { MessageList.push_back(message); @@ -69,7 +69,6 @@ namespace carla friend class s11n::CAMDataSerializer; }; - class CustomV2XDataS { @@ -78,19 +77,19 @@ namespace carla CustomV2XDataS &operator=(CustomV2XDataS &&) = default; - // /// Returns the number of current received messages. + // Returns the number of current received messages. size_t GetMessageCount() const { return MessageList.size(); } - /// Deletes the current messages. + // Deletes the current messages. void Reset() { MessageList.clear(); } - /// Adds a new detection. + // Adds a new detection. void WriteMessage(CustomV2XData message) { MessageList.push_back(message); @@ -100,7 +99,7 @@ namespace carla std::vector MessageList; friend class s11n::CustomV2XDataSerializer; - }; + }; } // namespace s11n } // namespace sensor diff --git a/LibCarla/source/carla/sensor/data/V2XEvent.h b/LibCarla/source/carla/sensor/data/V2XEvent.h index 817630fe7..f146bffb1 100644 --- a/LibCarla/source/carla/sensor/data/V2XEvent.h +++ b/LibCarla/source/carla/sensor/data/V2XEvent.h @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -54,7 +54,7 @@ namespace carla { return Super::size(); } - }; + }; } // namespace data } // namespace sensor diff --git a/PythonAPI/carla/source/libcarla/SensorData.cpp b/PythonAPI/carla/source/libcarla/SensorData.cpp index 73559acee..61c39ab15 100644 --- a/PythonAPI/carla/source/libcarla/SensorData.cpp +++ b/PythonAPI/carla/source/libcarla/SensorData.cpp @@ -197,7 +197,6 @@ namespace data { out << "CustomV2XData(power=" << std::to_string(data.Power) << ", stationId=" << std::to_string(data.Message.header.stationID) << ", messageId=" << std::to_string(data.Message.header.messageID) - // << ", message=" << data.Message.message << ')'; return out; } diff --git a/PythonAPI/carla/source/libcarla/V2XData.cpp b/PythonAPI/carla/source/libcarla/V2XData.cpp index 748e330a9..2be2198ce 100644 --- a/PythonAPI/carla/source/libcarla/V2XData.cpp +++ b/PythonAPI/carla/source/libcarla/V2XData.cpp @@ -267,7 +267,6 @@ static boost::python::dict GetBVCHighFrequency(const CAM_t message) HeadingValueConfidence["Value"] = bvchf.heading.headingValue; HeadingValueConfidence["Confidence"] = GetHeadingConfidenceString(bvchf.heading.headingConfidence); BVCHighFrequency["Heading"] = HeadingValueConfidence; - // ValueConfidence.clear(); boost::python::dict SpeedValueConfidence; SpeedValueConfidence["Value"] = bvchf.speed.speedValue; SpeedValueConfidence["Confidence"] = GetSpeedConfidenceString(bvchf.speed.speedConfidence); @@ -335,7 +334,6 @@ static boost::python::dict GetBVCHighFrequency(const CAM_t message) ValueConfidence["Value"] = bvchf.lateralAcceleration.lateralAccelerationValue; ValueConfidence["Confidence"] = GetAccelerationConfidenceString(bvchf.lateralAcceleration.lateralAccelerationConfidence); BVCHighFrequency["Lateral Acceleration"] = ValueConfidence; - // BVCHighFrequency["Lateral Acceleration"] = boost::python::object(); } else { @@ -423,7 +421,6 @@ static boost::python::dict GetHighFrequencyContainer(const CAM_t message) { boost::python::dict HFC; CAMContainer::HighFrequencyContainer_t hfc = message.cam.camParameters.highFrequencyContainer; - // HFC["High Frequency Container Present"] = GetHFCPresentString(hfc.present); switch (hfc.present) { case CAMContainer::HighFrequencyContainer_PR_basicVehicleContainerHighFrequency: diff --git a/PythonAPI/examples/V2XDemo.py b/PythonAPI/examples/V2XDemo.py new file mode 100644 index 000000000..eedfae968 --- /dev/null +++ b/PythonAPI/examples/V2XDemo.py @@ -0,0 +1,1419 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +# Allows controlling a vehicle with a keyboard. For a simpler and more +# documented example, please take a look at tutorial.py. + +""" +Welcome to CARLA manual control. + +Use ARROWS or WASD keys for control. + + W : throttle + S : brake + A/D : steer left/right + Q : toggle reverse + Space : hand-brake + P : toggle autopilot + M : toggle manual transmission + ,/. : gear up/down + CTRL + W : toggle constant velocity mode at 60 km/h + + L : toggle next light type + SHIFT + L : toggle high beam + Z/X : toggle right/left blinker + I : toggle interior light + + TAB : change sensor position + ` or N : next sensor + [1-9] : change to sensor [1-9] + G : toggle radar visualization + C : change weather (Shift+C reverse) + Backspace : change vehicle + + O : open/close all doors of vehicle + T : toggle vehicle's telemetry + + V : Select next map layer (Shift+V reverse) + B : Load current selected map layer (Shift+B to unload) + + R : toggle recording images to disk + + CTRL + R : toggle recording of simulation (replacing any previous) + CTRL + P : start replaying last recorded simulation + CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) + CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) + + F1 : toggle HUD + H/? : toggle help + ESC : quit +""" + +from __future__ import print_function + + +# ============================================================================== +# -- find carla module --------------------------------------------------------- +# ============================================================================== + + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/carla-0.9.15-py*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + + +import carla + +from carla import ColorConverter as cc + +import argparse +import collections +import datetime +import logging +import math +import random +import re +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + from pygame.locals import K_COMMA + from pygame.locals import K_DOWN + from pygame.locals import K_ESCAPE + from pygame.locals import K_F1 + from pygame.locals import K_LEFT + from pygame.locals import K_PERIOD + from pygame.locals import K_RIGHT + from pygame.locals import K_SLASH + from pygame.locals import K_SPACE + from pygame.locals import K_TAB + from pygame.locals import K_UP + from pygame.locals import K_a + from pygame.locals import K_b + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_f + from pygame.locals import K_g + from pygame.locals import K_h + from pygame.locals import K_i + from pygame.locals import K_l + from pygame.locals import K_m + from pygame.locals import K_n + from pygame.locals import K_o + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_t + from pygame.locals import K_v + from pygame.locals import K_w + from pygame.locals import K_x + from pygame.locals import K_z + from pygame.locals import K_MINUS + from pygame.locals import K_EQUALS +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.sync = args.sync + self.actor_role_name = args.rolename + try: + self.map = self.world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self.hud = hud + self.player = None + self.collision_sensor = None + # self.lane_invasion_sensor = None + self.gnss_sensor = None + self.imu_sensor = None + self.v2x_sensor = None + self.radar_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._actor_generation = args.generation + self._gamma = args.gamma + self.restart() + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + self.constant_velocity_enabled = False + self.show_vehicle_telemetry = False + self.doors_are_open = False + self.current_map_layer = 0 + self.map_layer_names = [ + carla.MapLayer.NONE, + carla.MapLayer.Buildings, + carla.MapLayer.Decals, + carla.MapLayer.Foliage, + carla.MapLayer.Ground, + carla.MapLayer.ParkedVehicles, + carla.MapLayer.Particles, + carla.MapLayer.Props, + carla.MapLayer.StreetLights, + carla.MapLayer.Walls, + carla.MapLayer.All + ] + + def restart(self): + self.player_max_speed = 1.589 + self.player_max_speed_fast = 3.713 + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 + # Get a random blueprint. + blueprint = random.choice(get_actor_blueprints(self.world, self._actor_filter, self._actor_generation)) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('terramechanics'): + blueprint.set_attribute('terramechanics', 'true') + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # set the max speed + if blueprint.has_attribute('speed'): + self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) + self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) + + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + #self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.imu_sensor = IMUSensor(self.player) + self.v2x_sensor = V2XSensor(self.player, self.hud) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_index + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + if self.sync: + self.world.tick() + else: + self.world.wait_for_tick() + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def next_map_layer(self, reverse=False): + self.current_map_layer += -1 if reverse else 1 + self.current_map_layer %= len(self.map_layer_names) + selected = self.map_layer_names[self.current_map_layer] + self.hud.notification('LayerMap selected: %s' % selected) + + def load_map_layer(self, unload=False): + selected = self.map_layer_names[self.current_map_layer] + if unload: + self.hud.notification('Unloading map layer: %s' % selected) + self.world.unload_map_layer(selected) + else: + self.hud.notification('Loading map layer: %s' % selected) + self.world.load_map_layer(selected) + + def toggle_radar(self): + if self.radar_sensor is None: + self.radar_sensor = RadarSensor(self.player) + elif self.radar_sensor.sensor is not None: + self.radar_sensor.sensor.destroy() + self.radar_sensor = None + + def modify_vehicle_physics(self, actor): + #If actor is not a vehicle, we cannot use the physics control + try: + physics_control = actor.get_physics_control() + physics_control.use_sweep_wheel_collision = True + actor.apply_physics_control(physics_control) + except Exception: + pass + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + if self.radar_sensor is not None: + self.toggle_radar() + sensors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + # self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.imu_sensor.sensor, + self.v2x_sensor.sensor] + for sensor in sensors: + if sensor is not None: + sensor.stop() + sensor.destroy() + if self.player is not None: + self.player.destroy() + + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + """Class that handles keyboard input.""" + def __init__(self, world, start_in_autopilot): + self._autopilot_enabled = start_in_autopilot + self._ackermann_enabled = False + self._ackermann_reverse = 1 + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + self._ackermann_control = carla.VehicleAckermannControl() + self._lights = carla.VehicleLightState.NONE + world.player.set_autopilot(self._autopilot_enabled) + world.player.set_light_state(self._lights) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self, client, world, clock, sync_mode): + if isinstance(self._control, carla.VehicleControl): + current_lights = self._lights + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + elif event.key == K_BACKSPACE: + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: + world.next_map_layer(reverse=True) + elif event.key == K_v: + world.next_map_layer() + elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: + world.load_map_layer(unload=True) + elif event.key == K_b: + world.load_map_layer() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: + world.next_weather(reverse=True) + elif event.key == K_c: + world.next_weather() + elif event.key == K_g: + world.toggle_radar() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key == K_n: + world.camera_manager.next_sensor() + elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): + if world.constant_velocity_enabled: + world.player.disable_constant_velocity() + world.constant_velocity_enabled = False + world.hud.notification("Disabled Constant Velocity Mode") + else: + world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) + world.constant_velocity_enabled = True + world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") + elif event.key == K_o: + try: + if world.doors_are_open: + world.hud.notification("Closing Doors") + world.doors_are_open = False + world.player.close_door(carla.VehicleDoor.All) + else: + world.hud.notification("Opening doors") + world.doors_are_open = True + world.player.open_door(carla.VehicleDoor.All) + except Exception: + pass + elif event.key == K_t: + if world.show_vehicle_telemetry: + world.player.show_debug_telemetry(False) + world.show_vehicle_telemetry = False + world.hud.notification("Disabled Vehicle Telemetry") + else: + try: + world.player.show_debug_telemetry(True) + world.show_vehicle_telemetry = True + world.hud.notification("Enabled Vehicle Telemetry") + except Exception: + pass + elif event.key > K_0 and event.key <= K_9: + index_ctrl = 0 + if pygame.key.get_mods() & KMOD_CTRL: + index_ctrl = 9 + world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) + elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): + world.camera_manager.toggle_recording() + elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): + if (world.recording_enabled): + client.stop_recorder() + world.recording_enabled = False + world.hud.notification("Recorder is OFF") + else: + client.start_recorder("manual_recording.rec") + world.recording_enabled = True + world.hud.notification("Recorder is ON") + elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): + # stop recorder + client.stop_recorder() + world.recording_enabled = False + # work around to fix camera at start of replaying + current_index = world.camera_manager.index + world.destroy_sensors() + # disable autopilot + self._autopilot_enabled = False + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification("Replaying file 'manual_recording.rec'") + # replayer + client.replay_file("manual_recording.rec", world.recording_start, 0, 0) + world.camera_manager.set_sensor(current_index) + elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): + if pygame.key.get_mods() & KMOD_SHIFT: + world.recording_start -= 10 + else: + world.recording_start -= 1 + world.hud.notification("Recording start time is %d" % (world.recording_start)) + elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): + if pygame.key.get_mods() & KMOD_SHIFT: + world.recording_start += 10 + else: + world.recording_start += 1 + world.hud.notification("Recording start time is %d" % (world.recording_start)) + if isinstance(self._control, carla.VehicleControl): + if event.key == K_f: + # Toggle ackermann controller + self._ackermann_enabled = not self._ackermann_enabled + world.hud.show_ackermann_info(self._ackermann_enabled) + world.hud.notification("Ackermann Controller %s" % + ("Enabled" if self._ackermann_enabled else "Disabled")) + if event.key == K_q: + if not self._ackermann_enabled: + self._control.gear = 1 if self._control.reverse else -1 + else: + self._ackermann_reverse *= -1 + # Reset ackermann control + self._ackermann_control = carla.VehicleAckermannControl() + elif event.key == K_m: + self._control.manual_gear_shift = not self._control.manual_gear_shift + self._control.gear = world.player.get_control().gear + world.hud.notification('%s Transmission' % + ('Manual' if self._control.manual_gear_shift else 'Automatic')) + elif self._control.manual_gear_shift and event.key == K_COMMA: + self._control.gear = max(-1, self._control.gear - 1) + elif self._control.manual_gear_shift and event.key == K_PERIOD: + self._control.gear = self._control.gear + 1 + elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL: + if not self._autopilot_enabled and not sync_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + self._autopilot_enabled = not self._autopilot_enabled + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification( + 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: + current_lights ^= carla.VehicleLightState.Special1 + elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT: + current_lights ^= carla.VehicleLightState.HighBeam + elif event.key == K_l: + # Use 'L' key to switch between lights: + # closed -> position -> low beam -> fog + if not self._lights & carla.VehicleLightState.Position: + world.hud.notification("Position lights") + current_lights |= carla.VehicleLightState.Position + else: + world.hud.notification("Low beam lights") + current_lights |= carla.VehicleLightState.LowBeam + if self._lights & carla.VehicleLightState.LowBeam: + world.hud.notification("Fog lights") + current_lights |= carla.VehicleLightState.Fog + if self._lights & carla.VehicleLightState.Fog: + world.hud.notification("Lights off") + current_lights ^= carla.VehicleLightState.Position + current_lights ^= carla.VehicleLightState.LowBeam + current_lights ^= carla.VehicleLightState.Fog + elif event.key == K_i: + current_lights ^= carla.VehicleLightState.Interior + elif event.key == K_z: + current_lights ^= carla.VehicleLightState.LeftBlinker + elif event.key == K_x: + current_lights ^= carla.VehicleLightState.RightBlinker + + if not self._autopilot_enabled: + if isinstance(self._control, carla.VehicleControl): + self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) + self._control.reverse = self._control.gear < 0 + # Set automatic control-related vehicle lights + if self._control.brake: + current_lights |= carla.VehicleLightState.Brake + else: # Remove the Brake flag + current_lights &= ~carla.VehicleLightState.Brake + if self._control.reverse: + current_lights |= carla.VehicleLightState.Reverse + else: # Remove the Reverse flag + current_lights &= ~carla.VehicleLightState.Reverse + if current_lights != self._lights: # Change the light state only if necessary + self._lights = current_lights + world.player.set_light_state(carla.VehicleLightState(self._lights)) + # Apply control + if not self._ackermann_enabled: + world.player.apply_control(self._control) + else: + world.player.apply_ackermann_control(self._ackermann_control) + # Update control to the last one applied by the ackermann controller. + self._control = world.player.get_control() + # Update hud with the newest ackermann control + world.hud.update_ackermann_control(self._ackermann_control) + + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) + world.player.apply_control(self._control) + + def _parse_vehicle_keys(self, keys, milliseconds): + if keys[K_UP] or keys[K_w]: + if not self._ackermann_enabled: + self._control.throttle = min(self._control.throttle + 0.01, 1.00) + else: + self._ackermann_control.speed += round(milliseconds * 0.005, 2) * self._ackermann_reverse + else: + if not self._ackermann_enabled: + self._control.throttle = 0.0 + + if keys[K_DOWN] or keys[K_s]: + if not self._ackermann_enabled: + self._control.brake = min(self._control.brake + 0.2, 1) + else: + self._ackermann_control.speed -= min(abs(self._ackermann_control.speed), round(milliseconds * 0.005, 2)) * self._ackermann_reverse + self._ackermann_control.speed = max(0, abs(self._ackermann_control.speed)) * self._ackermann_reverse + else: + if not self._ackermann_enabled: + self._control.brake = 0 + + steer_increment = 5e-4 * milliseconds + if keys[K_LEFT] or keys[K_a]: + if self._steer_cache > 0: + self._steer_cache = 0 + else: + self._steer_cache -= steer_increment + elif keys[K_RIGHT] or keys[K_d]: + if self._steer_cache < 0: + self._steer_cache = 0 + else: + self._steer_cache += steer_increment + else: + self._steer_cache = 0.0 + self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) + if not self._ackermann_enabled: + self._control.steer = round(self._steer_cache, 1) + self._control.hand_brake = keys[K_SPACE] + else: + self._ackermann_control.steer = round(self._steer_cache, 1) + + def _parse_walker_keys(self, keys, milliseconds, world): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @staticmethod + def _is_quit_shortcut(key): + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 16), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + self._show_ackermann_info = False + self._ackermann_control = carla.VehicleAckermannControl() + + def on_world_tick(self, timestamp): + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + self._notifications.tick(world, clock) + if not self._show_info: + return + t = world.player.get_transform() + v = world.player.get_velocity() + c = world.player.get_control() + compass = world.imu_sensor.compass + heading = 'N' if compass > 270.5 or compass < 89.5 else '' + heading += 'S' if 90.5 < compass < 269.5 else '' + heading += 'E' if 0.5 < compass < 179.5 else '' + heading += 'W' if 180.5 < compass < 359.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name.split('/')[-1], + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), + u'Compass:% 17.0f\N{DEGREE SIGN} % 2s' % (compass, heading), + 'Accelero: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.accelerometer), + 'Gyroscop: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.gyroscope), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % t.location.z, + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + if self._show_ackermann_info: + self._info_text += [ + '', + 'Ackermann Controller:', + ' Target speed: % 8.0f km/h' % (3.6*self._ackermann_control.speed), + ] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) + vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] + for d, vehicle in sorted(vehicles, key=lambda vehicles: vehicles[0]): + if d > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (d, vehicle_type)) + + def show_ackermann_info(self, enabled): + self._show_ackermann_info = enabled + + def update_ackermann_control(self, ackermann_control): + self._ackermann_control = ackermann_control + + def toggle_info(self): + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + f = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """Helper class to handle text output using pygame""" + def __init__(self, font, width, height): + lines = __doc__.split('\n') + self.font = font + self.line_space = 18 + self.dim = (780, len(lines) * self.line_space + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, n * self.line_space)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + self._render = not self._render + + def render(self, display): + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + + # If the spawn object is not a vehicle, we cannot use the Lane Invasion Sensor + if parent_actor.type_id.startswith("vehicle."): + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + self.hud.notification('Crossed line %s' % ' and '.join(text)) + + +# ============================================================================== +# -- GnssSensor ---------------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + + +# ============================================================================== +# -- IMUSensor ----------------------------------------------------------------- +# ============================================================================== + + +class IMUSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.accelerometer = (0.0, 0.0, 0.0) + self.gyroscope = (0.0, 0.0, 0.0) + self.compass = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.imu') + self.sensor = world.spawn_actor( + bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda sensor_data: IMUSensor._IMU_callback(weak_self, sensor_data)) + + @staticmethod + def _IMU_callback(weak_self, sensor_data): + self = weak_self() + if not self: + return + limits = (-99.9, 99.9) + self.accelerometer = ( + max(limits[0], min(limits[1], sensor_data.accelerometer.x)), + max(limits[0], min(limits[1], sensor_data.accelerometer.y)), + max(limits[0], min(limits[1], sensor_data.accelerometer.z))) + self.gyroscope = ( + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.x))), + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.y))), + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.z)))) + self.compass = math.degrees(sensor_data.compass) + +# ============================================================================== +# -- V2XSensor ----------------------------------------------------------------- +# ============================================================================== + + +class V2XSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._parent = parent_actor + world = self._parent.get_world() + #bp = world.get_blueprint_library().find('sensor.other.v2x_custom') + bp = world.get_blueprint_library().find('sensor.other.v2x') + bp.set_attribute("path_loss_model", "geometric") + self.hud = hud + self.sensor = world.spawn_actor( + bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda sensor_data: V2XSensor._V2X_callback(weak_self, sensor_data)) + + @staticmethod + def _V2X_callback(weak_self, sensor_data): + self = weak_self() + if not self: + return + print(sensor_data.get_message_count()) + for data in sensor_data: + msg = data.get() + # stationId = msg["Header"]["Station ID"] + power = data.power + print(msg) + # self.hud.notification('Cam message received from %s ' % stationId) + self.hud.notification('Cam message received with power %f ' % power) +# ============================================================================== +# -- RadarSensor --------------------------------------------------------------- +# ============================================================================== + + +class RadarSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + bound_x = 0.5 + self._parent.bounding_box.extent.x + bound_y = 0.5 + self._parent.bounding_box.extent.y + bound_z = 0.5 + self._parent.bounding_box.extent.z + + self.velocity_range = 7.5 # m/s + world = self._parent.get_world() + self.debug = world.debug + bp = world.get_blueprint_library().find('sensor.other.radar') + bp.set_attribute('horizontal_fov', str(35)) + bp.set_attribute('vertical_fov', str(20)) + self.sensor = world.spawn_actor( + bp, + carla.Transform( + carla.Location(x=bound_x + 0.05, z=bound_z+0.05), + carla.Rotation(pitch=5)), + attach_to=self._parent) + # We need a weak reference to self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda radar_data: RadarSensor._Radar_callback(weak_self, radar_data)) + + @staticmethod + def _Radar_callback(weak_self, radar_data): + self = weak_self() + if not self: + return + # To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]: + # points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) + # points = np.reshape(points, (len(radar_data), 4)) + + current_rot = radar_data.transform.rotation + for detect in radar_data: + azi = math.degrees(detect.azimuth) + alt = math.degrees(detect.altitude) + # The 0.25 adjusts a bit the distance so the dots can + # be properly seen + fw_vec = carla.Vector3D(x=detect.depth - 0.25) + carla.Transform( + carla.Location(), + carla.Rotation( + pitch=current_rot.pitch + alt, + yaw=current_rot.yaw + azi, + roll=current_rot.roll)).transform(fw_vec) + + def clamp(min_v, max_v, value): + return max(min_v, min(value, max_v)) + + norm_velocity = detect.velocity / self.velocity_range # range [-1, 1] + r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0) + g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0) + b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0) + self.debug.draw_point( + radar_data.transform.location + fw_vec, + size=0.075, + life_time=0.06, + persistent_lines=False, + color=carla.Color(r, g, b)) + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + def __init__(self, parent_actor, hud, gamma_correction): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_x = 0.5 + self._parent.bounding_box.extent.x + bound_y = 0.5 + self._parent.bounding_box.extent.y + bound_z = 0.5 + self._parent.bounding_box.extent.z + Attachment = carla.AttachmentType + + if not self._parent.type_id.startswith("walker.pedestrian"): + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-2.0*bound_x, y=+0.0*bound_y, z=2.0*bound_z), carla.Rotation(pitch=8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=+0.8*bound_x, y=+0.0*bound_y, z=1.3*bound_z)), Attachment.Rigid), + (carla.Transform(carla.Location(x=+1.9*bound_x, y=+1.0*bound_y, z=1.2*bound_z)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-2.8*bound_x, y=+0.0*bound_y, z=4.6*bound_z), carla.Rotation(pitch=6.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] + else: + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-2.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), + (carla.Transform(carla.Location(x=2.5, y=0.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-4.0, z=2.0), carla.Rotation(pitch=6.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=0, y=-2.5, z=-0.0), carla.Rotation(yaw=90.0)), Attachment.Rigid)] + + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.CityScapesPalette, 'Camera Instance Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.Raw, 'Camera Instance Segmentation (Raw)', {}], + # ['sensor.other.v2x', None, 'Lidar (Ray-Cast)', {'range': '50'}], + ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], + ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', + {'lens_circle_multiplier': '3.0', + 'lens_circle_falloff': '3.0', + 'chromatic_aberration_intensity': '0.5', + 'chromatic_aberration_offset': '0'}], + ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], + ['sensor.camera.normals', cc.Raw, 'Camera Normals', {}], + ] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + if bp.has_attribute('gamma'): + bp.set_attribute('gamma', str(gamma_correction)) + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + elif item[0].startswith('sensor.lidar'): + self.lidar_range = 50 + + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + if attr_name == 'range': + self.lidar_range = float(attr_value) + + item.append(bp) + self.index = None + + def toggle_camera(self): + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + index = index % len(self.sensors) + needs_respawn = True if self.index is None else \ + (force_respawn or (self.sensors[index][2] != self.sensors[self.index][2])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index][0], + attach_to=self._parent, + attachment_type=self._camera_transforms[self.transform_index][1]) + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self.hud.notification(self.sensors[index][2]) + self.index = index + + def next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 4), 4)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / (2.0 * self.lidar_range) + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + elif self.sensors[self.index][0].startswith('sensor.camera.dvs'): + # Example of converting the raw_data from a carla.DVSEventArray + # sensor into a NumPy array and using it as an image + dvs_events = np.frombuffer(image.raw_data, dtype=np.dtype([ + ('x', np.uint16), ('y', np.uint16), ('t', np.int64), ('pol', np.bool)])) + dvs_img = np.zeros((image.height, image.width, 3), dtype=np.uint8) + # Blue is positive, red is negative + dvs_img[dvs_events[:]['y'], dvs_events[:]['x'], dvs_events[:]['pol'] * 2] = 255 + self.surface = pygame.surfarray.make_surface(dvs_img.swapaxes(0, 1)) + elif self.sensors[self.index][0].startswith('sensor.camera.optical_flow'): + image = image.get_color_coded_flow() + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('_out/%08d' % image.frame) + + +# ============================================================================== +# -- game_loop() --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + original_settings = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2000.0) + + sim_world = client.get_world() + if args.sync: + original_settings = sim_world.get_settings() + settings = sim_world.get_settings() + if not settings.synchronous_mode: + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + sim_world.apply_settings(settings) + + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + + if args.autopilot and not sim_world.get_settings().synchronous_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + display.fill((0,0,0)) + pygame.display.flip() + # monitors = get_monitors() + # resolutions = [(monitor.name, monitor.width, monitor.height) for monitor in monitors] + # return resolutions + hud = HUD(args.width, args.height) + world = World(sim_world, hud, args) + controller = KeyboardControl(world, args.autopilot) + + if args.sync: + sim_world.tick() + else: + sim_world.wait_for_tick() + + clock = pygame.time.Clock() + while True: + if args.sync: + sim_world.tick() + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock, args.sync): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if original_settings: + sim_world.apply_settings(original_settings) + + if (world and world.recording_enabled): + client.stop_recorder() + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='actor filter (default: "vehicle.*")') + argparser.add_argument( + '--generation', + metavar='G', + default='2', + help='restrict to certain actor generation (values: "1","2","All" - default: "2")') + argparser.add_argument( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + argparser.add_argument( + '--sync', + action='store_true', + help='Activate synchronous mode execution') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/PythonAPI/examples/test_addsecondvx.py b/PythonAPI/examples/test_addsecondvx.py new file mode 100644 index 000000000..367508b14 --- /dev/null +++ b/PythonAPI/examples/test_addsecondvx.py @@ -0,0 +1,95 @@ +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/carla-0.9.15-py*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +import carla +import random +import weakref + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + +class V2XSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + world = self._parent.get_world() + #bp = world.get_blueprint_library().find('sensor.other.v2x_custom') + bp = world.get_blueprint_library().find('sensor.other.v2x') + self.sensor = world.spawn_actor( + bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda sensor_data: V2XSensor._V2X_callback(weak_self, sensor_data)) + + def destroy(self): + self.sensor.stop() + self.sensor.destroy() + + @staticmethod + def _V2X_callback(weak_self, sensor_data): + self = weak_self() + if not self: + return + for data in sensor_data: + msg = data.get() + # stationId = msg["Header"]["Station ID"] + power = data.power + print(msg) + # print('Cam message received from %s ' % stationId) + print('Cam message received with power %f ' % power) + +client = carla.Client("localhost",2000) +client.set_timeout(2000.0) + +world = client.get_world() +smap = world.get_map() +# acl = world.get_actor(28) +# acl.send("test") + + +spawn_points = smap.get_spawn_points() +spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() +blueprint = random.choice(get_actor_blueprints(world, "vehicle.*", "2")) +blueprint.set_attribute('role_name', "test") +player = world.try_spawn_actor(blueprint, spawn_point) +v2x_sensor = V2XSensor(player) + +world.wait_for_tick() +try: + while True: + world.wait_for_tick() +finally: + v2x_sensor.destroy() + player.destroy() \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp index 4899c7e29..2de3050a2 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp @@ -1029,7 +1029,6 @@ void UActorBlueprintFunctionLibrary::MakeV2XDefinition( FActorDefinition &Definition) { FillIdAndTags(Definition, TEXT("sensor"), TEXT("other"), TEXT("v2x")); - // AddRecommendedValuesForSensorRoleNames(Definition); AddVariationsForSensor(Definition); // - Noise seed -------------------------------- @@ -1263,7 +1262,6 @@ void UActorBlueprintFunctionLibrary::MakeCustomV2XDefinition( FActorDefinition &Definition) { FillIdAndTags(Definition, TEXT("sensor"), TEXT("other"), TEXT("v2x_custom")); - // AddRecommendedValuesForSensorRoleNames(Definition); AddVariationsForSensor(Definition); // - Noise seed -------------------------------- diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.cpp index 99d552a43..0c4ea10ef 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -12,42 +12,43 @@ #include static const float scLowFrequencyContainerInterval = 0.5; - ITSContainer::SpeedValue_t CaService::buildSpeedValue(const float vel) { - static const float lower = 0.0; // meter_per_second - static const float upper = 163.82; //meter_per_second + static const float lower = 0.0; // meter_per_second + static const float upper = 163.82; // meter_per_second - ITSContainer::SpeedValue_t speed = ITSContainer::SpeedValue_unavailable; - if (vel >= upper) { - speed = 16382; // see CDD A.74 (TS 102 894 v1.2.1) - } else if (vel >= lower) { - //to cm per second - speed = std::round(vel * 100.0) * ITSContainer::SpeedValue_oneCentimeterPerSec; - } - return speed; + ITSContainer::SpeedValue_t speed = ITSContainer::SpeedValue_unavailable; + if (vel >= upper) + { + speed = 16382; // see CDD A.74 (TS 102 894 v1.2.1) + } + else if (vel >= lower) + { + // to cm per second + speed = std::round(vel * 100.0) * ITSContainer::SpeedValue_oneCentimeterPerSec; + } + return speed; } -CaService::CaService(URandomEngine* random_engine) +CaService::CaService(URandomEngine *random_engine) { mRandomEngine = random_engine; // The starting point now std::chrono::system_clock::time_point start_Point = std::chrono::system_clock::now(); - //the reference point for ETSI (2004-01-01T00:00:00:000Z) + // the reference point for ETSI (2004-01-01T00:00:00:000Z) std::chrono::system_clock::time_point ref_point = std::chrono::system_clock::from_time_t(1072915200); // Unix time for 2004-01-01 - mGenerationDelta0 = std::chrono::duration_cast(start_Point - ref_point); + mGenerationDelta0 = std::chrono::duration_cast(start_Point - ref_point); } - -void CaService::SetOwner(UWorld* world, AActor *Owner) +void CaService::SetOwner(UWorld *world, AActor *Owner) { UE_LOG(LogCarla, Warning, TEXT("CaService:SetOwner function called")); mWorld = world; mCarlaEpisode = UCarlaStatics::GetCurrentEpisode(world); CurrentGeoReference = mCarlaEpisode->GetGeoReference(); - + mActorOwner = Owner; mCarlaEpisode = UCarlaStatics::GetCurrentEpisode(mWorld); mCarlaActor = mCarlaEpisode->FindCarlaActor(Owner); @@ -56,16 +57,15 @@ void CaService::SetOwner(UWorld* world, AActor *Owner) { mVehicle = Cast(Owner); - if(mCarlaActor->GetActorType() == FCarlaActor::ActorType::Vehicle) + if (mCarlaActor->GetActorType() == FCarlaActor::ActorType::Vehicle) { UE_LOG(LogCarla, Warning, TEXT("CaService:Initialize Vehicle type")); - + mLastCamTimeStamp = mCarlaEpisode->GetElapsedGameTime() - mGenCamMax; mLastLowCamTimeStamp = mCarlaEpisode->GetElapsedGameTime() - scLowFrequencyContainerInterval; - //Can add logic for this later + // Can add logic for this later mDccRestriction = false; - mElapsedTime = 0; VehicleSpeed = 0; VehiclePosition = {0, 0, 0}; @@ -73,10 +73,9 @@ void CaService::SetOwner(UWorld* world, AActor *Owner) mLastCamSpeed = 0; mLastCamPosition = {0, 0, 0}; mLastCamHeading = {0, 0, 0}; - } - else if((mCarlaActor->GetActorType() == FCarlaActor::ActorType::TrafficLight)|| - (mCarlaActor->GetActorType() == FCarlaActor::ActorType::TrafficSign)) + else if ((mCarlaActor->GetActorType() == FCarlaActor::ActorType::TrafficLight) || + (mCarlaActor->GetActorType() == FCarlaActor::ActorType::TrafficSign)) { mGenerationInterval = 0.5; mLastCamTimeStamp = -mGenerationInterval; @@ -85,36 +84,32 @@ void CaService::SetOwner(UWorld* world, AActor *Owner) mStationId = static_cast(mCarlaActor->GetActorId()); mStationType = GetStationType(); - } - } - void CaService::SetParams(const float GenCamMin, const float GenCamMax, const bool FixedRate) { UE_LOG(LogCarla, Warning, TEXT("CaService:SetParams function called")); - //Max and Min for generation rate + // Max and Min for generation rate - mGenCamMin = GenCamMin; //in second - mGenCamMax = GenCamMax; //in second + mGenCamMin = GenCamMin; // in second + mGenCamMax = GenCamMax; // in second mGenCam = mGenCamMax; - //If we want set a fix interval make this as true + // If we want set a fix interval make this as true mFixedRate = FixedRate; - } /* -* Function to check trigger condition for RSU -*/ + * Function to check trigger condition for RSU + */ bool CaService::Trigger(float DeltaSeconds) { - + mElapsedTime = mCarlaEpisode->GetElapsedGameTime(); bool Trigger = false; - if(mStationType == ITSContainer::StationType_roadSideUnit) + if (mStationType == ITSContainer::StationType_roadSideUnit) { - if (mElapsedTime - mLastCamTimeStamp >= mGenerationInterval) + if (mElapsedTime - mLastCamTimeStamp >= mGenerationInterval) { Trigger = true; mCAMMessage = CreateCooperativeAwarenessMessage(DeltaSeconds); @@ -124,24 +119,22 @@ bool CaService::Trigger(float DeltaSeconds) else { Trigger = CheckTriggeringConditions(DeltaSeconds); - } return Trigger; - } /* -* Function to provide CAM message to other objects if necessary -*/ + * Function to provide CAM message to other objects if necessary + */ CAM_t CaService::GetCamMessage() { return mCAMMessage; } /* -* Check the trigger condition in case of vehicles and if trigger is true request -* to generate CAM message -*/ + * Check the trigger condition in case of vehicles and if trigger is true request + * to generate CAM message + */ bool CaService::CheckTriggeringConditions(float DeltaSeconds) { float &T_GenCam = mGenCam; @@ -149,10 +142,10 @@ bool CaService::CheckTriggeringConditions(float DeltaSeconds) const float T_GenCamMax = mGenCamMax; const float T_GenCamDcc = mDccRestriction ? 0 : T_GenCamMin; const float T_elapsed = mElapsedTime - mLastCamTimeStamp; - if(T_elapsed >= T_GenCamDcc) + if (T_elapsed >= T_GenCamDcc) { - //If message need to be generated every sim tick then set this to true. - if(mFixedRate) + // If message need to be generated every sim tick then set this to true. + if (mFixedRate) { GenerateCamMessage(DeltaSeconds); return true; @@ -162,15 +155,16 @@ bool CaService::CheckTriggeringConditions(float DeltaSeconds) { GenerateCamMessage(DeltaSeconds); T_GenCam = std::min(T_elapsed, T_GenCamMax); - mGenCamLowDynamicsCounter = 0; + mGenCamLowDynamicsCounter = 0; return true; } else if (T_elapsed >= T_GenCam) { GenerateCamMessage(DeltaSeconds); - if (++mGenCamLowDynamicsCounter >= mGenCamLowDynamicsLimit) { - T_GenCam = T_GenCamMax; - } + if (++mGenCamLowDynamicsCounter >= mGenCamLowDynamicsLimit) + { + T_GenCam = T_GenCamMax; + } return true; } } @@ -179,10 +173,10 @@ bool CaService::CheckTriggeringConditions(float DeltaSeconds) bool CaService::CheckPositionDelta(float DeltaSeconds) { - //If position change is more the 4m + // If position change is more the 4m VehiclePosition = mVehicle->GetActorLocation(); - double Distance = FVector::Distance(VehiclePosition, mLastCamPosition) / 100.0f; //From cm to m - if(Distance > 4.0f) + double Distance = FVector::Distance(VehiclePosition, mLastCamPosition) / 100.0f; // From cm to m + if (Distance > 4.0f) { return true; } @@ -194,8 +188,8 @@ bool CaService::CheckSpeedDelta(float DeltaSeconds) VehicleSpeed = mVehicle->GetVehicleForwardSpeed() / 100.0f; // From cm/s to m/s float DeltaSpeed = std::abs(VehicleSpeed - mLastCamSpeed); - //Speed differance is greater than 0.5m/s - if(DeltaSpeed > 0.5) + // Speed differance is greater than 0.5m/s + if (DeltaSpeed > 0.5) { return true; } @@ -204,7 +198,7 @@ bool CaService::CheckSpeedDelta(float DeltaSeconds) } double CaService::GetFVectorAngle(const FVector &a, const FVector &b) -{ +{ double Dot = a.X * b.X + a.Y * b.Y + a.Z * b.Z; return std::acos(Dot / (a.Size() * b.Size())); } @@ -218,108 +212,106 @@ void CaService::GenerateCamMessage(float DeltaTime) mLastCamTimeStamp = mElapsedTime; } -//Function to get the station type +// Function to get the station type ITSContainer::StationType_t CaService::GetStationType() { check(mActorOwner != nullptr); mCarlaEpisode = UCarlaStatics::GetCurrentEpisode(mWorld); mCarlaActor = mCarlaEpisode->FindCarlaActor(mActorOwner); ITSContainer::StationType_t stationType = ITSContainer::StationType_unknown; - //return unknown if carla actor is gone - if(mCarlaActor == nullptr) + // return unknown if carla actor is gone + if (mCarlaActor == nullptr) { return static_cast(stationType); } auto Tag = ATagger::GetTagOfTaggedComponent(*mVehicle->GetMesh()); - - switch(Tag){ - case crp::CityObjectLabel::None: - stationType = ITSContainer::StationType_unknown; - break; - case crp::CityObjectLabel::Pedestrians: - stationType = ITSContainer::StationType_pedestrian; - break; - case crp::CityObjectLabel::Bicycle: - stationType = ITSContainer::StationType_cyclist; - break; - case crp::CityObjectLabel::Motorcycle: - stationType = ITSContainer::StationType_motorcycle; - break; - case crp::CityObjectLabel::Car: - stationType = ITSContainer::StationType_passengerCar; - break; - case crp::CityObjectLabel::Bus: - stationType = ITSContainer::StationType_bus; - break; - //TODO Modify this in future is CARLA adds difference truck - case crp::CityObjectLabel::Truck: - stationType = ITSContainer::StationType_lightTruck; - break; - case crp::CityObjectLabel::Buildings: - case crp::CityObjectLabel::Walls: - case crp::CityObjectLabel::Fences: - case crp::CityObjectLabel::Poles: - case crp::CityObjectLabel::TrafficLight: - case crp::CityObjectLabel::TrafficSigns: - stationType = ITSContainer::StationType_roadSideUnit; - break; - case crp::CityObjectLabel::Train: - stationType = ITSContainer::StationType_tram; - break; - default: - stationType = ITSContainer::StationType_unknown; + + switch (Tag) + { + case crp::CityObjectLabel::None: + stationType = ITSContainer::StationType_unknown; + break; + case crp::CityObjectLabel::Pedestrians: + stationType = ITSContainer::StationType_pedestrian; + break; + case crp::CityObjectLabel::Bicycle: + stationType = ITSContainer::StationType_cyclist; + break; + case crp::CityObjectLabel::Motorcycle: + stationType = ITSContainer::StationType_motorcycle; + break; + case crp::CityObjectLabel::Car: + stationType = ITSContainer::StationType_passengerCar; + break; + case crp::CityObjectLabel::Bus: + stationType = ITSContainer::StationType_bus; + break; + // TODO Modify this in future is CARLA adds difference truck + case crp::CityObjectLabel::Truck: + stationType = ITSContainer::StationType_lightTruck; + break; + case crp::CityObjectLabel::Buildings: + case crp::CityObjectLabel::Walls: + case crp::CityObjectLabel::Fences: + case crp::CityObjectLabel::Poles: + case crp::CityObjectLabel::TrafficLight: + case crp::CityObjectLabel::TrafficSigns: + stationType = ITSContainer::StationType_roadSideUnit; + break; + case crp::CityObjectLabel::Train: + stationType = ITSContainer::StationType_tram; + break; + default: + stationType = ITSContainer::StationType_unknown; } - //Can improve this later for different special vehicles once carla implements it + // Can improve this later for different special vehicles once carla implements it FCarlaActor::ActorType Type = mCarlaActor->GetActorType(); if (Type == FCarlaActor::ActorType::Vehicle) { - if(mCarlaActor->GetActorInfo()->Description.Variations.Contains("special_type")) + if (mCarlaActor->GetActorInfo()->Description.Variations.Contains("special_type")) { std::string special_type = carla::rpc::FromFString(*mCarlaActor->GetActorInfo()->Description.Variations["special_type"].Value); - if(special_type.compare("emergency") == 0 ) + if (special_type.compare("emergency") == 0) { stationType = ITSContainer::StationType_specialVehicles; } } - } return static_cast(stationType); } FVector CaService::GetReferencePosition() { - FVector RefPos; - carla::geom::Location ActorLocation = mActorOwner->GetActorLocation(); - ALargeMapManager * LargeMap = UCarlaStatics::GetLargeMapManager(mWorld); - if (LargeMap) - { - ActorLocation = LargeMap->LocalToGlobalLocation(ActorLocation); - } - carla::geom::Location Location = ActorLocation; - carla::geom::GeoLocation CurrentLocation = CurrentGeoReference.Transform(Location); + FVector RefPos; + carla::geom::Location ActorLocation = mActorOwner->GetActorLocation(); + ALargeMapManager *LargeMap = UCarlaStatics::GetLargeMapManager(mWorld); + if (LargeMap) + { + ActorLocation = LargeMap->LocalToGlobalLocation(ActorLocation); + } + carla::geom::Location Location = ActorLocation; + carla::geom::GeoLocation CurrentLocation = CurrentGeoReference.Transform(Location); - // Compute the noise for the sensor - const float LatError = mRandomEngine->GetNormalDistribution(0.0f, LatitudeDeviation); - const float LonError = mRandomEngine->GetNormalDistribution(0.0f, LongitudeDeviation); - const float AltError = mRandomEngine->GetNormalDistribution(0.0f, AltitudeDeviation); + // Compute the noise for the sensor + const float LatError = mRandomEngine->GetNormalDistribution(0.0f, LatitudeDeviation); + const float LonError = mRandomEngine->GetNormalDistribution(0.0f, LongitudeDeviation); + const float AltError = mRandomEngine->GetNormalDistribution(0.0f, AltitudeDeviation); - // Apply the noise to the sensor - double Latitude = CurrentLocation.latitude + LatitudeBias + LatError; - double Longitude = CurrentLocation.longitude + LongitudeBias + LonError; - double Altitude = CurrentLocation.altitude + AltitudeBias + AltError; - - RefPos.X = Latitude; - RefPos.Y = Longitude; - RefPos.Z = Altitude; - return RefPos; + // Apply the noise to the sensor + double Latitude = CurrentLocation.latitude + LatitudeBias + LatError; + double Longitude = CurrentLocation.longitude + LongitudeBias + LonError; + double Altitude = CurrentLocation.altitude + AltitudeBias + AltError; + RefPos.X = Latitude; + RefPos.Y = Longitude; + RefPos.Z = Altitude; + return RefPos; } - float CaService::GetHeading() { - // Magnetometer: orientation with respect to the North in rad + // Magnetometer: orientation with respect to the North in rad const FVector CarlaNorthVector = FVector(0.0f, -1.0f, 0.0f); const FVector ForwVect = mActorOwner->GetActorForwardVector().GetSafeNormal2D(); const float DotProd = FVector::DotProduct(CarlaNorthVector, ForwVect); @@ -337,33 +329,33 @@ float CaService::GetHeading() // Compute the noise for the sensor const float HeadingError = mRandomEngine->GetNormalDistribution(0.0f, HeadingDeviation); - - //add errors + + // add errors return HeadingDegree + HeadingBias + HeadingError; } -//Function to get the vehicle role +// Function to get the vehicle role long CaService::GetVehicleRole() { - long VehicleRole = ITSContainer::VehicleRole_default; + long VehicleRole = ITSContainer::VehicleRole_default; long StationType = GetStationType(); switch (StationType) { - case ITSContainer::StationType_cyclist: - case ITSContainer::StationType_moped: - case ITSContainer::StationType_motorcycle: - VehicleRole = ITSContainer::VehicleRole_default; - break; - case ITSContainer::StationType_bus: - case ITSContainer::StationType_tram: - VehicleRole = ITSContainer::VehicleRole_publicTransport; - break; - case ITSContainer::StationType_specialVehicles: - VehicleRole = ITSContainer::VehicleRole_emergency; - break; - default: - VehicleRole = ITSContainer::VehicleRole_default; - break; + case ITSContainer::StationType_cyclist: + case ITSContainer::StationType_moped: + case ITSContainer::StationType_motorcycle: + VehicleRole = ITSContainer::VehicleRole_default; + break; + case ITSContainer::StationType_bus: + case ITSContainer::StationType_tram: + VehicleRole = ITSContainer::VehicleRole_publicTransport; + break; + case ITSContainer::StationType_specialVehicles: + VehicleRole = ITSContainer::VehicleRole_emergency; + break; + default: + VehicleRole = ITSContainer::VehicleRole_default; + break; } return VehicleRole; } @@ -380,33 +372,31 @@ CAM_t CaService::CreateCooperativeAwarenessMessage(float DeltaTime) void CaService::CreateITSPduHeader(CAM_t &message) { - ITSContainer::ItsPduHeader_t& header = message.header; + ITSContainer::ItsPduHeader_t &header = message.header; header.protocolVersion = mProtocolVersion; header.messageID = mMessageId; header.stationID = mStationId; } -void CaService::AddCooperativeAwarenessMessage(CAMContainer::CoopAwareness_t& CoopAwarenessMessage, float DeltaTime) +void CaService::AddCooperativeAwarenessMessage(CAMContainer::CoopAwareness_t &CoopAwarenessMessage, float DeltaTime) { /* GenerationDeltaTime */ auto genDeltaTime = mGenerationDelta0 + std::chrono::milliseconds(static_cast(mCarlaEpisode->GetElapsedGameTime() * 1000)); - CoopAwarenessMessage.generationDeltaTime = genDeltaTime.count() % 65536 * CAMContainer::GenerationDeltaTime_oneMilliSec; //TODOCheck this logic + CoopAwarenessMessage.generationDeltaTime = genDeltaTime.count() % 65536 * CAMContainer::GenerationDeltaTime_oneMilliSec; // TODOCheck this logic AddBasicContainer(CoopAwarenessMessage.camParameters.basicContainer); - if(CoopAwarenessMessage.camParameters.basicContainer.stationType == ITSContainer::StationType_roadSideUnit) + if (CoopAwarenessMessage.camParameters.basicContainer.stationType == ITSContainer::StationType_roadSideUnit) { - //TODO Future Implementation + // TODO Future Implementation AddRSUContainerHighFrequency(CoopAwarenessMessage.camParameters.highFrequencyContainer); - } - else if(CoopAwarenessMessage.camParameters.basicContainer.stationType == ITSContainer::StationType_pedestrian) + else if (CoopAwarenessMessage.camParameters.basicContainer.stationType == ITSContainer::StationType_pedestrian) { - //TODO no container available for Pedestrains - + // TODO no container available for Pedestrains } else { - //BasicVehicleContainer + // BasicVehicleContainer AddBasicVehicleContainerHighFrequency(CoopAwarenessMessage.camParameters.highFrequencyContainer, DeltaTime); if (mElapsedTime - mLastLowCamTimeStamp >= scLowFrequencyContainerInterval) @@ -416,21 +406,18 @@ void CaService::AddCooperativeAwarenessMessage(CAMContainer::CoopAwareness_t& Co } else { - //Store nothing if not used + // Store nothing if not used CoopAwarenessMessage.camParameters.lowFrequencyContainer.present = CAMContainer::LowFrequencyContainer_PR_NOTHING; } /* - *TODO Add Special container if it a special vehicle - */ - + *TODO Add Special container if it a special vehicle + */ } - - } void CaService::AddBasicContainer(CAMContainer::BasicContainer_t &BasicContainer) { - BasicContainer.stationType = mStationType; + BasicContainer.stationType = mStationType; /* CamParameters ReferencePosition */ FVector RefPos = GetReferencePosition(); @@ -448,15 +435,14 @@ void CaService::SetAccelerationStandardDeviation(const FVector &Vec) StdDevAccel = Vec; } - void CaService::SetGNSSDeviation(const float noise_lat_stddev, - const float noise_lon_stddev, - const float noise_alt_stddev, - const float noise_head_stddev, - const float noise_lat_bias, - const float noise_lon_bias, - const float noise_alt_bias, - const float noise_head_bias) + const float noise_lon_stddev, + const float noise_alt_stddev, + const float noise_head_stddev, + const float noise_lat_bias, + const float noise_lon_bias, + const float noise_alt_bias, + const float noise_head_bias) { LatitudeDeviation = noise_lat_stddev; LongitudeDeviation = noise_lon_stddev; @@ -468,86 +454,93 @@ void CaService::SetGNSSDeviation(const float noise_lat_stddev, HeadingBias = noise_head_bias; } - - void CaService::SetVelDeviation(const float noise_vel_stddev_x) - { +void CaService::SetVelDeviation(const float noise_vel_stddev_x) +{ VelocityDeviation = noise_vel_stddev_x; - } +} - void CaService::SetYawrateDeviation(const float noise_yawrate_stddev, const float noise_yawrate_bias) - { +void CaService::SetYawrateDeviation(const float noise_yawrate_stddev, const float noise_yawrate_bias) +{ YawrateDeviation = noise_yawrate_stddev; YawrateBias = noise_yawrate_bias; - } - +} void CaService::AddBasicVehicleContainerHighFrequency(CAMContainer::HighFrequencyContainer_t &hfc, float DeltaTime) { hfc.present = CAMContainer::HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - CAMContainer::BasicVehicleContainerHighFrequency_t& bvc = hfc.basicVehicleContainerHighFrequency; - //heading - bvc.heading.headingValue = std::round(GetHeading() * 10.0); - bvc.heading.headingConfidence = ITSContainer::HeadingConfidence_equalOrWithinOneDegree; //TODO - //speed - //speed with noise - bvc.speed.speedValue = buildSpeedValue(ComputeSpeed()); - bvc.speed.speedConfidence = ITSContainer::SpeedConfidence_equalOrWithInOneCentimerterPerSec * 3; //TODO - //direction - bvc.driveDirection = (mVehicle->GetVehicleForwardSpeed() / 100.0f) >= 0.0 ? ITSContainer::DriveDirection_forward : ITSContainer::DriveDirection_backward; - //length and width - auto bb = UBoundingBoxCalculator::GetActorBoundingBox(mActorOwner); //cm - float length = bb.Extent.X *2.0; //half box - float width = bb.Extent.Y * 2.0; //half box + CAMContainer::BasicVehicleContainerHighFrequency_t &bvc = hfc.basicVehicleContainerHighFrequency; + // heading + bvc.heading.headingValue = std::round(GetHeading() * 10.0); + bvc.heading.headingConfidence = ITSContainer::HeadingConfidence_equalOrWithinOneDegree; // TODO + // speed + // speed with noise + bvc.speed.speedValue = buildSpeedValue(ComputeSpeed()); + bvc.speed.speedConfidence = ITSContainer::SpeedConfidence_equalOrWithInOneCentimerterPerSec * 3; // TODO + // direction + bvc.driveDirection = (mVehicle->GetVehicleForwardSpeed() / 100.0f) >= 0.0 ? ITSContainer::DriveDirection_forward : ITSContainer::DriveDirection_backward; + // length and width + auto bb = UBoundingBoxCalculator::GetActorBoundingBox(mActorOwner); // cm + float length = bb.Extent.X * 2.0; // half box + float width = bb.Extent.Y * 2.0; // half box - bvc.vehicleLength.vehicleLengthValue = std::round(length * 10.0); //0.1 meter + bvc.vehicleLength.vehicleLengthValue = std::round(length * 10.0); // 0.1 meter bvc.vehicleLength.vehicleLengthConfidenceIndication = ITSContainer::VehicleLengthConfidenceIndication_unavailable; - bvc.vehicleWidth = std::round(width * 10.0); //0.1 meter - - //acceleration - carla::geom::Vector3D Accel = ComputeAccelerometer(DeltaTime); + bvc.vehicleWidth = std::round(width * 10.0); // 0.1 meter + + // acceleration + carla::geom::Vector3D Accel = ComputeAccelerometer(DeltaTime); const double lonAccelValue = Accel.x * 10.0; // m/s to 0.1 m/s - // limit changes - if (lonAccelValue >= -160.0 && lonAccelValue <= 161.0) { - bvc.longitudinalAcceleration.longitudinalAccelerationValue = std::round(lonAccelValue) * ITSContainer::LongitudinalAccelerationValue_pointOneMeterPerSecSquaredForward; - } else { - bvc.longitudinalAcceleration.longitudinalAccelerationValue = ITSContainer::LongitudinalAccelerationValue_unavailable; - } - bvc.longitudinalAcceleration.longitudinalAccelerationConfidence = ITSContainer::AccelerationConfidence_unavailable; //TODO + // limit changes + if (lonAccelValue >= -160.0 && lonAccelValue <= 161.0) + { + bvc.longitudinalAcceleration.longitudinalAccelerationValue = std::round(lonAccelValue) * ITSContainer::LongitudinalAccelerationValue_pointOneMeterPerSecSquaredForward; + } + else + { + bvc.longitudinalAcceleration.longitudinalAccelerationValue = ITSContainer::LongitudinalAccelerationValue_unavailable; + } + bvc.longitudinalAcceleration.longitudinalAccelerationConfidence = ITSContainer::AccelerationConfidence_unavailable; // TODO - - //curvature TODO - bvc.curvature.curvatureValue = ITSContainer::CurvatureValue_unavailable; + // curvature TODO + bvc.curvature.curvatureValue = ITSContainer::CurvatureValue_unavailable; bvc.curvature.curvatureConfidence = ITSContainer::CurvatureConfidence_unavailable; bvc.curvatureCalculationMode = ITSContainer::CurvatureCalculationMode_yarRateUsed; - - //yaw rate is in rad/s --> to centidegree per second - bvc.yawRate.yawRateValue = std::round( carla::geom::Math::ToDegrees(ComputeYawRate()) * 100.0) * ITSContainer::YawRateValue_degSec_000_01ToLeft; - if (bvc.yawRate.yawRateValue < -32766 || bvc.yawRate.yawRateValue > 32766) { - bvc.yawRate.yawRateValue = ITSContainer::YawRateValue_unavailable; - } - bvc.yawRate.yawRateConfidence = ITSContainer::YawRateConfidence_unavailable; //TODO - //optional lat and vertical accelerations + // yaw rate is in rad/s --> to centidegree per second + bvc.yawRate.yawRateValue = std::round(carla::geom::Math::ToDegrees(ComputeYawRate()) * 100.0) * ITSContainer::YawRateValue_degSec_000_01ToLeft; + if (bvc.yawRate.yawRateValue < -32766 || bvc.yawRate.yawRateValue > 32766) + { + bvc.yawRate.yawRateValue = ITSContainer::YawRateValue_unavailable; + } + bvc.yawRate.yawRateConfidence = ITSContainer::YawRateConfidence_unavailable; // TODO + + // optional lat and vertical accelerations bvc.lateralAccelerationAvailable = true; const double latAccelValue = Accel.y * 10.0; // m/s to 0.1 m/s - if (latAccelValue >= -160.0 && latAccelValue <= 161.0) { - bvc.lateralAcceleration.lateralAccelerationValue = std::round(latAccelValue) * ITSContainer::LateralAccelerationValue_pointOneMeterPerSecSquaredToLeft; - } else { - bvc.lateralAcceleration.lateralAccelerationValue = ITSContainer::LateralAccelerationValue_unavailable; - } - bvc.lateralAcceleration.lateralAccelerationConfidence = ITSContainer::AccelerationConfidence_unavailable; //TODO - + if (latAccelValue >= -160.0 && latAccelValue <= 161.0) + { + bvc.lateralAcceleration.lateralAccelerationValue = std::round(latAccelValue) * ITSContainer::LateralAccelerationValue_pointOneMeterPerSecSquaredToLeft; + } + else + { + bvc.lateralAcceleration.lateralAccelerationValue = ITSContainer::LateralAccelerationValue_unavailable; + } + bvc.lateralAcceleration.lateralAccelerationConfidence = ITSContainer::AccelerationConfidence_unavailable; // TODO + bvc.verticalAccelerationAvailable = true; const double vertAccelValue = Accel.z * 10.0; // m/s to 0.1 m/s - if (vertAccelValue >= -160.0 && vertAccelValue <= 161.0) { - bvc.verticalAcceleration.verticalAccelerationValue = std::round(vertAccelValue) * ITSContainer::VerticalAccelerationValue_pointOneMeterPerSecSquaredUp; - } else { - bvc.verticalAcceleration.verticalAccelerationValue = ITSContainer::VerticalAccelerationValue_unavailable; - } - bvc.verticalAcceleration.verticalAccelerationConfidence = ITSContainer::AccelerationConfidence_unavailable; //TODO + if (vertAccelValue >= -160.0 && vertAccelValue <= 161.0) + { + bvc.verticalAcceleration.verticalAccelerationValue = std::round(vertAccelValue) * ITSContainer::VerticalAccelerationValue_pointOneMeterPerSecSquaredUp; + } + else + { + bvc.verticalAcceleration.verticalAccelerationValue = ITSContainer::VerticalAccelerationValue_unavailable; + } + bvc.verticalAcceleration.verticalAccelerationConfidence = ITSContainer::AccelerationConfidence_unavailable; // TODO - //TODO + // TODO bvc.accelerationControlAvailable = false; bvc.lanePositionAvailable = false; bvc.steeringWheelAngleAvailable = false; @@ -558,62 +551,60 @@ void CaService::AddBasicVehicleContainerHighFrequency(CAMContainer::HighFrequenc const carla::geom::Vector3D CaService::ComputeAccelerometerNoise( const FVector &Accelerometer) { - // Normal (or Gaussian or Gauss) distribution will be used as noise function. - // A mean of 0.0 is used as a first parameter, the standard deviation is - // determined by the client - constexpr float Mean = 0.0f; - return carla::geom::Vector3D { - Accelerometer.X + mRandomEngine->GetNormalDistribution(Mean, StdDevAccel.X), - Accelerometer.Y + mRandomEngine->GetNormalDistribution(Mean, StdDevAccel.Y), - Accelerometer.Z + mRandomEngine->GetNormalDistribution(Mean, StdDevAccel.Z) - }; + // Normal (or Gaussian or Gauss) distribution will be used as noise function. + // A mean of 0.0 is used as a first parameter, the standard deviation is + // determined by the client + constexpr float Mean = 0.0f; + return carla::geom::Vector3D{ + Accelerometer.X + mRandomEngine->GetNormalDistribution(Mean, StdDevAccel.X), + Accelerometer.Y + mRandomEngine->GetNormalDistribution(Mean, StdDevAccel.Y), + Accelerometer.Z + mRandomEngine->GetNormalDistribution(Mean, StdDevAccel.Z)}; } carla::geom::Vector3D CaService::ComputeAccelerometer( const float DeltaTime) { - // Used to convert from UE4's cm to meters - constexpr float TO_METERS = 1e-2; - // Earth's gravitational acceleration is approximately 9.81 m/s^2 - constexpr float GRAVITY = 9.81f; + // Used to convert from UE4's cm to meters + constexpr float TO_METERS = 1e-2; + // Earth's gravitational acceleration is approximately 9.81 m/s^2 + constexpr float GRAVITY = 9.81f; - // 2nd derivative of the polynomic (quadratic) interpolation - // using the point in current time and two previous steps: - // d2[i] = -2.0*(y1/(h1*h2)-y2/((h2+h1)*h2)-y0/(h1*(h2+h1))) - const FVector CurrentLocation = mVehicle->GetActorLocation(); + // 2nd derivative of the polynomic (quadratic) interpolation + // using the point in current time and two previous steps: + // d2[i] = -2.0*(y1/(h1*h2)-y2/((h2+h1)*h2)-y0/(h1*(h2+h1))) + const FVector CurrentLocation = mVehicle->GetActorLocation(); - const FVector Y2 = PrevLocation[0]; - const FVector Y1 = PrevLocation[1]; - const FVector Y0 = CurrentLocation; - const float H1 = DeltaTime; - const float H2 = PrevDeltaTime; + const FVector Y2 = PrevLocation[0]; + const FVector Y1 = PrevLocation[1]; + const FVector Y0 = CurrentLocation; + const float H1 = DeltaTime; + const float H2 = PrevDeltaTime; - const float H1AndH2 = H2 + H1; - const FVector A = Y1 / ( H1 * H2 ); - const FVector B = Y2 / ( H2 * (H1AndH2) ); - const FVector C = Y0 / ( H1 * (H1AndH2) ); - FVector FVectorAccelerometer = TO_METERS * -2.0f * ( A - B - C ); + const float H1AndH2 = H2 + H1; + const FVector A = Y1 / (H1 * H2); + const FVector B = Y2 / (H2 * (H1AndH2)); + const FVector C = Y0 / (H1 * (H1AndH2)); + FVector FVectorAccelerometer = TO_METERS * -2.0f * (A - B - C); - // Update the previous locations - PrevLocation[0] = PrevLocation[1]; - PrevLocation[1] = CurrentLocation; - PrevDeltaTime = DeltaTime; + // Update the previous locations + PrevLocation[0] = PrevLocation[1]; + PrevLocation[1] = CurrentLocation; + PrevDeltaTime = DeltaTime; - // Add gravitational acceleration - FVectorAccelerometer.Z += GRAVITY; + // Add gravitational acceleration + FVectorAccelerometer.Z += GRAVITY; - FQuat ImuRotation = mActorOwner->GetRootComponent()->GetComponentTransform().GetRotation(); - FVectorAccelerometer = ImuRotation.UnrotateVector(FVectorAccelerometer); + FQuat ImuRotation = mActorOwner->GetRootComponent()->GetComponentTransform().GetRotation(); + FVectorAccelerometer = ImuRotation.UnrotateVector(FVectorAccelerometer); - // Cast from FVector to our Vector3D to correctly send the data in m/s^2 - // and apply the desired noise function, in this case a normal distribution - const carla::geom::Vector3D Accelerometer = - ComputeAccelerometerNoise(FVectorAccelerometer); + // Cast from FVector to our Vector3D to correctly send the data in m/s^2 + // and apply the desired noise function, in this case a normal distribution + const carla::geom::Vector3D Accelerometer = + ComputeAccelerometerNoise(FVectorAccelerometer); - return Accelerometer; + return Accelerometer; } - float CaService::ComputeSpeed() { @@ -624,44 +615,45 @@ float CaService::ComputeSpeed() // A mean of 0.0 is used as a first parameter.The standard deviation and the // bias are determined by the client constexpr float Mean = 0.0f; - return boost::algorithm::clamp(speed + mRandomEngine->GetNormalDistribution(Mean, VelocityDeviation),0.0f,std::numeric_limits::max()); + return boost::algorithm::clamp(speed + mRandomEngine->GetNormalDistribution(Mean, VelocityDeviation), 0.0f, std::numeric_limits::max()); } float CaService::ComputeYawRate() { - check(mActorOwner != nullptr); - const FVector AngularVelocity = - FIMU_GetActorAngularVelocityInRadians(*mActorOwner); + check(mActorOwner != nullptr); + const FVector AngularVelocity = + FIMU_GetActorAngularVelocityInRadians(*mActorOwner); - const FQuat SensorLocalRotation = - mActorOwner->GetRootComponent()->GetRelativeTransform().GetRotation(); + const FQuat SensorLocalRotation = + mActorOwner->GetRootComponent()->GetRelativeTransform().GetRotation(); - const FVector FVectorGyroscope = - SensorLocalRotation.RotateVector(AngularVelocity); + const FVector FVectorGyroscope = + SensorLocalRotation.RotateVector(AngularVelocity); - // Cast from FVector to our Vector3D to correctly send the data in rad/s - // and apply the desired noise function, in this case a normal distribution - float yawrate = - ComputeYawNoise(FVectorGyroscope); + // Cast from FVector to our Vector3D to correctly send the data in rad/s + // and apply the desired noise function, in this case a normal distribution + float yawrate = + ComputeYawNoise(FVectorGyroscope); - return yawrate; //rad/s + return yawrate; // rad/s } const float CaService::ComputeYawNoise( const FVector &Gyroscope) { - // Normal (or Gaussian or Gauss) distribution and a bias will be used as - // noise function. - // A mean of 0.0 is used as a first parameter.The standard deviation and the - // bias are determined by the client - constexpr float Mean = 0.0f; - return Gyroscope.Z + YawrateBias + mRandomEngine->GetNormalDistribution(Mean, YawrateDeviation); + // Normal (or Gaussian or Gauss) distribution and a bias will be used as + // noise function. + // A mean of 0.0 is used as a first parameter.The standard deviation and the + // bias are determined by the client + constexpr float Mean = 0.0f; + return Gyroscope.Z + YawrateBias + mRandomEngine->GetNormalDistribution(Mean, YawrateDeviation); } -long millisecondsSince2004() { +long millisecondsSince2004() +{ // Define the epoch time (2004-01-01T00:00:00.000Z) std::tm epoch_time = {}; - epoch_time = {0, 0, 0, 1, 0, 104}; // January 1, 2004 + epoch_time = {0, 0, 0, 1, 0, 104}; // January 1, 2004 // Convert epoch time to a std::chrono::time_point std::chrono::system_clock::time_point epoch = std::chrono::system_clock::from_time_t(std::mktime(&epoch_time)); @@ -679,17 +671,16 @@ long millisecondsSince2004() { void CaService::AddRSUContainerHighFrequency(CAMContainer::HighFrequencyContainer_t &hfc) { hfc.present = CAMContainer::HighFrequencyContainer_PR_rsuContainerHighFrequency; - CAMContainer::RSUContainerHighFrequency_t& rsu = hfc.rsuContainerHighFrequency; - //TODO For future implementation - // ITSContainer::ProtectedCommunicationZonesRSU_t PCZR; - - uint8_t ProtectedZoneDataLength = 16; //Maximum number of elements in path history + CAMContainer::RSUContainerHighFrequency_t &rsu = hfc.rsuContainerHighFrequency; + // TODO For future implementation ITSContainer::ProtectedCommunicationZonesRSU_t PCZR + + uint8_t ProtectedZoneDataLength = 16; // Maximum number of elements in path history for (uint8_t i = 0; i <= ProtectedZoneDataLength; ++i) { ITSContainer::ProtectedCommunicationZone_t PCZ; PCZ.protectedZoneType = ITSContainer::ProtectedZoneType_cenDsrcTolling; - PCZ.expiryTimeAvailable = false; + PCZ.expiryTimeAvailable = false; PCZ.protectedZoneLatitude = 50; PCZ.protectedZoneLongitude = 50; PCZ.protectedZoneRadiusAvailable = false; @@ -699,42 +690,42 @@ void CaService::AddRSUContainerHighFrequency(CAMContainer::HighFrequencyContaine } } -void CaService::AddLowFrequencyContainer(CAMContainer::LowFrequencyContainer_t& lfc) +void CaService::AddLowFrequencyContainer(CAMContainer::LowFrequencyContainer_t &lfc) { lfc.present = CAMContainer::LowFrequencyContainer_PR_basicVehicleContainerLowFrequency; - CAMContainer::BasicVehicleContainerLowFrequency_t& bvc = lfc.basicVehicleContainerLowFrequency; + CAMContainer::BasicVehicleContainerLowFrequency_t &bvc = lfc.basicVehicleContainerLowFrequency; /*Vehicle Role*/ bvc.vehicleRole = GetVehicleRole(); /*Exterior Lights*/ - uint8_t* buf = &bvc.exteriorLights; + uint8_t *buf = &bvc.exteriorLights; FVehicleLightState LightStateData = mVehicle->GetVehicleLightState(); - if(LightStateData.LowBeam) - { + if (LightStateData.LowBeam) + { buf[0] |= 1 << (7 - ITSContainer::ExteriorLights_lowBeamHeadlightsOn); } - if(LightStateData.HighBeam) + if (LightStateData.HighBeam) { buf[0] |= 1 << (7 - ITSContainer::ExteriorLights_highBeamHeadlightsOn); } - if(LightStateData.LeftBlinker) + if (LightStateData.LeftBlinker) { buf[0] |= 1 << (7 - ITSContainer::ExteriorLights_leftTurnSignalOn); } - if(LightStateData.RightBlinker) + if (LightStateData.RightBlinker) { buf[0] |= 1 << (7 - ITSContainer::ExteriorLights_rightTurnSignalOn); } - if(LightStateData.Reverse) + if (LightStateData.Reverse) { buf[0] |= 1 << (7 - ITSContainer::ExteriorLights_reverseLightOn); } - if(LightStateData.Fog) + if (LightStateData.Fog) { buf[0] |= 1 << (7 - ITSContainer::ExteriorLights_fogLightOn); } - if(LightStateData.Position) + if (LightStateData.Position) { buf[0] |= 1 << (7 - ITSContainer::ExteriorLights_parkingLightsOn); } @@ -743,13 +734,12 @@ void CaService::AddLowFrequencyContainer(CAMContainer::LowFrequencyContainer_t& bool CaService::CheckHeadingDelta(float DeltaSeconds) { - //if heading diff is more than 4degree + // if heading diff is more than 4degree VehicleHeading = mVehicle->GetVehicleOrientation(); double HeadingDelta = carla::geom::Math::ToDegrees(GetFVectorAngle(mLastCamHeading, VehicleHeading)); - if(HeadingDelta > 4.0) + if (HeadingDelta > 4.0) { return true; } return false; } - diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.h index 749b54e65..070f185ad 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.h @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -20,32 +20,30 @@ class CaService { public: - - CaService(URandomEngine* random_engine); - void SetOwner(UWorld* world, AActor *Owner); + CaService(URandomEngine *random_engine); + void SetOwner(UWorld *world, AActor *Owner); void SetParams(const float GenCamMin, const float GenCamMax, const bool FixedRate); - void SetVelDeviation(const float noise_vel_stddev_x); - void SetYawrateDeviation(const float noise_yawrate_stddev, const float noise_yawrate_bias); + void SetVelDeviation(const float noise_vel_stddev_x); + void SetYawrateDeviation(const float noise_yawrate_stddev, const float noise_yawrate_bias); void SetAccelerationStandardDeviation(const FVector &Vec); void SetGNSSDeviation(const float noise_lat_stddev, - const float noise_lon_stddev, - const float noise_alt_stddev, - const float noise_head_stddev, - const float noise_lat_bias, - const float noise_lon_bias, - const float noise_alt_bias, - const float noise_head_bias); + const float noise_lon_stddev, + const float noise_alt_stddev, + const float noise_head_stddev, + const float noise_lat_bias, + const float noise_lon_bias, + const float noise_alt_bias, + const float noise_head_bias); bool Trigger(float DeltaSeconds); CAM_t GetCamMessage(); private: - AActor *mActorOwner; - FCarlaActor* mCarlaActor; - UCarlaEpisode* mCarlaEpisode; - UWorld* mWorld; - ACarlaWheeledVehicle* mVehicle; + FCarlaActor *mCarlaActor; + UCarlaEpisode *mCarlaEpisode; + UWorld *mWorld; + ACarlaWheeledVehicle *mVehicle; float mLastCamTimeStamp; float mLastLowCamTimeStamp; float mGenCamMin; @@ -68,7 +66,6 @@ private: FVector mLastCamHeading; std::chrono::milliseconds mGenerationDelta0; - bool CheckTriggeringConditions(float DeltaSeconds); bool CheckHeadingDelta(float DeltaSeconds); bool CheckPositionDelta(float DeltaSeconds); @@ -85,21 +82,21 @@ private: const long mMessageId = ITSContainer::messageID_cam; long mStationId; long mStationType; - + carla::geom::Vector3D ComputeAccelerometer(const float DeltaTime); const carla::geom::Vector3D ComputeAccelerometerNoise(const FVector &Accelerometer); /// Standard deviation for acceleration settings. FVector StdDevAccel; - /// Used to compute the acceleration + /// Used to compute the acceleration std::array PrevLocation; /// Used to compute the acceleration float PrevDeltaTime; - //GNSS reference position and heading + // GNSS reference position and heading FVector GetReferencePosition(); - carla::geom::GeoLocation CurrentGeoReference; + carla::geom::GeoLocation CurrentGeoReference; float LatitudeDeviation; float LongitudeDeviation; float AltitudeDeviation; @@ -109,29 +106,27 @@ private: float LongitudeBias; float AltitudeBias; float HeadingBias; - - //Velocity - float ComputeSpeed(); - float VelocityDeviation; - - //Yaw rate + + // Velocity + float ComputeSpeed(); + float VelocityDeviation; + + // Yaw rate const float ComputeYawNoise(const FVector &Gyroscope); float ComputeYawRate(); float YawrateDeviation; float YawrateBias; CAM_t CreateCooperativeAwarenessMessage(float DeltaTime); - void CreateITSPduHeader(CAM_t& message); - void AddCooperativeAwarenessMessage(CAMContainer::CoopAwareness_t& CoopAwarenessMessage, float DeltaTime); - void AddBasicContainer(CAMContainer::BasicContainer_t& BasicContainer); - void AddBasicVehicleContainerHighFrequency(CAMContainer::HighFrequencyContainer_t& hfc, float DeltaTime); - void AddRSUContainerHighFrequency(CAMContainer::HighFrequencyContainer_t& hfc); - void AddLowFrequencyContainer(CAMContainer::LowFrequencyContainer_t& lfc); + void CreateITSPduHeader(CAM_t &message); + void AddCooperativeAwarenessMessage(CAMContainer::CoopAwareness_t &CoopAwarenessMessage, float DeltaTime); + void AddBasicContainer(CAMContainer::BasicContainer_t &BasicContainer); + void AddBasicVehicleContainerHighFrequency(CAMContainer::HighFrequencyContainer_t &hfc, float DeltaTime); + void AddRSUContainerHighFrequency(CAMContainer::HighFrequencyContainer_t &hfc); + void AddLowFrequencyContainer(CAMContainer::LowFrequencyContainer_t &lfc); CAM_t mCAMMessage; - //random for noise - URandomEngine* mRandomEngine; + // random for noise + URandomEngine *mRandomEngine; ITSContainer::SpeedValue_t buildSpeedValue(const float vel); - - }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.cpp index df87f9423..53e40ee69 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -10,10 +10,9 @@ #include #include - double PathLossModel::Frequency_GHz = 5.9f; -double PathLossModel::Frequency = 5.9f * std::pow(10,9); -double PathLossModel::lambda = PathLossModel::c_speedoflight/(5.9f * std::pow(10,9)); +double PathLossModel::Frequency = 5.9f * std::pow(10, 9); +double PathLossModel::lambda = PathLossModel::c_speedoflight / (5.9f * std::pow(10, 9)); PathLossModel::PathLossModel(URandomEngine *random_engine) { @@ -44,8 +43,8 @@ void PathLossModel::SetParams(const float TransmitPower, this->custom_fading_stddev = custom_fading_stddev; this->combined_antenna_gain = combined_antenna_gain; PathLossModel::Frequency_GHz = Frequency; - PathLossModel::Frequency = PathLossModel::Frequency_GHz * std::pow(10,9); - PathLossModel::lambda = PathLossModel::c_speedoflight/PathLossModel::Frequency; + PathLossModel::Frequency = PathLossModel::Frequency_GHz * std::pow(10, 9); + PathLossModel::lambda = PathLossModel::c_speedoflight / PathLossModel::Frequency; // when reference distance is set, we prepare the FSPL for the reference distance to be used in LDPL CalculateFSPL_d0(); } @@ -316,7 +315,6 @@ void PathLossModel::SetPathLossModel(const EPathLossModel path_loss_model) model = path_loss_model; } - float PathLossModel::ComputeLoss(AActor *OtherActor, FVector Source, FVector Destination, double Distance3d, double TxHeight, double RxHeight, double reference_z) { // TxHeight in m @@ -524,7 +522,7 @@ float PathLossModel::CalculateShadowFading(EPathState state) } else { - //custom fading param + // custom fading param std_dev_dB = custom_fading_stddev; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.h index beb37398b..1c43e58bc 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.h @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -9,21 +9,24 @@ #include #include -using ActorPowerMap = std::map; +using ActorPowerMap = std::map; using ActorPowerPair = std::pair; -enum EPathState{ +enum EPathState +{ LOS, NLOSb, NLOSv }; -enum EPathLossModel{ +enum EPathLossModel +{ Winner, Geometric, }; -enum EScenario{ +enum EScenario +{ Highway, Rural, Urban @@ -32,7 +35,7 @@ enum EScenario{ struct DiffractionObstacle { DiffractionObstacle(double distTx, double height); - //meter + // meter double d; // distance to transmitter double h; // height of obstacle }; @@ -42,103 +45,100 @@ struct DiffractionPath DiffractionPath(); double attenuation; - double d; //meter + double d; // meter }; - class PathLossModel { public: - PathLossModel(URandomEngine* random_engine); + PathLossModel(URandomEngine *random_engine); void SetOwner(AActor *Owner); void SetScenario(EScenario scenario); - void Simulate(const std::vector ActorList, UCarlaEpisode *CarlaEpisode, UWorld* World); + void Simulate(const std::vector ActorList, UCarlaEpisode *CarlaEpisode, UWorld *World); ActorPowerMap GetReceiveActorPowerList(); void SetParams(const float TransmitPower, - const float ReceiverSensitivity, - const float Frequency, - const float combined_antenna_gain, - const float path_loss_exponent, - const float reference_distance_fspl, - const float filter_distance, - const bool use_etsi_fading, - const float custom_fading_stddev); + const float ReceiverSensitivity, + const float Frequency, + const float combined_antenna_gain, + const float path_loss_exponent, + const float reference_distance_fspl, + const float filter_distance, + const bool use_etsi_fading, + const float custom_fading_stddev); float GetTransmitPower() { return TransmitPower; } void SetPathLossModel(const EPathLossModel path_loss_model); private: - //multiple knife edge diffraction for NLOSv - double computeVehiclePathLoss(const FVector& pos_tx, const FVector& pos_rx, const double reference_z, std::vector& vehicle_obstacles); - double computeSimpleKnifeEdge(double heightTx, double heightRx,double heightObs, double distTxRx, double distTxObs); - DiffractionPath computeMultipleKnifeEdge(const std::list& obs); - std::list buildTopObstacles(const std::vector& vehicles, const FVector& pos_tx, const FVector& pos_rx); + // multiple knife edge diffraction for NLOSv + double computeVehiclePathLoss(const FVector &pos_tx, const FVector &pos_rx, const double reference_z, std::vector &vehicle_obstacles); + double computeSimpleKnifeEdge(double heightTx, double heightRx, double heightObs, double distTxRx, double distTxObs); + DiffractionPath computeMultipleKnifeEdge(const std::list &obs); + std::list buildTopObstacles(const std::vector &vehicles, const FVector &pos_tx, const FVector &pos_rx); - //powers - float CalculateReceivedPower(AActor* OtherActor, - const float OtherTransmitPower, - const FVector Source, - const FVector Destination, - const double Distance3d, - const double ht, - const double ht_local, - const double hr, - const double hr_local, - const double reference_z); - void EstimatePathStateAndVehicleObstacles(AActor* OtherActor, FVector Source, double TxHeight, double RxHeight, double reference_z, EPathState& state, std::vector& vehicle_obstacles); + // powers + float CalculateReceivedPower(AActor *OtherActor, + const float OtherTransmitPower, + const FVector Source, + const FVector Destination, + const double Distance3d, + const double ht, + const double ht_local, + const double hr, + const double hr_local, + const double reference_z); + void EstimatePathStateAndVehicleObstacles(AActor *OtherActor, FVector Source, double TxHeight, double RxHeight, double reference_z, EPathState &state, std::vector &vehicle_obstacles); double MakeVehicleBlockageLoss(double TxHeight, double RxHeight, double obj_height, double obj_distance); - //variables + // variables AActor *mActorOwner; - UCarlaEpisode* mCarlaEpisode; - UWorld* mWorld; - URandomEngine* mRandomEngine; - + UCarlaEpisode *mCarlaEpisode; + UWorld *mWorld; + URandomEngine *mRandomEngine; + ActorPowerMap mReceiveActorPowerList; FVector CurrentActorLocation; - //constants - constexpr static float c_speedoflight = 299792458.0; //m/s + // constants + constexpr static float c_speedoflight = 299792458.0; // m/s - //full two ray path loss - const double epsilon_r = 1.02; + // full two ray path loss + const double epsilon_r = 1.02; - //params - static double Frequency_GHz; // 5.9f;//5.9 GHz - static double Frequency; // Frequency_GHz * std::pow(10,9); - static double lambda; // c_speedoflight/Frequency; - float reference_distance_fspl; //m - float TransmitPower; //dBm - float ReceiverSensitivity; //dBm + // params + static double Frequency_GHz; // 5.9f;//5.9 GHz + static double Frequency; // Frequency_GHz * std::pow(10,9); + static double lambda; // c_speedoflight/Frequency; + float reference_distance_fspl; // m + float TransmitPower; // dBm + float ReceiverSensitivity; // dBm EScenario scenario; float path_loss_exponent; // no unit, default 2.7; - float filter_distance; //in meters default 500.0 + float filter_distance; // in meters default 500.0 EPathLossModel model; bool use_etsi_fading; float custom_fading_stddev; - float combined_antenna_gain; //10.0 dBi + float combined_antenna_gain; // 10.0 dBi - //dependent params that are precalculated on setting of params + // dependent params that are precalculated on setting of params float m_fspl_d0; protected: + /// Method that allow to preprocess if the rays will be traced. - /// Method that allow to preprocess if the rays will be traced. + float ComputeLoss(AActor *OtherActor, FVector Source, FVector Destination, double Distance3d, double TxHeight, double RxHeight, double reference_z); + bool IsVehicle(const FHitResult &HitInfo); + bool GetLocationIfVehicle(const FVector CurrentActorLocation, const FHitResult &HitInfo, const double reference_z, FVector &location); + bool HitIsSelfOrOther(const FHitResult &HitInfo, AActor *OtherActor); + float CalculatePathLoss_WINNER(EPathState state, double Distance); + double CalculateNLOSvLoss(const FVector Source, const FVector Destination, const double TxHeight, const double RxHeight, const double RxDistance3d, std::vector &vehicle_obstacles); - float ComputeLoss(AActor* OtherActor, FVector Source, FVector Destination, double Distance3d, double TxHeight, double RxHeight, double reference_z); - bool IsVehicle(const FHitResult& HitInfo); - bool GetLocationIfVehicle(const FVector CurrentActorLocation, const FHitResult &HitInfo, const double reference_z, FVector& location); - bool HitIsSelfOrOther(const FHitResult &HitInfo, AActor* OtherActor); - float CalculatePathLoss_WINNER(EPathState state, double Distance); - double CalculateNLOSvLoss( const FVector Source, const FVector Destination, const double TxHeight, const double RxHeight, const double RxDistance3d, std::vector& vehicle_obstacles); + float CalculateShadowFading(EPathState state); - float CalculateShadowFading(EPathState state); - - //full two ray model - double CalculateTwoRayPathLoss(double Distance3d, double TxHeight, double RxHeight); - //simplified two ray model - float CalculateTwoRayPathLossSimple(double Distance3d, double TxHeight, double RxHeight); - - //functions for precalculation - void CalculateFSPL_d0(); - TArray HitResult; + // full two ray model + double CalculateTwoRayPathLoss(double Distance3d, double TxHeight, double RxHeight); + // simplified two ray model + float CalculateTwoRayPathLossSimple(double Distance3d, double TxHeight, double RxHeight); + // functions for precalculation + void CalculateFSPL_d0(); + TArray HitResult; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/VehicleObstacle.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/VehicleObstacle.cpp index 4f44a31d1..fd27ed80e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/VehicleObstacle.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/VehicleObstacle.cpp @@ -1,5 +1,5 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -14,32 +14,27 @@ #include "PathLossModel.h" +namespace +{ + auto compareDistance = [](const DiffractionObstacle &a, const DiffractionObstacle &b) + { return a.d < b.d; }; + auto compareHeight = [](const DiffractionObstacle &a, const DiffractionObstacle &b) + { return a.h < b.h; }; -namespace { -auto compareDistance = [](const DiffractionObstacle& a, const DiffractionObstacle& b) { return a.d < b.d; }; -auto compareHeight = [](const DiffractionObstacle& a, const DiffractionObstacle& b) { return a.h < b.h; }; - -using ObstacleIterator = std::list::const_iterator; -ObstacleIterator findMainObstacle(ObstacleIterator, ObstacleIterator); -ObstacleIterator findSecondaryObstacle(ObstacleIterator, ObstacleIterator); + using ObstacleIterator = std::list::const_iterator; + ObstacleIterator findMainObstacle(ObstacleIterator, ObstacleIterator); + ObstacleIterator findSecondaryObstacle(ObstacleIterator, ObstacleIterator); } // namespace -DiffractionObstacle::DiffractionObstacle(double distTx, double height) : - d(distTx), h(height) +DiffractionObstacle::DiffractionObstacle(double distTx, double height) : d(distTx), h(height) { } -DiffractionPath::DiffractionPath() : - attenuation(0.0), d(0.0) +DiffractionPath::DiffractionPath() : attenuation(0.0), d(0.0) { } - - - - - -double PathLossModel::computeSimpleKnifeEdge(double heightTx, double heightRx,double heightObs, double distTxRx, double distTxObs) +double PathLossModel::computeSimpleKnifeEdge(double heightTx, double heightRx, double heightObs, double distTxRx, double distTxObs) { // following calculations are similar to equation 29 of ITU-R P.526-13: // v = sqrt(2d/lambda * alpha1 * alpha2) @@ -63,32 +58,35 @@ double PathLossModel::computeSimpleKnifeEdge(double heightTx, double heightRx,do const double v = root_two * h / r; double loss = 0.0; - if (v > -0.78) { + if (v > -0.78) + { // approximation of Fresnel-Kirchoff loss given by ITU-R P.526, equation 31 (result in dB): // J(v) = 6.9 + 20 log(sqrt((v - 01)^2 + 1) + v - 0.1) - loss = 6.9 + 20.0 * log10(sqrt(std::pow(v - 0.1,2) + 1.0) + v - 0.1); + loss = 6.9 + 20.0 * log10(sqrt(std::pow(v - 0.1, 2) + 1.0) + v - 0.1); } return loss; } -DiffractionPath PathLossModel::computeMultipleKnifeEdge(const std::list& obs) +DiffractionPath PathLossModel::computeMultipleKnifeEdge(const std::list &obs) { DiffractionPath path; // determine main and secondary obstacles std::vector mainObs; mainObs.push_back(obs.begin()); // Tx - for (ObstacleIterator it = obs.begin(); it != obs.end();) { + for (ObstacleIterator it = obs.begin(); it != obs.end();) + { it = findMainObstacle(it, obs.end()); - if (it != obs.end()) { + if (it != obs.end()) + { mainObs.push_back(it); } } // NOTE: Rx is added by loop as last main obstacle - struct SecondaryObstacle { - SecondaryObstacle(ObstacleIterator tx, ObstacleIterator obs, ObstacleIterator rx) : - tx(tx), obstacle(obs), rx(rx) {} + struct SecondaryObstacle + { + SecondaryObstacle(ObstacleIterator tx, ObstacleIterator obs, ObstacleIterator rx) : tx(tx), obstacle(obs), rx(rx) {} ObstacleIterator tx; ObstacleIterator obstacle; ObstacleIterator rx; @@ -96,31 +94,37 @@ DiffractionPath PathLossModel::computeMultipleKnifeEdge(const std::list secObs; std::vector mainObsDistances; - for (std::size_t i = 0, j = 1; j < mainObs.size(); ++i, ++j) { + for (std::size_t i = 0, j = 1; j < mainObs.size(); ++i, ++j) + { const double d = mainObs[j]->d - mainObs[i]->d; - path.d += sqrt(std::pow(d,2) + std::pow(mainObs[j]->h - mainObs[i]->h,2)); + path.d += sqrt(std::pow(d, 2) + std::pow(mainObs[j]->h - mainObs[i]->h, 2)); mainObsDistances.push_back(d); const auto delta = std::distance(mainObs[i], mainObs[j]); - if (delta == 2) { + if (delta == 2) + { // single other obstacle between two main obstacles secObs.emplace_back(mainObs[i], std::next(mainObs[i]), mainObs[j]); - } else if (delta > 2) { + } + else if (delta > 2) + { secObs.emplace_back(mainObs[i], findSecondaryObstacle(mainObs[i], mainObs[j]), mainObs[j]); } } // attenuation due to main obstacles double attMainObs = 0.0; - for (std::size_t i = 0; i < mainObs.size() - 2; ++i) { + for (std::size_t i = 0; i < mainObs.size() - 2; ++i) + { const double distTxObs = mainObsDistances[i]; - const double distTxRx = distTxObs + mainObsDistances[i+1]; - attMainObs += computeSimpleKnifeEdge(mainObs[i]->h, mainObs[i+2]->h, mainObs[i+1]->h, distTxRx, distTxObs); + const double distTxRx = distTxObs + mainObsDistances[i + 1]; + attMainObs += computeSimpleKnifeEdge(mainObs[i]->h, mainObs[i + 2]->h, mainObs[i + 1]->h, distTxRx, distTxObs); } // attenuation due to secondary obstacles double attSecObs = 0.0; - for (const SecondaryObstacle& sec : secObs) { + for (const SecondaryObstacle &sec : secObs) + { const double distTxRx = sec.rx->d - sec.tx->d; const double distTxObs = sec.obstacle->d - sec.tx->d; attSecObs += computeSimpleKnifeEdge(sec.tx->h, sec.rx->h, sec.obstacle->h, distTxRx, distTxObs); @@ -128,31 +132,35 @@ DiffractionPath PathLossModel::computeMultipleKnifeEdge(const std::listd - mainObs.front()->d; // distance between Tx and Rx - for (double d : mainObsDistances) { + for (double d : mainObsDistances) + { C *= d; } double pairwiseDistProduct = 1.0; - for (std::size_t i = 1; i < mainObsDistances.size(); ++i) { - pairwiseDistProduct *= mainObsDistances[i-1] + mainObsDistances[i]; + for (std::size_t i = 1; i < mainObsDistances.size(); ++i) + { + pairwiseDistProduct *= mainObsDistances[i - 1] + mainObsDistances[i]; } C /= mainObsDistances.front() * mainObsDistances.back() * pairwiseDistProduct; - path.attenuation = attMainObs + attSecObs - 10.0* log10(C); + path.attenuation = attMainObs + attSecObs - 10.0 * log10(C); return path; } -std::list PathLossModel::buildTopObstacles(const std::vector& vehicles, const FVector& pos_tx, const FVector& pos_rx) +std::list PathLossModel::buildTopObstacles(const std::vector &vehicles, const FVector &pos_tx, const FVector &pos_rx) { std::list diffTop; const double vx = pos_rx.X - pos_tx.X; const double vy = pos_rx.Y - pos_tx.Y; - for (auto vehicle : vehicles) { + for (auto vehicle : vehicles) + { const double midpoint_x = vehicle.X; const double midpoint_y = vehicle.Y; - const double k = (midpoint_x * vx - vy * pos_tx.Y + vy * midpoint_y - pos_tx.X * vx) / (std::pow(vx,2) + std::pow(vy,2)); - if (k < 0.0 || k > 1.0) continue; /*< skip points beyond the ends of TxRx line segment */ - const double d = k * sqrt(std::pow(vx,2) + std::pow(vy,2)); + const double k = (midpoint_x * vx - vy * pos_tx.Y + vy * midpoint_y - pos_tx.X * vx) / (std::pow(vx, 2) + std::pow(vy, 2)); + if (k < 0.0 || k > 1.0) + continue; /*< skip points beyond the ends of TxRx line segment */ + const double d = k * sqrt(std::pow(vx, 2) + std::pow(vy, 2)); diffTop.emplace_back(d, vehicle.Z); } @@ -160,69 +168,72 @@ std::list PathLossModel::buildTopObstacles(const std::vecto return diffTop; } - - -namespace { - -ObstacleIterator findMainObstacle(ObstacleIterator begin, ObstacleIterator end) +namespace { - ObstacleIterator mainIterator = end; - double mainAngle = -std::numeric_limits::infinity(); - if (begin != end) { - for (ObstacleIterator it = std::next(begin); it != end; ++it) { - double angle = (it->h - begin->h) / (it->d - begin->d); - if (angle > mainAngle) { - mainIterator = it; - mainAngle = angle; + ObstacleIterator findMainObstacle(ObstacleIterator begin, ObstacleIterator end) + { + ObstacleIterator mainIterator = end; + double mainAngle = -std::numeric_limits::infinity(); + + if (begin != end) + { + for (ObstacleIterator it = std::next(begin); it != end; ++it) + { + double angle = (it->h - begin->h) / (it->d - begin->d); + if (angle > mainAngle) + { + mainIterator = it; + mainAngle = angle; + } } } + + return mainIterator; } - return mainIterator; -} + ObstacleIterator findSecondaryObstacle(ObstacleIterator first, ObstacleIterator last) + { + ObstacleIterator secIterator = last; + double secHeightGap{std::numeric_limits::infinity()}; -ObstacleIterator findSecondaryObstacle(ObstacleIterator first, ObstacleIterator last) -{ - ObstacleIterator secIterator = last; - double secHeightGap { std::numeric_limits::infinity() }; - - const double distFirstLast = last->d - first->d; - const double heightFirstLast = last->h - first->h; - const auto offset = first->h * last->d - first->d * last->h; - for (ObstacleIterator it = std::next(first); it != last; ++it) { - const double heightGap = ((it->d * heightFirstLast + offset) / distFirstLast) - it->h; - if (heightGap < secHeightGap) { - secIterator = it; - secHeightGap = heightGap; + const double distFirstLast = last->d - first->d; + const double heightFirstLast = last->h - first->h; + const auto offset = first->h * last->d - first->d * last->h; + for (ObstacleIterator it = std::next(first); it != last; ++it) + { + const double heightGap = ((it->d * heightFirstLast + offset) / distFirstLast) - it->h; + if (heightGap < secHeightGap) + { + secIterator = it; + secHeightGap = heightGap; + } } - } - return secIterator; -} + return secIterator; + } } // namespace - -//statistical model from ETSI TR 103 257-1 V1.1.1 (2019-05) +// statistical model from ETSI TR 103 257-1 V1.1.1 (2019-05) double PathLossModel::MakeVehicleBlockageLoss(double TxHeight, double RxHeight, double obj_height, double obj_distance) { - //according to ETSI, stochastic method - if( TxHeight > obj_height && RxHeight > obj_height) + // according to ETSI, stochastic method + if (TxHeight > obj_height && RxHeight > obj_height) { - //no blocking if higher than obj + // no blocking if higher than obj return 0.0; } - else if(TxHeight < obj_height && RxHeight < obj_height) + else if (TxHeight < obj_height && RxHeight < obj_height) { - //worst case: obj is higher than both tx and rx - float mean = 9.0f + fmax(0.0f, 15.0f * log10(obj_distance)-41.0f); + // worst case: obj is higher than both tx and rx + float mean = 9.0f + fmax(0.0f, 15.0f * log10(obj_distance) - 41.0f); return mRandomEngine->GetNormalDistribution(mean, 4.5f); } else { - //something in between - float mean = 5.0f + fmax(0.0f, 15.0f * log10(obj_distance)-41.0f); + // something in between + float mean = 5.0f + fmax(0.0f, 15.0f * log10(obj_distance) - 41.0f); return mRandomEngine->GetNormalDistribution(mean, 4.0f); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.cpp index 503f1e923..2e5c88692 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -40,7 +40,7 @@ void AV2XSensor::SetOwner(AActor *Owner) Super::SetOwner(Owner); // Store the actor into the static list if the actor details are not available - if(Owner != nullptr) + if (Owner != nullptr) { if (std::find(AV2XSensor::mV2XActorContainer.begin(), AV2XSensor::mV2XActorContainer.end(), Owner) == AV2XSensor::mV2XActorContainer.end()) { @@ -51,10 +51,8 @@ void AV2XSensor::SetOwner(AActor *Owner) CaServiceObj->SetOwner(world, Owner); PathLossModelObj->SetOwner(Owner); } - } - FActorDefinition AV2XSensor::GetSensorDefinition() { return UActorBlueprintFunctionLibrary::MakeV2XDefinition(); @@ -88,7 +86,8 @@ void AV2XSensor::SetPropagationParams(const float TransmitPower, PathLossModelObj->SetParams(TransmitPower, ReceiverSensitivity, Frequency, combined_antenna_gain, path_loss_exponent, reference_distance_fspl, filter_distance, use_etsi_fading, custom_fading_stddev); } -void AV2XSensor::SetPathLossModel(const EPathLossModel path_loss_model){ +void AV2XSensor::SetPathLossModel(const EPathLossModel path_loss_model) +{ PathLossModelObj->SetPathLossModel(path_loss_model); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.h index 89b6be27a..9ce46dd2a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.h @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the +// Copyright (c) 2024 Institut fuer Technik der Informationsverarbeitung (ITIV) at the // Karlsruhe Institute of Technology // // This work is licensed under the terms of the MIT license. @@ -35,30 +35,30 @@ public: void SetCaServiceParams(const float GenCamMin, const float GenCamMax, const bool FixedRate); void SetPropagationParams(const float TransmitPower, - const float ReceiverSensitivity, - const float Frequency, - const float combined_antenna_gain, - const float path_loss_exponent, - const float reference_distance_fspl, - const float filter_distance, - const bool use_etsi_fading, - const float custom_fading_stddev); + const float ReceiverSensitivity, + const float Frequency, + const float combined_antenna_gain, + const float path_loss_exponent, + const float reference_distance_fspl, + const float filter_distance, + const bool use_etsi_fading, + const float custom_fading_stddev); void SetScenario(EScenario scenario); - //CAM params + // CAM params void SetAccelerationStandardDeviation(const FVector &Vec); void SetGNSSDeviation(const float noise_lat_stddev, - const float noise_lon_stddev, - const float noise_alt_stddev, - const float noise_head_stddev, - const float noise_lat_bias, - const float noise_lon_bias, - const float noise_alt_bias, - const float noise_head_bias); - void SetVelDeviation(const float noise_vel_stddev); + const float noise_lon_stddev, + const float noise_alt_stddev, + const float noise_head_stddev, + const float noise_lat_bias, + const float noise_lon_bias, + const float noise_alt_bias, + const float noise_head_bias); + void SetVelDeviation(const float noise_vel_stddev); void SetYawrateDeviation(const float noise_yawrate_stddev, const float noise_yawrate_bias); void SetPathLossModel(const EPathLossModel path_loss_model); - + virtual void PrePhysTick(float DeltaSeconds) override; virtual void PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime) override; void SetOwner(AActor *Owner) override; @@ -68,10 +68,10 @@ private: CaService *CaServiceObj; PathLossModel *PathLossModelObj; - //store data + // store data static ActorV2XDataMap mActorV2XDataMap; FV2XData mV2XData; - //write + // write void WriteMessageToV2XData(const V2XDataList &msg_received_power_list); };