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

Fixed style in some old code

This commit is contained in:
Marc Garcia Puig 2019-01-26 16:25:56 +01:00
parent 4421358610
commit 8c8c256543
2 changed files with 155 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicleState.h
View File

@ -11,11 +11,12 @@
/// State of a ACarlaWheeledVehicle, to be displayed in editor for debugging
/// purposes.
UENUM(BlueprintType)
enum class ECarlaWheeledVehicleState : uint8 {
AutopilotOff UMETA(DisplayName = "Autopilot Off"),
FreeDriving UMETA(DisplayName = "Free driving"),
FollowingFixedRoute UMETA(DisplayName = "Following fixed route"),
WaitingForRedLight UMETA(DisplayName = "Waiting for red light"),
ObstacleAhead UMETA(DisplayName = "Obstacle ahead"),
UNKNOWN UMETA(DisplayName = "Unknown")
enum class ECarlaWheeledVehicleState : uint8
{
AutopilotOff UMETA(DisplayName = "Autopilot Off"),
FreeDriving UMETA(DisplayName = "Free driving"),
FollowingFixedRoute UMETA(DisplayName = "Following fixed route"),
WaitingForRedLight UMETA(DisplayName = "Waiting for red light"),
ObstacleAhead UMETA(DisplayName = "Obstacle ahead"),
UNKNOWN UMETA(DisplayName = "Unknown")
};

210
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp
View File

@ -18,18 +18,19 @@
// -- Static local methods -----------------------------------------------------
// =============================================================================
static bool RayTrace(const AActor &Actor, const FVector &Start, const FVector &End) {
static bool RayTrace(const AActor &Actor, const FVector &Start, const FVector &End)
{
FHitResult OutHit;
static FName TraceTag = FName(TEXT("VehicleTrace"));
FCollisionQueryParams CollisionParams(TraceTag, true);
CollisionParams.AddIgnoredActor(&Actor);
const bool Success = Actor.GetWorld()->LineTraceSingleByObjectType(
OutHit,
Start,
End,
FCollisionObjectQueryParams(FCollisionObjectQueryParams::AllDynamicObjects),
CollisionParams);
OutHit,
Start,
End,
FCollisionObjectQueryParams(FCollisionObjectQueryParams::AllDynamicObjects),
CollisionParams);
return Success && OutHit.bBlockingHit;
}
@ -46,19 +47,22 @@ static bool IsThereAnObstacleAhead(
const float Distance = std::max(50.0f, Speed * Speed); // why?
const FVector StartCenter = Vehicle.GetActorLocation() + (ForwardVector * (250.0f + VehicleBounds.X / 2.0f)) + FVector(0.0f, 0.0f, 50.0f);
const FVector StartCenter = Vehicle.GetActorLocation() +
(ForwardVector * (250.0f + VehicleBounds.X / 2.0f)) + FVector(0.0f, 0.0f, 50.0f);
const FVector EndCenter = StartCenter + NormDirection * (Distance + VehicleBounds.X / 2.0f);
const FVector StartRight = StartCenter + (FVector(ForwardVector.Y, -ForwardVector.X, ForwardVector.Z) * 100.0f);
const FVector StartRight = StartCenter +
(FVector(ForwardVector.Y, -ForwardVector.X, ForwardVector.Z) * 100.0f);
const FVector EndRight = StartRight + NormDirection * (Distance + VehicleBounds.X / 2.0f);
const FVector StartLeft = StartCenter + (FVector(-ForwardVector.Y, ForwardVector.X, ForwardVector.Z) * 100.0f);
const FVector StartLeft = StartCenter +
(FVector(-ForwardVector.Y, ForwardVector.X, ForwardVector.Z) * 100.0f);
const FVector EndLeft = StartLeft + NormDirection * (Distance + VehicleBounds.X / 2.0f);
return
RayTrace(Vehicle, StartCenter, EndCenter) ||
RayTrace(Vehicle, StartRight, EndRight) ||
RayTrace(Vehicle, StartLeft, EndLeft);
RayTrace(Vehicle, StartCenter, EndCenter) ||
RayTrace(Vehicle, StartRight, EndRight) ||
RayTrace(Vehicle, StartLeft, EndLeft);
}
template <typename T>
@ -72,8 +76,8 @@ static void ClearQueue(std::queue<T> &Queue)
// -- Constructor and destructor -----------------------------------------------
// =============================================================================
AWheeledVehicleAIController::AWheeledVehicleAIController(const FObjectInitializer& ObjectInitializer) :
Super(ObjectInitializer)
AWheeledVehicleAIController::AWheeledVehicleAIController(const FObjectInitializer &ObjectInitializer)
: Super(ObjectInitializer)
{
RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
@ -152,8 +156,8 @@ void AWheeledVehicleAIController::ConfigureAutopilot(const bool Enable)
ClearQueue(TargetLocations);
Vehicle->SetAIVehicleState(
bAutopilotEnabled ?
ECarlaWheeledVehicleState::FreeDriving :
ECarlaWheeledVehicleState::AutopilotOff);
ECarlaWheeledVehicleState::FreeDriving :
ECarlaWheeledVehicleState::AutopilotOff);
}
// =============================================================================
@ -164,10 +168,12 @@ void AWheeledVehicleAIController::SetFixedRoute(
const TArray<FVector> &Locations,
const bool bOverwriteCurrent)
{
if (bOverwriteCurrent) {
if (bOverwriteCurrent)
{
ClearQueue(TargetLocations);
}
for (auto &Location : Locations) {
for (auto &Location : Locations)
{
TargetLocations.emplace(Location);
}
}
@ -178,8 +184,9 @@ void AWheeledVehicleAIController::SetFixedRoute(
void AWheeledVehicleAIController::TickAutopilotController()
{
#if WITH_EDITOR
if (Vehicle == nullptr) { // This happens in simulation mode in editor.
#if WITH_EDITOR // This happens in simulation mode in editor.
if (Vehicle == nullptr)
{
bAutopilotEnabled = false;
return;
}
@ -190,9 +197,12 @@ void AWheeledVehicleAIController::TickAutopilotController()
FVector Direction;
float Steering;
if (!TargetLocations.empty()) {
if (!TargetLocations.empty())
{
Steering = GoToNextTargetLocation(Direction);
} else {
}
else
{
Steering = CalcStreeringValue(Direction);
}
@ -200,20 +210,28 @@ void AWheeledVehicleAIController::TickAutopilotController()
const auto Speed = Vehicle->GetVehicleForwardSpeed() * 0.036f;
float Throttle;
if (TrafficLightState != ETrafficLightState::Green) {
if (TrafficLightState != ETrafficLightState::Green)
{
Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::WaitingForRedLight);
Throttle = Stop(Speed);
} else if (IsThereAnObstacleAhead(*Vehicle, Speed, Direction)) {
}
else if (IsThereAnObstacleAhead(*Vehicle, Speed, Direction))
{
Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::ObstacleAhead);
Throttle = Stop(Speed);
} else {
}
else
{
Throttle = Move(Speed);
}
if (Throttle < 0.001f) {
if (Throttle < 0.001f)
{
AutopilotControl.Brake = 1.0f;
AutopilotControl.Throttle = 0.0f;
} else {
}
else
{
AutopilotControl.Brake = 0.0f;
AutopilotControl.Throttle = Throttle;
}
@ -223,22 +241,26 @@ void AWheeledVehicleAIController::TickAutopilotController()
float AWheeledVehicleAIController::GoToNextTargetLocation(FVector &Direction)
{
// Get middle point between the two front wheels.
const auto CurrentLocation = [&](){
const auto CurrentLocation = [&]() {
const auto &Wheels = Vehicle->GetVehicleMovementComponent()->Wheels;
check((Wheels.Num() > 1) && (Wheels[0u] != nullptr) && (Wheels[1u] != nullptr));
return (Wheels[0u]->Location + Wheels[1u]->Location) / 2.0f;
}();
} ();
const auto Target = [&](){
const auto Target = [&]() {
const auto &Result = TargetLocations.front();
return FVector{Result.X, Result.Y, CurrentLocation.Z};
}();
} ();
if (Target.Equals(CurrentLocation, 200.0f)) {
if (Target.Equals(CurrentLocation, 200.0f))
{
TargetLocations.pop();
if (!TargetLocations.empty()) {
if (!TargetLocations.empty())
{
return GoToNextTargetLocation(Direction);
} else {
}
else
{
return CalcStreeringValue(Direction);
}
}
@ -255,16 +277,26 @@ float AWheeledVehicleAIController::GoToNextTargetLocation(FVector &Direction)
float angle = dirAngle - actorAngle;
if (angle > 180.0f) { angle -= 360.0f;} else if (angle < -180.0f) {
if (angle > 180.0f)
{
angle -= 360.0f;
}
else if (angle < -180.0f)
{
angle += 360.0f;
}
float Steering = 0.0f;
if (angle < -MaximumSteerAngle) {
if (angle < -MaximumSteerAngle)
{
Steering = -1.0f;
} else if (angle > MaximumSteerAngle) {
}
else if (angle > MaximumSteerAngle)
{
Steering = 1.0f;
} else {
}
else
{
Steering += angle / MaximumSteerAngle;
}
@ -283,9 +315,10 @@ float AWheeledVehicleAIController::CalcStreeringValue(FVector &direction)
float forwardMagnitude = BoxExtent.X / 2.0f;
float Magnitude = (float) sqrt(pow((double) leftSensorPosition.X, 2.0) + pow((double) leftSensorPosition.Y, 2.0));
float Magnitude =
(float) sqrt(pow((double) leftSensorPosition.X, 2.0) + pow((double) leftSensorPosition.Y, 2.0));
//same for the right and left
// same for the right and left
float offset = FGenericPlatformMath::Acos(forwardMagnitude / Magnitude);
float actorAngle = forward.UnitCartesianToSpherical().Y;
@ -302,19 +335,32 @@ float AWheeledVehicleAIController::CalcStreeringValue(FVector &direction)
leftSensorPosition.Y = sinL * Magnitude;
leftSensorPosition.X = cosL * Magnitude;
FVector rightPositon = GetPawn()->GetActorLocation() + FVector(rightSensorPosition.X, rightSensorPosition.Y, 0.0f);
FVector leftPosition = GetPawn()->GetActorLocation() + FVector(leftSensorPosition.X, leftSensorPosition.Y, 0.0f);
FVector rightPositon = GetPawn()->GetActorLocation() + FVector(rightSensorPosition.X,
rightSensorPosition.Y,
0.0f);
FVector leftPosition = GetPawn()->GetActorLocation() + FVector(leftSensorPosition.X,
leftSensorPosition.Y,
0.0f);
FRoadMapPixelData rightRoadData = RoadMap->GetDataAt(rightPositon);
if (!rightRoadData.IsRoad()) { steering -= 0.2f;}
if (!rightRoadData.IsRoad())
{
steering -= 0.2f;
}
FRoadMapPixelData leftRoadData = RoadMap->GetDataAt(leftPosition);
if (!leftRoadData.IsRoad()) { steering += 0.2f;}
if (!leftRoadData.IsRoad())
{
steering += 0.2f;
}
FRoadMapPixelData roadData = RoadMap->GetDataAt(GetPawn()->GetActorLocation());
if (!roadData.IsRoad()) {
if (!roadData.IsRoad())
{
steering = 0.0f;
} else if (roadData.HasDirection()) {
}
else if (roadData.HasDirection())
{
direction = roadData.GetDirection();
FVector right = rightRoadData.GetDirection();
@ -332,30 +378,61 @@ float AWheeledVehicleAIController::CalcStreeringValue(FVector &direction)
actorAngle *= (180.0 / PI);
float min = dirAngle - 90.0f;
if (min < -180.0f) { min = 180.0f + (min + 180.0f);}
if (min < -180.0f)
{
min = 180.0f + (min + 180.0f);
}
float max = dirAngle + 90.0f;
if (max > 180.0f) { max = -180.0f + (max - 180.0f);}
if (max > 180.0f)
{
max = -180.0f + (max - 180.0f);
}
if (dirAngle < -90.0 || dirAngle > 90.0) {
if (rightAngle < min && rightAngle > max) { steering -= 0.2f;}
if (leftAngle < min && leftAngle > max) { steering += 0.2f;}
} else {
if (rightAngle < min || rightAngle > max) { steering -= 0.2f;}
if (leftAngle < min || leftAngle > max) { steering += 0.2f;}
if (dirAngle < -90.0 || dirAngle > 90.0)
{
if (rightAngle < min && rightAngle > max)
{
steering -= 0.2f;
}
if (leftAngle < min && leftAngle > max)
{
steering += 0.2f;
}
}
else
{
if (rightAngle < min || rightAngle > max)
{
steering -= 0.2f;
}
if (leftAngle < min || leftAngle > max)
{
steering += 0.2f;
}
}
float angle = dirAngle - actorAngle;
if (angle > 180.0f) { angle -= 360.0f;} else if (angle < -180.0f) {
if (angle > 180.0f)
{
angle -= 360.0f;
}
else if (angle < -180.0f)
{
angle += 360.0f;
}
if (angle < -MaximumSteerAngle) {
if (angle < -MaximumSteerAngle)
{
steering = -1.0f;
} else if (angle > MaximumSteerAngle) {
}
else if (angle > MaximumSteerAngle)
{
steering = 1.0f;
} else {
}
else
{
steering += angle / MaximumSteerAngle;
}
}
@ -364,16 +441,23 @@ float AWheeledVehicleAIController::CalcStreeringValue(FVector &direction)
return steering;
}
float AWheeledVehicleAIController::Stop(const float Speed) {
float AWheeledVehicleAIController::Stop(const float Speed)
{
return (Speed >= 1.0f ? -Speed / SpeedLimit : 0.0f);
}
float AWheeledVehicleAIController::Move(const float Speed) {
if (Speed >= SpeedLimit) {
float AWheeledVehicleAIController::Move(const float Speed)
{
if (Speed >= SpeedLimit)
{
return Stop(Speed);
} else if (Speed >= SpeedLimit - 10.0f) {
}
else if (Speed >= SpeedLimit - 10.0f)
{
return 0.5f;
} else {
}
else
{
return 1.0f;
}
}