CARLA
WheeledVehicleAIController.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include "Carla.h"
9 
10 #include "MapGen/RoadMap.h"
11 #include "Traffic/RoutePlanner.h"
13 #include "Carla/CityMapGenerator.h"
15 
16 #include "EngineUtils.h"
17 #include "GameFramework/Pawn.h"
18 #include "WheeledVehicleMovementComponent.h"
19 
20 // =============================================================================
21 // -- Static local methods -----------------------------------------------------
22 // =============================================================================
23 
24 static bool RayCast(const AActor &Actor, const FVector &Start, const FVector &End)
25 {
26  FHitResult OutHit;
27  static FName TraceTag = FName(TEXT("VehicleTrace"));
28  FCollisionQueryParams CollisionParams(TraceTag, true);
29  CollisionParams.AddIgnoredActor(&Actor);
30 
31  const bool Success = Actor.GetWorld()->LineTraceSingleByObjectType(
32  OutHit,
33  Start,
34  End,
35  FCollisionObjectQueryParams(FCollisionObjectQueryParams::AllDynamicObjects),
36  CollisionParams);
37 
38  // DrawDebugLine(Actor.GetWorld(), Start, End,
39  // Success ? FColor(255, 0, 0) : FColor(0, 255, 0), false);
40 
41  return Success && OutHit.bBlockingHit;
42 }
43 
46  const float Speed,
47  const FVector &Direction)
48 {
49  const auto ForwardVector = Vehicle.GetVehicleOrientation();
50  const auto VehicleBounds = Vehicle.GetVehicleBoundingBoxExtent();
51 
52  FVector NormDirection = Direction.GetSafeNormal();
53 
54  const float Distance = std::max(50.0f, Speed * Speed); // why?
55 
56  const FVector StartCenter = Vehicle.GetActorLocation() +
57  (ForwardVector * (250.0f + VehicleBounds.X / 2.0f)) + FVector(0.0f, 0.0f, 50.0f);
58  const FVector EndCenter = StartCenter + NormDirection * (Distance + VehicleBounds.X / 2.0f);
59 
60  const FVector StartRight = StartCenter +
61  (FVector(ForwardVector.Y, -ForwardVector.X, ForwardVector.Z) * 100.0f);
62  const FVector EndRight = StartRight + NormDirection * (Distance + VehicleBounds.X / 2.0f);
63 
64  const FVector StartLeft = StartCenter +
65  (FVector(-ForwardVector.Y, ForwardVector.X, ForwardVector.Z) * 100.0f);
66  const FVector EndLeft = StartLeft + NormDirection * (Distance + VehicleBounds.X / 2.0f);
67 
68  return
69  RayCast(Vehicle, StartCenter, EndCenter) ||
70  RayCast(Vehicle, StartRight, EndRight) ||
71  RayCast(Vehicle, StartLeft, EndLeft);
72 }
73 
74 template <typename T>
75 static void ClearQueue(std::queue<T> &Queue)
76 {
77  std::queue<T> EmptyQueue;
78  Queue.swap(EmptyQueue);
79 }
80 
81 // =============================================================================
82 // -- Constructor and destructor -----------------------------------------------
83 // =============================================================================
84 
85 AWheeledVehicleAIController::AWheeledVehicleAIController(const FObjectInitializer &ObjectInitializer)
86  : Super(ObjectInitializer)
87 {
88  RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
89 
91 
92  PrimaryActorTick.bCanEverTick = true;
93  PrimaryActorTick.TickGroup = TG_PrePhysics;
94 }
95 
97 
98 // =============================================================================
99 // -- AController --------------------------------------------------------------
100 // =============================================================================
101 
103 {
104  Super::OnPossess(aPawn);
105 
106  if (IsPossessingAVehicle())
107  {
108  UE_LOG(LogCarla, Error, TEXT("Controller already possessing a vehicle!"));
109  return;
110  }
111  Vehicle = Cast<ACarlaWheeledVehicle>(aPawn);
112  check(Vehicle != nullptr);
114  check(MaximumSteerAngle > 0.0f);
116 
117  if (RoadMap == nullptr)
118  {
119  TActorIterator<ACityMapGenerator> It(GetWorld());
120  RoadMap = (It ? It->GetRoadMap() : nullptr);
121  }
122 }
123 
125 {
126  Super::OnUnPossess();
127 
128  Vehicle = nullptr;
129 }
130 
131 void AWheeledVehicleAIController::Tick(const float DeltaTime)
132 {
133  TRACE_CPUPROFILER_EVENT_SCOPE(AWheeledVehicleAIController::Tick);
134  Super::Tick(DeltaTime);
135 
136  if (!IsPossessingAVehicle())
137  {
138  return;
139  }
140 
142  {
143  Vehicle->ApplyVehicleControl(FVehicleControl{}, EVehicleInputPriority::Relaxation);
144  }
145 
147 }
148 
149 // =============================================================================
150 // -- Autopilot ----------------------------------------------------------------
151 // =============================================================================
152 
153 void AWheeledVehicleAIController::ConfigureAutopilot(const bool Enable, const bool KeepState)
154 {
155  bAutopilotEnabled = Enable;
156  if (!KeepState)
157  {
158  // Reset state.
159  Vehicle->SetSteeringInput(0.0f);
160  Vehicle->SetThrottleInput(0.0f);
161  Vehicle->SetBrakeInput(0.0f);
162  Vehicle->SetReverse(false);
163  Vehicle->SetHandbrakeInput(false);
167  ECarlaWheeledVehicleState::FreeDriving :
168  ECarlaWheeledVehicleState::AutopilotOff);
169  }
170 
171  TrafficLightState = ETrafficLightState::Green;
172 }
173 
174 // =============================================================================
175 // -- Traffic ------------------------------------------------------------------
176 // =============================================================================
177 
179  const TArray<FVector> &Locations,
180  const bool bOverwriteCurrent)
181 {
182  if (bOverwriteCurrent)
183  {
185  }
186  for (auto &Location : Locations)
187  {
188  TargetLocations.emplace(Location);
189  }
190 }
void ApplyVehicleControl(const FVehicleControl &Control, EVehicleInputPriority Priority)
void SetFixedRoute(const TArray< FVector > &Locations, bool bOverwriteCurrent=true)
Set a fixed route to follow if autopilot is enabled.
void SetThrottleInput(float Value)
void SetAIVehicleState(ECarlaWheeledVehicleState InState)
void Tick(float DeltaTime) override
FVector GetVehicleBoundingBoxExtent() const
Extent of the vehicle&#39;s bounding box.
void OnPossess(APawn *aPawn) override
float GetMaximumSteerAngle() const
Get the maximum angle at which the front wheel can steer.
static void ClearQueue(std::queue< T > &Queue)
static bool RayCast(const AActor &Actor, const FVector &Start, const FVector &End)
geom::Location Location
Definition: rpc/Location.h:14
FVector GetVehicleOrientation() const
Orientation vector of the vehicle, pointing forward.
carla::SharedPtr< cc::Actor > Actor
void ConfigureAutopilot(const bool Enable, const bool KeepState=false)
void SetSteeringInput(float Value)
void Seed(int32 InSeed)
Seed the random engine.
Definition: RandomEngine.h:44
void SetHandbrakeInput(bool Value)
AWheeledVehicleAIController(const FObjectInitializer &ObjectInitializer)
static bool IsThereAnObstacleAhead(const ACarlaWheeledVehicle &Vehicle, const float Speed, const FVector &Direction)
void SetBrakeInput(float Value)
Base class for CARLA wheeled vehicles.
static int32 GenerateRandomSeed()
Generate a non-deterministic random seed.