CARLA
TrafficLightComponent.cpp
Go to the documentation of this file.
1 // Copyright (c) 2020 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 
9 #include "Kismet/GameplayStatics.h"
10 #include "TrafficLightController.h"
11 #include "TrafficLightGroup.h"
12 #include "TrafficLightInterface.h"
13 #include "TrafficLightManager.h"
17 
21 
22 UTrafficLightComponent::UTrafficLightComponent()
23  : Super()
24 {
25 }
26 
27 void UTrafficLightComponent::InitializeSign(const carla::road::Map &Map)
28 {
29  const double epsilon = 0.00001;
30 
31  auto References = GetAllReferencesToThisSignal(Map);
32 
33  for (auto& Reference : References)
34  {
35  auto RoadId = Reference.first;
36  const auto* SignalReference = Reference.second;
37  for(auto &validity : SignalReference->GetValidities())
38  {
39  for(auto lane : carla::geom::Math::GenerateRange(validity._from_lane, validity._to_lane))
40  {
41  if(lane == 0)
42  continue;
43 
44  carla::road::element::Waypoint signal_waypoint;
45  boost::optional<carla::road::element::Waypoint> opt_signal_waypoint = Map.GetWaypoint(
46  RoadId, lane, SignalReference->GetS());
47  if(opt_signal_waypoint){
48  signal_waypoint = opt_signal_waypoint.get();
49  }else{
50  UE_LOG(LogCarla, Error, TEXT("signal_waypoint is not valid") );
51  continue;
52  }
53  // Prevent adding the bounding box inside the intersection
54  if (Map.IsJunction(RoadId)) {
55  auto predecessors = Map.GetPredecessors(signal_waypoint);
56  if (predecessors.size() == 1) {
57  auto predecessor = predecessors.front();
58  if (!Map.IsJunction(predecessor.road_id)) {
59  signal_waypoint = predecessor;
60  }
61  }
62  }
63 
64  if(Map.GetLane(signal_waypoint).GetType() != cr::Lane::LaneType::Driving)
65  continue;
66 
67  // Get 50% of the half size of the width of the lane
68  float BoxWidth = static_cast<float>(
69  0.5f*Map.GetLaneWidth(signal_waypoint)*0.5);
70  float BoxLength = 1.5f;
71  float BoxHeight = 1.0f;
72 
73  // Prevent a situation where the road width is 0,
74  // this could happen in a lane that is just appearing
75  BoxWidth = std::max(0.01f, BoxWidth);
76  // Get min and max
77  double LaneLength = Map.GetLane(signal_waypoint).GetLength();
78  double LaneDistance = Map.GetLane(signal_waypoint).GetDistance();
79  // Safe distance to avoid overlapping the bounding box with the intersection
80  float AdditionalDistance = 1.5f;
81  if(lane < 0)
82  {
83  signal_waypoint.s = FMath::Clamp(signal_waypoint.s - (BoxLength + AdditionalDistance),
84  LaneDistance + epsilon, LaneDistance + LaneLength - epsilon);
85  }
86  else
87  {
88  signal_waypoint.s = FMath::Clamp(signal_waypoint.s + (BoxLength + AdditionalDistance),
89  LaneDistance + epsilon, LaneDistance + LaneLength - epsilon);
90  }
91  FTransform BoxTransform = Map.ComputeTransform(signal_waypoint);
92  ALargeMapManager* LargeMapManager = UCarlaStatics::GetLargeMapManager(GetWorld());
93  if (LargeMapManager)
94  {
95  BoxTransform = LargeMapManager->GlobalToLocalTransform(BoxTransform);
96  }
97  GenerateTrafficLightBox(BoxTransform, FVector(100*BoxLength, 100*BoxWidth, 100*BoxHeight));
98  }
99  }
100  }
101 }
102 
103 void UTrafficLightComponent::GenerateTrafficLightBox(const FTransform BoxTransform,
104  const FVector BoxSize)
105 {
106  UBoxComponent* BoxComponent = GenerateTriggerBox(BoxTransform, BoxSize);
107  BoxComponent->OnComponentBeginOverlap.AddDynamic(this, &UTrafficLightComponent::OnBeginOverlapTriggerBox);
108  BoxComponent->OnComponentEndOverlap.AddDynamic(this, &UTrafficLightComponent::OnEndOverlapTriggerBox);
109  AddEffectTriggerVolume(BoxComponent);
110 }
111 
112 void UTrafficLightComponent::SetLightState(ETrafficLightState NewState)
113 {
114  LightState = NewState;
115  LightChangeDispatcher.Broadcast();
116  if (GetOwner()->Implements<UTrafficLightInterface>())
117  {
118  ITrafficLightInterface::Execute_LightChanged(GetOwner(), LightState);
119  }
120  ATrafficLightBase* OldTrafficLight = Cast<ATrafficLightBase>(GetOwner());
121  if (OldTrafficLight)
122  {
123  OldTrafficLight->LightChangedCompatibility(NewState);
124  }
125 
126  for (auto Controller : Vehicles)
127  {
128  if (Controller != nullptr)
129  {
130  Controller->SetTrafficLightState(LightState);
131  }
132  }
133 }
134 
135 ETrafficLightState UTrafficLightComponent::GetLightState() const
136 {
137  return LightState;
138 }
139 
140 void UTrafficLightComponent::SetFrozenGroup(bool InFreeze)
141 {
142  if (GetGroup())
143  {
144  GetGroup()->SetFrozenGroup(InFreeze);
145  }
146 }
147 
148 ATrafficLightGroup* UTrafficLightComponent::GetGroup()
149 {
150  return TrafficLightController->GetGroup();
151 }
152 
153 const ATrafficLightGroup* UTrafficLightComponent::GetGroup() const
154 {
155  return TrafficLightController->GetGroup();
156 }
157 
158 UTrafficLightController* UTrafficLightComponent::GetController()
159 {
160  return TrafficLightController;
161 }
162 
163 const UTrafficLightController* UTrafficLightComponent::GetController() const
164 {
165  return TrafficLightController;
166 }
167 
168 void UTrafficLightComponent::OnBeginOverlapTriggerBox(UPrimitiveComponent *OverlappedComp,
169  AActor *OtherActor,
170  UPrimitiveComponent *OtherComp,
171  int32 OtherBodyIndex,
172  bool bFromSweep,
173  const FHitResult &SweepResult)
174 {
175  ACarlaWheeledVehicle * Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
176  if (Vehicle)
177  {
178  AWheeledVehicleAIController* VehicleController =
179  Cast<AWheeledVehicleAIController>(Vehicle->GetController());
180  if (VehicleController)
181  {
182  VehicleController->SetTrafficLightState(LightState);
183  Vehicles.Add(VehicleController);
184  VehicleController->SetTrafficLight(Cast<ATrafficLightBase>(GetOwner()));
185  }
186  }
187 }
188 
189 void UTrafficLightComponent::OnEndOverlapTriggerBox(UPrimitiveComponent *OverlappedComp,
190  AActor *OtherActor,
191  UPrimitiveComponent *OtherComp,
192  int32 OtherBodyIndex)
193 {
194  ACarlaWheeledVehicle * Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
195  if (Vehicle)
196  {
197  AWheeledVehicleAIController* VehicleController =
198  Cast<AWheeledVehicleAIController>(Vehicle->GetController());
199  if (VehicleController)
200  {
201  VehicleController->SetTrafficLightState(ETrafficLightState::Green);
202  VehicleController->SetTrafficLight(nullptr);
203  Vehicles.Remove(VehicleController);
204  }
205  }
206 }
207 
208 void UTrafficLightComponent::SetController(UTrafficLightController* Controller)
209 {
210  TrafficLightController = Controller;
211 }
LaneType GetType() const
Definition: Lane.cpp:38
TArray< AWheeledVehicleAIController * > Vehicles
boost::optional< element::Waypoint > GetWaypoint(const geom::Location &location, int32_t lane_type=static_cast< int32_t >(Lane::LaneType::Driving)) const
Definition: road/Map.cpp:212
const Lane & GetLane(Waypoint waypoint) const
======================================================================== – Road information --------...
Definition: road/Map.cpp:834
Class which implements the state changing of traffic lights.
double GetDistance() const
Definition: Lane.cpp:46
double GetLength() const
Definition: Lane.cpp:51
void SetTrafficLight(ATrafficLightBase *InTrafficLight)
Set traffic light currently affecting this vehicle.
void SetTrafficLightState(ETrafficLightState InTrafficLightState)
Set traffic light state currently affecting this vehicle.
void LightChangedCompatibility(ETrafficLightState NewLightState)
Wheeled vehicle controller with optional AI.
geom::Transform ComputeTransform(Waypoint waypoint) const
Definition: road/Map.cpp:273
bool IsJunction(RoadId road_id) const
Definition: road/Map.cpp:302
std::vector< Waypoint > GetPredecessors(Waypoint waypoint) const
Definition: road/Map.cpp:536
static std::vector< int > GenerateRange(int a, int b)
Definition: Math.cpp:151
FTransform GlobalToLocalTransform(const FTransform &InTransform) const
static ALargeMapManager * GetLargeMapManager(const UObject *WorldContextObject)
Definition: CarlaStatics.h:100
Base class for CARLA wheeled vehicles.
Maps a controller from OpenDrive.
double GetLaneWidth(Waypoint waypoint) const
Definition: road/Map.cpp:285