CARLA
Loading...
Searching...
No Matches
YieldSignComponent.cpp
Go to the documentation of this file.
1// Copyright (c) 2026 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
8#include "TrafficLightState.h"
9#include <queue>
10
14
19
20
21void UYieldSignComponent::InitializeSign(const carla::road::Map &Map)
22{
23
24 const double epsilon = 0.00001;
25
26 auto References = GetAllReferencesToThisSignal(Map);
27
28 for (auto& Reference : References)
29 {
30 auto RoadId = Reference.first;
31 const auto* SignalReference = Reference.second;
32 TSet<carla::road::RoadId> SignalPredecessors;
33 // Yield box
34 for(auto &validity : SignalReference->GetValidities())
35 {
36 for(auto lane : carla::geom::Math::GenerateRange(validity._from_lane, validity._to_lane))
37 {
38 if(lane == 0)
39 continue;
40
41 auto signal_waypoint_optional = Map.GetWaypoint(RoadId, lane, SignalReference->GetS());
42 if (!signal_waypoint_optional) {
43 carla::log_warning("YieldSignComponent::InitializeSignsignal() waypoint seems to be invalid, ignoring. RoadId:", RoadId, " LaneId:", lane, " s:", SignalReference->GetS());
44 continue;
45 }
46 auto signal_waypoint = signal_waypoint_optional.value();
47 if(Map.GetLane(signal_waypoint).GetType() != cr::Lane::LaneType::Driving) {
48 continue;
49 }
50
51 auto box_waypoint = signal_waypoint;
52 // Prevent adding the bounding box inside the intersection
53 if (Map.IsJunction(RoadId)) {
54 auto predecessors = Map.GetPredecessors(box_waypoint);
55 if (predecessors.size() == 1) {
56 auto predecessor = predecessors.front();
57 if (!Map.IsJunction(predecessor.road_id)) {
58 box_waypoint = predecessor;
59 }
60 }
61 }
62
63 // Get 50% of the half size of the width of the lane
64 float BoxWidth = static_cast<float>(
65 0.5f*Map.GetLaneWidth(box_waypoint)*0.5);
66 float BoxLength = 1.5f;
67 float BoxHeight = 1.0f;
68 // Prevent a situation where the road width is 0,
69 // this could happen in a lane that is just appearing
70 BoxWidth = std::max(0.01f, BoxWidth);
71 // Get min and max
72 double LaneLength = Map.GetLane(box_waypoint).GetLength();
73 double LaneDistance = Map.GetLane(box_waypoint).GetDistance();
74 // Safe distance to avoid overlapping the bounding box with the intersection
75 float AdditionalDistance = 1.5f;
76 if(lane < 0)
77 {
78 box_waypoint.s = FMath::Clamp(box_waypoint.s - (BoxLength + AdditionalDistance),
79 LaneDistance + epsilon, LaneDistance + LaneLength - epsilon);
80 }
81 else
82 {
83 box_waypoint.s = FMath::Clamp(box_waypoint.s + (BoxLength + AdditionalDistance),
84 LaneDistance + epsilon, LaneDistance + LaneLength - epsilon);
85 }
86 FTransform BoxTransform = Map.ComputeTransform(box_waypoint);
87 ALargeMapManager* LargeMapManager = UCarlaStatics::GetLargeMapManager(GetWorld());
88 if (LargeMapManager)
89 {
90 BoxTransform = LargeMapManager->GlobalToLocalTransform(BoxTransform);
91 }
92 GenerateYieldBox(BoxTransform, FVector(100*BoxLength, 100*BoxWidth, 100*BoxHeight));
93
94 auto Predecessors = Map.GetPredecessors(signal_waypoint);
95 for(auto &Prev : Predecessors)
96 {
97 if(!SignalPredecessors.Contains(Prev.road_id))
98 {
99 SignalPredecessors.Add(Prev.road_id);
100 }
101 }
102 }
103 }
104
105 //Check boxes
106 if(Map.IsJunction(RoadId))
107 {
108 auto JuncId = Map.GetJunctionId(RoadId);
109 const auto * Junction = Map.GetJunction(JuncId);
110 if(Junction->RoadHasConflicts(RoadId))
111 {
112 const auto &ConflictingRoads = Junction->GetConflictsOfRoad(RoadId);
113 for(const auto &Conflict : ConflictingRoads)
114 {
115 auto Waypoints = Map.GenerateWaypointsInRoad(Conflict);
116 for(auto& Waypoint : Waypoints)
117 {
118 // Skip roads that share the same previous road
119 bool bHasSamePredecessor = false;
120 auto Predecessors = Map.GetPredecessors(Waypoint);
121 for(auto &Prev : Predecessors)
122 {
123 if(SignalPredecessors.Contains(Prev.road_id))
124 {
125 bHasSamePredecessor = true;
126 }
127 }
128 if(bHasSamePredecessor)
129 {
130 continue;
131 }
132
133 if(Map.GetLane(Waypoint).GetType() != cr::Lane::LaneType::Driving)
134 continue;
135
136 // Cover the road within the junction
137 auto CurrentWaypoint = Waypoint;
138 auto NextWaypoint = CurrentWaypoint;
139 float BoxSize = static_cast<float>(
140 0.9*Map.GetLaneWidth(NextWaypoint)/2.0);
141 // Prevent a situation where the road width is 0,
142 // this could happen in a lane that is just appearing
143 BoxSize = std::max(0.01f, BoxSize);
144 float UEBoxSize = 100*BoxSize;
145
146 FTransform BoxTransform = Map.ComputeTransform(NextWaypoint);
147 ALargeMapManager* LargeMapManager = UCarlaStatics::GetLargeMapManager(GetWorld());
148 if (LargeMapManager)
149 {
150 BoxTransform = LargeMapManager->GlobalToLocalTransform(BoxTransform);
151 }
152 GenerateCheckBox(BoxTransform, UEBoxSize);
153
154 while (true)
155 {
156 auto Next = Map.GetNext(NextWaypoint, 2*BoxSize);
157 if (Next.size() != 1)
158 {
159 break;
160 }
161 NextWaypoint = Next.front();
162 if(NextWaypoint.road_id != Waypoint.road_id)
163 {
164 break;
165 }
166 BoxTransform = Map.ComputeTransform(NextWaypoint);
167 if (LargeMapManager)
168 {
169 BoxTransform = LargeMapManager->GlobalToLocalTransform(BoxTransform);
170 }
171 GenerateCheckBox(BoxTransform, UEBoxSize);
172
173 }
174 // Cover the road before the junction
175 // Hard coded anticipation time (boxes placed prior to the junction)
176 double AnticipationTime = 0.1;
177 auto Previous = Map.GetPrevious(Waypoint, 2*BoxSize);
178 std::queue<std::pair<float, carla::road::element::Waypoint>>
179 WaypointQueue;
180 for (auto & Prev : Previous)
181 {
182 WaypointQueue.push({AnticipationTime, Prev});
183 }
184 while (!WaypointQueue.empty())
185 {
186 auto CurrentElement = WaypointQueue.front();
187 WaypointQueue.pop();
188 GenerateCheckBox(Map.ComputeTransform(CurrentElement.second), UEBoxSize);
189
190 float Speed = 40;
191 auto* InfoSpeed = Map.GetLane(CurrentElement.second).GetRoad()->GetInfo<carla::road::element::RoadInfoSpeed>(CurrentElement.second.s);
192 if(InfoSpeed)
193 {
194 Speed = InfoSpeed->GetSpeed();
195 }
196 float RemainingTime = CurrentElement.first - BoxSize/Speed;
197 if(RemainingTime > 0)
198 {
199 Previous = Map.GetPrevious(CurrentElement.second, 2*BoxSize);
200 for (auto & Prev : Previous)
201 {
202 WaypointQueue.push({RemainingTime, Prev});
203 }
204 }
205 }
206 }
207 }
208 }
209 }
210 }
211}
212
213void UYieldSignComponent::GenerateYieldBox(const FTransform BoxTransform,
214 const FVector BoxSize)
215{
216 UBoxComponent* BoxComponent = GenerateTriggerBox(BoxTransform, BoxSize);
217 BoxComponent->OnComponentBeginOverlap.AddDynamic(this, &UYieldSignComponent::OnOverlapBeginYieldEffectBox);
218 BoxComponent->OnComponentEndOverlap.AddDynamic(this, &UYieldSignComponent::OnOverlapEndYieldEffectBox);
219 AddEffectTriggerVolume(BoxComponent);
220}
221
222void UYieldSignComponent::GenerateCheckBox(const FTransform BoxTransform,
223 float BoxSize)
224{
225 UBoxComponent* BoxComponent = GenerateTriggerBox(BoxTransform, BoxSize);
226 BoxComponent->OnComponentBeginOverlap.AddDynamic(this, &UYieldSignComponent::OnOverlapBeginYieldCheckBox);
227 BoxComponent->OnComponentEndOverlap.AddDynamic(this, &UYieldSignComponent::OnOverlapEndYieldCheckBox);
228}
229
230void UYieldSignComponent::GiveWayIfPossible()
231{
232 if (VehiclesToCheck.Num() == 0)
233 {
234 for (auto Vehicle : VehiclesInYield)
235 {
236 AWheeledVehicleAIController* VehicleController =
237 Cast<AWheeledVehicleAIController>(Vehicle->GetController());
238 VehicleController->SetTrafficLightState(ETrafficLightState::Green);
239 }
240 }
241 else
242 {
243 if(VehiclesInYield.Num())
244 {
245 for (auto Vehicle : VehiclesInYield)
246 {
247 AWheeledVehicleAIController* VehicleController =
248 Cast<AWheeledVehicleAIController>(Vehicle->GetController());
249 VehicleController->SetTrafficLightState(ETrafficLightState::Red);
250 }
251 // 0.5 second delay
252 DelayedGiveWay(0.5f);
253 }
254 }
255}
256
257void UYieldSignComponent::DelayedGiveWay(float Delay)
258{
259 FTimerHandle TimerHandler;
260 GetWorld()->GetTimerManager().
261 SetTimer(TimerHandler, this, &UYieldSignComponent::GiveWayIfPossible, Delay);
262}
263
264void UYieldSignComponent::OnOverlapBeginYieldEffectBox(UPrimitiveComponent *OverlappedComp,
265 AActor *OtherActor,
266 UPrimitiveComponent *OtherComp,
267 int32 OtherBodyIndex,
268 bool bFromSweep,
269 const FHitResult &SweepResult)
270{
271 ACarlaWheeledVehicle * Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
272 if (Vehicle)
273 {
274 AWheeledVehicleAIController* VehicleController =
275 Cast<AWheeledVehicleAIController>(Vehicle->GetController());
276 if (VehicleController)
277 {
278 VehiclesInYield.Add(Vehicle);
279 RemoveSameVehicleInBothLists();
280 GiveWayIfPossible();
281 }
282 }
283}
284
285void UYieldSignComponent::OnOverlapEndYieldEffectBox(UPrimitiveComponent *OverlappedComp,
286 AActor *OtherActor,
287 UPrimitiveComponent *OtherComp,
288 int32 OtherBodyIndex)
289{
290 ACarlaWheeledVehicle * Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
291 if (Vehicle)
292 {
293 VehiclesInYield.Remove(Vehicle);
294 }
295}
296
297void UYieldSignComponent::OnOverlapBeginYieldCheckBox(UPrimitiveComponent *OverlappedComp,
298 AActor *OtherActor,
299 UPrimitiveComponent *OtherComp,
300 int32 OtherBodyIndex,
301 bool bFromSweep,
302 const FHitResult &SweepResult)
303{
304 ACarlaWheeledVehicle * Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
305 if (Vehicle)
306 {
307 if(!VehiclesInYield.Contains(Vehicle))
308 {
309 if (!VehiclesToCheck.Contains(Vehicle))
310 {
311 VehiclesToCheck.Add(Vehicle, 0);
312 }
313 VehiclesToCheck[Vehicle]++;
314 }
315 GiveWayIfPossible();
316 }
317}
318
319void UYieldSignComponent::OnOverlapEndYieldCheckBox(UPrimitiveComponent *OverlappedComp,
320 AActor *OtherActor,
321 UPrimitiveComponent *OtherComp,
322 int32 OtherBodyIndex)
323{
324 ACarlaWheeledVehicle * Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
325 if (Vehicle)
326 {
327 if(VehiclesToCheck.Contains(Vehicle))
328 {
329 VehiclesToCheck[Vehicle]--;
330 if(VehiclesToCheck[Vehicle] <= 0)
331 {
332 VehiclesToCheck.Remove(Vehicle);
333 }
334 }
335 // 0.5 second delay
336 DelayedGiveWay(0.5);
337 }
338}
339void UYieldSignComponent::RemoveSameVehicleInBothLists()
340{
341 for(auto* Vehicle : VehiclesInYield)
342 {
343 if(VehiclesToCheck.Contains(Vehicle))
344 {
345 VehiclesToCheck.Remove(Vehicle);
346 }
347 }
348}
Base class for CARLA wheeled vehicles.
FTransform GlobalToLocalTransform(const FTransform &InTransform) const
Wheeled vehicle controller with optional AI.
void SetTrafficLightState(ETrafficLightState InTrafficLightState)
Set traffic light state currently affecting this vehicle.
static ALargeMapManager * GetLargeMapManager(const UObject *WorldContextObject)
static std::vector< int > GenerateRange(int a, int b)
Definition Math.cpp:159
Road * GetRoad() const
Definition road/Lane.cpp:29
LaneType GetType() const
Definition road/Lane.cpp:38
double GetDistance() const
Definition road/Lane.cpp:46
double GetLength() const
Definition road/Lane.cpp:51
JuncId GetJunctionId(RoadId road_id) const
Definition road/Map.cpp:303
std::vector< Waypoint > GetPrevious(Waypoint waypoint, double distance) const
Return the list of waypoints at distance in the reversed direction that a vehicle at waypoint could d...
Definition road/Map.cpp:718
bool IsJunction(RoadId road_id) const
Definition road/Map.cpp:307
double GetLaneWidth(Waypoint waypoint) const
Definition road/Map.cpp:290
std::optional< element::Waypoint > GetWaypoint(const geom::Location &location, int32_t lane_type=static_cast< int32_t >(Lane::LaneType::Driving)) const
Definition road/Map.cpp:217
std::vector< Waypoint > GetNext(Waypoint waypoint, double distance) const
Return the list of waypoints at distance such that a vehicle at waypoint could drive to.
Definition road/Map.cpp:669
Junction * GetJunction(JuncId id)
std::vector< Waypoint > GenerateWaypointsInRoad(RoadId road_id, Lane::LaneType lane_type=Lane::LaneType::Driving) const
Generate waypoints at the entry of each lane of the specified road.
Definition road/Map.cpp:854
geom::Transform ComputeTransform(Waypoint waypoint) const
Definition road/Map.cpp:278
std::vector< Waypoint > GetPredecessors(Waypoint waypoint) const
Definition road/Map.cpp:643
const Lane & GetLane(Waypoint waypoint) const
========================================================================
Definition road/Map.cpp:994
const T * GetInfo(const double s) const
Definition road/Road.h:122
carla::SharedPtr< carla::client::Junction > Junction
static void log_warning(Args &&... args)
Definition Logging.h:96