p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

comments and formatting

This commit is contained in:
Daniel Grimm 2024-04-23 15:41:54 +02:00 committed by Blyron
parent 780234b9a6
commit bace2f1aab
15 changed files with 2062 additions and 587 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
LibCarla/source/carla/sensor/data/LibITS.h
View File

@ -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*/

17
LibCarla/source/carla/sensor/data/V2XData.h
View File

@ -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<CustomV2XData> MessageList;
friend class s11n::CustomV2XDataSerializer;
};
};
} // namespace s11n
} // namespace sensor

4
LibCarla/source/carla/sensor/data/V2XEvent.h
View File

@ -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

1
PythonAPI/carla/source/libcarla/SensorData.cpp
View File

@ -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;
}

3
PythonAPI/carla/source/libcarla/V2XData.cpp
View File

@ -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:

1419
PythonAPI/examples/V2XDemo.py Normal file
View File

File diff suppressed because it is too large Load Diff

95
PythonAPI/examples/test_addsecondvx.py Normal file
View File

@ -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()

2
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp
View File

@ -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 --------------------------------

612
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.cpp
View File

@ -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 <chrono>
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<std::chrono::milliseconds>(start_Point - ref_point);
mGenerationDelta0 = std::chrono::duration_cast<std::chrono::milliseconds>(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<ACarlaWheeledVehicle>(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<long>(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<long>(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<long>(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<long long>(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<float>::max());
return boost::algorithm::clamp(speed + mRandomEngine->GetNormalDistribution(Mean, VelocityDeviation), 0.0f, std::numeric_limits<float>::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;
}

73
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/CaService.h
View File

@ -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<FVector, 2> 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);
};

14
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.cpp
View File

@ -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 <random>
#include <limits>
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;
}

142
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/PathLossModel.h
View File

@ -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 <vector>
#include <cmath>
using ActorPowerMap = std::map<AActor*, float>;
using ActorPowerMap = std::map<AActor *, float>;
using ActorPowerPair = std::pair<AActor *, float>;
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<ActorPowerPair> ActorList, UCarlaEpisode *CarlaEpisode, UWorld* World);
void Simulate(const std::vector<ActorPowerPair> 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<FVector>& vehicle_obstacles);
double computeSimpleKnifeEdge(double heightTx, double heightRx,double heightObs, double distTxRx, double distTxObs);
DiffractionPath computeMultipleKnifeEdge(const std::list<DiffractionObstacle>& obs);
std::list<DiffractionObstacle> buildTopObstacles(const std::vector<FVector>& 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<FVector> &vehicle_obstacles);
double computeSimpleKnifeEdge(double heightTx, double heightRx, double heightObs, double distTxRx, double distTxObs);
DiffractionPath computeMultipleKnifeEdge(const std::list<DiffractionObstacle> &obs);
std::list<DiffractionObstacle> buildTopObstacles(const std::vector<FVector> &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<FVector>& 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<FVector> &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<FVector> &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<FVector>& 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<FHitResult> 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<FHitResult> HitResult;
};

179
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2X/VehicleObstacle.cpp
View File

@ -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<DiffractionObstacle>::const_iterator;
ObstacleIterator findMainObstacle(ObstacleIterator, ObstacleIterator);
ObstacleIterator findSecondaryObstacle(ObstacleIterator, ObstacleIterator);
using ObstacleIterator = std::list<DiffractionObstacle>::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<DiffractionObstacle>& obs)
DiffractionPath PathLossModel::computeMultipleKnifeEdge(const std::list<DiffractionObstacle> &obs)
{
DiffractionPath path;
// determine main and secondary obstacles
std::vector<ObstacleIterator> 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<Diffract
std::vector<SecondaryObstacle> secObs;
std::vector<double> 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::list<Diffract
// correction factor C (see eq. 46 in ITU-R P.526-13)
double C = mainObs.back()->d - 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<DiffractionObstacle> PathLossModel::buildTopObstacles(const std::vector<FVector>& vehicles, const FVector& pos_tx, const FVector& pos_rx)
std::list<DiffractionObstacle> PathLossModel::buildTopObstacles(const std::vector<FVector> &vehicles, const FVector &pos_tx, const FVector &pos_rx)
{
std::list<DiffractionObstacle> 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<DiffractionObstacle> PathLossModel::buildTopObstacles(const std::vecto
return diffTop;
}
namespace {
ObstacleIterator findMainObstacle(ObstacleIterator begin, ObstacleIterator end)
namespace
{
ObstacleIterator mainIterator = end;
double mainAngle = -std::numeric_limits<double>::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<double>::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<double>::infinity()};
ObstacleIterator findSecondaryObstacle(ObstacleIterator first, ObstacleIterator last)
{
ObstacleIterator secIterator = last;
double secHeightGap { std::numeric_limits<double>::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);
}
}

9
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.cpp
View File

@ -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);
}

42
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/V2XSensor.h
View File

@ -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);
};