24 const double epsilon = 0.00001;
26 auto References = GetAllReferencesToThisSignal(Map);
28 for (
auto& Reference : References)
30 auto RoadId = Reference.first;
31 const auto* SignalReference = Reference.second;
32 TSet<carla::road::RoadId> SignalPredecessors;
34 for(
auto &validity : SignalReference->GetValidities())
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());
46 auto signal_waypoint = signal_waypoint_optional.value();
51 auto box_waypoint = signal_waypoint;
55 if (predecessors.size() == 1) {
56 auto predecessor = predecessors.front();
58 box_waypoint = predecessor;
64 float BoxWidth =
static_cast<float>(
66 float BoxLength = 1.5f;
67 float BoxHeight = 1.0f;
70 BoxWidth = std::max(0.01f, BoxWidth);
75 float AdditionalDistance = 1.5f;
78 box_waypoint.s = FMath::Clamp(box_waypoint.s - (BoxLength + AdditionalDistance),
79 LaneDistance + epsilon, LaneDistance + LaneLength - epsilon);
83 box_waypoint.s = FMath::Clamp(box_waypoint.s + (BoxLength + AdditionalDistance),
84 LaneDistance + epsilon, LaneDistance + LaneLength - epsilon);
92 GenerateYieldBox(BoxTransform, FVector(100*BoxLength, 100*BoxWidth, 100*BoxHeight));
95 for(
auto &Prev : Predecessors)
97 if(!SignalPredecessors.Contains(Prev.road_id))
99 SignalPredecessors.Add(Prev.road_id);
110 if(
Junction->RoadHasConflicts(RoadId))
112 const auto &ConflictingRoads =
Junction->GetConflictsOfRoad(RoadId);
113 for(
const auto &Conflict : ConflictingRoads)
116 for(
auto& Waypoint : Waypoints)
119 bool bHasSamePredecessor =
false;
121 for(
auto &Prev : Predecessors)
123 if(SignalPredecessors.Contains(Prev.road_id))
125 bHasSamePredecessor =
true;
128 if(bHasSamePredecessor)
137 auto CurrentWaypoint = Waypoint;
138 auto NextWaypoint = CurrentWaypoint;
139 float BoxSize =
static_cast<float>(
143 BoxSize = std::max(0.01f, BoxSize);
144 float UEBoxSize = 100*BoxSize;
152 GenerateCheckBox(BoxTransform, UEBoxSize);
156 auto Next = Map.
GetNext(NextWaypoint, 2*BoxSize);
157 if (Next.size() != 1)
161 NextWaypoint = Next.front();
162 if(NextWaypoint.road_id != Waypoint.road_id)
171 GenerateCheckBox(BoxTransform, UEBoxSize);
176 double AnticipationTime = 0.1;
177 auto Previous = Map.
GetPrevious(Waypoint, 2*BoxSize);
178 std::queue<std::pair<float, carla::road::element::Waypoint>>
180 for (
auto & Prev : Previous)
182 WaypointQueue.push({AnticipationTime, Prev});
184 while (!WaypointQueue.empty())
186 auto CurrentElement = WaypointQueue.front();
194 Speed = InfoSpeed->GetSpeed();
196 float RemainingTime = CurrentElement.first - BoxSize/Speed;
197 if(RemainingTime > 0)
199 Previous = Map.
GetPrevious(CurrentElement.second, 2*BoxSize);
200 for (
auto & Prev : Previous)
202 WaypointQueue.push({RemainingTime, Prev});
213void UYieldSignComponent::GenerateYieldBox(
const FTransform BoxTransform,
214 const FVector BoxSize)
216 UBoxComponent* BoxComponent = GenerateTriggerBox(BoxTransform, BoxSize);
217 BoxComponent->OnComponentBeginOverlap.AddDynamic(
this, &UYieldSignComponent::OnOverlapBeginYieldEffectBox);
218 BoxComponent->OnComponentEndOverlap.AddDynamic(
this, &UYieldSignComponent::OnOverlapEndYieldEffectBox);
219 AddEffectTriggerVolume(BoxComponent);
222void UYieldSignComponent::GenerateCheckBox(
const FTransform BoxTransform,
225 UBoxComponent* BoxComponent = GenerateTriggerBox(BoxTransform, BoxSize);
226 BoxComponent->OnComponentBeginOverlap.AddDynamic(
this, &UYieldSignComponent::OnOverlapBeginYieldCheckBox);
227 BoxComponent->OnComponentEndOverlap.AddDynamic(
this, &UYieldSignComponent::OnOverlapEndYieldCheckBox);
230void UYieldSignComponent::GiveWayIfPossible()
232 if (VehiclesToCheck.Num() == 0)
234 for (
auto Vehicle : VehiclesInYield)
237 Cast<AWheeledVehicleAIController>(
Vehicle->GetController());
243 if(VehiclesInYield.Num())
245 for (
auto Vehicle : VehiclesInYield)
248 Cast<AWheeledVehicleAIController>(
Vehicle->GetController());
252 DelayedGiveWay(0.5f);
257void UYieldSignComponent::DelayedGiveWay(
float Delay)
259 FTimerHandle TimerHandler;
260 GetWorld()->GetTimerManager().
261 SetTimer(TimerHandler,
this, &UYieldSignComponent::GiveWayIfPossible, Delay);
264void UYieldSignComponent::OnOverlapBeginYieldEffectBox(UPrimitiveComponent *OverlappedComp,
266 UPrimitiveComponent *OtherComp,
267 int32 OtherBodyIndex,
269 const FHitResult &SweepResult)
275 Cast<AWheeledVehicleAIController>(
Vehicle->GetController());
276 if (VehicleController)
279 RemoveSameVehicleInBothLists();
285void UYieldSignComponent::OnOverlapEndYieldEffectBox(UPrimitiveComponent *OverlappedComp,
287 UPrimitiveComponent *OtherComp,
288 int32 OtherBodyIndex)
293 VehiclesInYield.Remove(
Vehicle);
297void UYieldSignComponent::OnOverlapBeginYieldCheckBox(UPrimitiveComponent *OverlappedComp,
299 UPrimitiveComponent *OtherComp,
300 int32 OtherBodyIndex,
302 const FHitResult &SweepResult)
307 if(!VehiclesInYield.Contains(
Vehicle))
309 if (!VehiclesToCheck.Contains(
Vehicle))
311 VehiclesToCheck.Add(
Vehicle, 0);
319void UYieldSignComponent::OnOverlapEndYieldCheckBox(UPrimitiveComponent *OverlappedComp,
321 UPrimitiveComponent *OtherComp,
322 int32 OtherBodyIndex)
327 if(VehiclesToCheck.Contains(
Vehicle))
330 if(VehiclesToCheck[
Vehicle] <= 0)
332 VehiclesToCheck.Remove(
Vehicle);
339void UYieldSignComponent::RemoveSameVehicleInBothLists()
341 for(
auto*
Vehicle : VehiclesInYield)
343 if(VehiclesToCheck.Contains(
Vehicle))
345 VehiclesToCheck.Remove(
Vehicle);
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)
double GetDistance() const
JuncId GetJunctionId(RoadId road_id) const
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...
bool IsJunction(RoadId road_id) const
double GetLaneWidth(Waypoint waypoint) const
std::optional< element::Waypoint > GetWaypoint(const geom::Location &location, int32_t lane_type=static_cast< int32_t >(Lane::LaneType::Driving)) const
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.
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.
geom::Transform ComputeTransform(Waypoint waypoint) const
std::vector< Waypoint > GetPredecessors(Waypoint waypoint) const
const Lane & GetLane(Waypoint waypoint) const
========================================================================
const T * GetInfo(const double s) const
carla::SharedPtr< carla::client::Junction > Junction
static void log_warning(Args &&... args)