TrafficLightPlugin::TrafficLightPlugin(QWidget* parent)
: rviz::Panel(parent) {
// Initialize Form
ui_.setupUi(this);
// Set initial state to "UNDEFINED"
StateInfo initial_info = {};
GetStateInfo(StateNumber::UNDEFINED, initial_info);
SetStateInfo(initial_info);
// Boot Callback function
signal_state_sub_ = node_handle_.subscribe("/light_color",
1,
&TrafficLightPlugin::SignalStateCallback,
this);
} // TrafficLightPlugin::TrafficLightPlugin()
void FAnimNode_StateMachine::Initialize(const FAnimationInitializeContext& Context)
{
UAnimBlueprintGeneratedClass* AnimBlueprintClass = Context.GetAnimBlueprintClass();
PRIVATE_MachineDescription = AnimBlueprintClass->BakedStateMachines.IsValidIndex(StateMachineIndexInClass) ? &(AnimBlueprintClass->BakedStateMachines[StateMachineIndexInClass]) : NULL;
if (FBakedAnimationStateMachine* Machine = GetMachineDescription())
{
ElapsedTime = 0.0f;
CurrentState = INDEX_NONE;
if (Machine->States.Num() > 0)
{
// Create a pose link for each state we can reach
StatePoseLinks.Empty(Machine->States.Num());
for (int32 StateIndex = 0; StateIndex < Machine->States.Num(); ++StateIndex)
{
FPoseLink* StatePoseLink = new (StatePoseLinks) FPoseLink();
// because conduits don't contain bound graphs, this link is no longer guaranteed to be valid
if (Machine->States[StateIndex].StateRootNodeIndex != INDEX_NONE)
{
StatePoseLink->LinkID = AnimBlueprintClass->AnimNodeProperties.Num() - 1 - Machine->States[StateIndex].StateRootNodeIndex; //@TODO: Crazysauce
}
}
// Reset transition related variables
StatesUpdated.Empty(StatesUpdated.Num());
ActiveTransitionArray.Empty(ActiveTransitionArray.Num());
// Move to the default state
SetState(Context, Machine->InitialState);
// initialize first update
bFirstUpdate = true;
// Fire off any entry notifications the default state has
Context.AnimInstance->AddAnimNotifyFromGeneratedClass(GetStateInfo(Machine->InitialState).StartNotify);
}
}
}
bool FAnimNode_StateMachine::IsAConduitState(int32 StateIndex) const
{
return ((PRIVATE_MachineDescription != NULL) && (StateIndex < PRIVATE_MachineDescription->States.Num())) ? GetStateInfo(StateIndex).bIsAConduit : false;
}
void FAnimNode_StateMachine::GatherDebugData(FNodeDebugData& DebugData)
{
FString DebugLine = DebugData.GetNodeName(this);
DebugLine += FString::Printf(TEXT("(%s->%s)"), *GetMachineDescription()->MachineName.ToString(), *GetStateInfo().StateName.ToString());
TMap<int32, float> StateWeightMap;
if (ActiveTransitionArray.Num() > 0)
{
for (int32 Index = 0; Index < ActiveTransitionArray.Num(); ++Index)
{
// if there is any source pose, blend it here
FAnimationActiveTransitionEntry& ActiveTransition = ActiveTransitionArray[Index];
if (ActiveTransition.bActive)
{
switch (ActiveTransition.LogicType)
{
case ETransitionLogicType::TLT_StandardBlend:
{
AddStateWeight(StateWeightMap, ActiveTransition.PreviousState, (1.0f - ActiveTransition.Alpha));
AddStateWeight(StateWeightMap, ActiveTransition.NextState, (ActiveTransition.Alpha));
break;
}
case ETransitionLogicType::TLT_Custom:
{
if (ActiveTransition.CustomTransitionGraph.LinkID != INDEX_NONE)
{
for (TArray<FAnimNode_TransitionPoseEvaluator*>::TIterator PoseEvaluatorListIt = ActiveTransition.PoseEvaluators.CreateIterator(); PoseEvaluatorListIt; ++PoseEvaluatorListIt)
{
FAnimNode_TransitionPoseEvaluator* Evaluator = *PoseEvaluatorListIt;
if (Evaluator->InputNodeNeedsUpdate())
{
const bool bUsePreviousState = (Evaluator->DataSource == EEvaluatorDataSource::EDS_SourcePose);
const int32 EffectiveStateIndex = bUsePreviousState ? ActiveTransition.PreviousState : ActiveTransition.NextState;
AddStateWeight(StateWeightMap, EffectiveStateIndex, 1.f);
}
}
}
break;
}
default:
{
break;
}
}
}
}
}
else if (!IsAConduitState(CurrentState))
{
StateWeightMap.Add(CurrentState) = 1.0f;
}
DebugData.AddDebugItem(DebugLine);
for (int32 PoseIndex = 0; PoseIndex < StatePoseLinks.Num(); ++PoseIndex)
{
float* WeightPtr = StateWeightMap.Find(PoseIndex);
float Weight = WeightPtr ? *WeightPtr : 0.f;
StatePoseLinks[PoseIndex].GatherDebugData(DebugData.BranchFlow(Weight));
}
}
bool FAnimNode_StateMachine::FindValidTransition(const FAnimationUpdateContext& Context, const FBakedAnimationState& StateInfo, /*out*/ FAnimationPotentialTransition& OutPotentialTransition, /*out*/ TArray<int32, TInlineAllocator<4>>& OutVisitedStateIndices)
{
// There is a possibility we'll revisit states connected through conduits,
// so we can avoid doing unnecessary work (and infinite loops) by caching off states we have already checked
const int32 CheckingStateIndex = GetStateIndex(StateInfo);
if (OutVisitedStateIndices.Contains(CheckingStateIndex))
{
return false;
}
OutVisitedStateIndices.Add(CheckingStateIndex);
const UAnimBlueprintGeneratedClass* AnimBlueprintClass = Context.GetAnimBlueprintClass();
// Conduit 'states' have an additional entry rule which must be true to consider taking any transitions via the conduit
//@TODO: It would add flexibility to be able to define this on normal state nodes as well, assuming the dual-graph editing is sorted out
if (FAnimNode_TransitionResult* StateEntryRuleNode = GetNodeFromPropertyIndex<FAnimNode_TransitionResult>(Context.AnimInstance, AnimBlueprintClass, StateInfo.EntryRuleNodeIndex))
{
if (StateEntryRuleNode->NativeTransitionDelegate.IsBound())
{
// attempt to evaluate native rule
StateEntryRuleNode->bCanEnterTransition = StateEntryRuleNode->NativeTransitionDelegate.Execute();
}
else
{
// Execute it and see if we can take this rule
StateEntryRuleNode->EvaluateGraphExposedInputs.Execute(Context);
}
// not ok, back out
if (!StateEntryRuleNode->bCanEnterTransition)
{
return false;
}
}
const int32 NumTransitions = StateInfo.Transitions.Num();
for (int32 TransitionIndex = 0; TransitionIndex < NumTransitions; ++TransitionIndex)
{
const FBakedStateExitTransition& TransitionRule = StateInfo.Transitions[TransitionIndex];
if (TransitionRule.CanTakeDelegateIndex == INDEX_NONE)
{
continue;
}
FAnimNode_TransitionResult* ResultNode = GetNodeFromPropertyIndex<FAnimNode_TransitionResult>(Context.AnimInstance, AnimBlueprintClass, TransitionRule.CanTakeDelegateIndex);
if (ResultNode->NativeTransitionDelegate.IsBound())
{
// attempt to evaluate native rule
ResultNode->bCanEnterTransition = ResultNode->NativeTransitionDelegate.Execute();
}
else
{
bool bStillCallEvaluate = true;
if (TransitionRule.StateSequencePlayerToQueryIndex != INDEX_NONE)
{
// Simple automatic rule
FAnimNode_SequencePlayer* SequencePlayer = GetNodeFromPropertyIndex<FAnimNode_SequencePlayer>(Context.AnimInstance, AnimBlueprintClass, TransitionRule.StateSequencePlayerToQueryIndex);
if ((SequencePlayer != nullptr) && (SequencePlayer->Sequence != nullptr))
{
const float SequenceLength = SequencePlayer->Sequence->GetMaxCurrentTime();
const float PlayerTime = SequencePlayer->InternalTimeAccumulator;
const float PlayerTimeLeft = SequenceLength - PlayerTime;
const FAnimationTransitionBetweenStates& TransitionInfo = GetTransitionInfo(TransitionRule.TransitionIndex);
ResultNode->bCanEnterTransition = (PlayerTimeLeft <= TransitionInfo.CrossfadeDuration);
bStillCallEvaluate = false;
}
}
if (bStillCallEvaluate)
{
// Execute it and see if we can take this rule
ResultNode->EvaluateGraphExposedInputs.Execute(Context);
}
}
if (ResultNode->bCanEnterTransition == TransitionRule.bDesiredTransitionReturnValue)
{
int32 NextState = GetTransitionInfo(TransitionRule.TransitionIndex).NextState;
const FBakedAnimationState& NextStateInfo = GetStateInfo(NextState);
// if next state is a conduit we want to check for transitions using that state as the root
if (NextStateInfo.bIsAConduit)
{
if (FindValidTransition(Context, NextStateInfo, /*out*/ OutPotentialTransition, /*out*/ OutVisitedStateIndices))
{
#if WITH_EDITORONLY_DATA
OutPotentialTransition.SourceTransitionIndices.Add(TransitionRule.TransitionIndex);
#endif
return true;
}
}
// otherwise we have found a content state, so we can record our potential transition
else
{
// clear out any potential transition we already have
OutPotentialTransition.Clear();
// fill out the potential transition information
OutPotentialTransition.TransitionRule = &TransitionRule;
OutPotentialTransition.TargetState = NextState;
#if WITH_EDITORONLY_DATA
OutPotentialTransition.SourceTransitionIndices.Add(TransitionRule.TransitionIndex);
#endif
return true;
}
}
}
return false;
}
void FAnimNode_StateMachine::Update(const FAnimationUpdateContext& Context)
{
if (FBakedAnimationStateMachine* Machine = GetMachineDescription())
{
if (Machine->States.Num() == 0)
{
return;
}
}
else
{
return;
}
SCOPE_CYCLE_COUNTER(STAT_AnimStateMachineUpdate);
bool bFoundValidTransition = false;
int32 TransitionCountThisFrame = 0;
int32 TransitionIndex = INDEX_NONE;
// Look for legal transitions to take; can move across multiple states in one frame (up to MaxTransitionsPerFrame)
do
{
bFoundValidTransition = false;
FAnimationPotentialTransition PotentialTransition;
{
SCOPE_CYCLE_COUNTER(STAT_AnimStateMachineFindTransition);
// Evaluate possible transitions out of this state
//@TODO: Evaluate if a set is better than an array for the probably low N encountered here
TArray<int32, TInlineAllocator<4>> VisitedStateIndices;
FindValidTransition(Context, GetStateInfo(), /*Out*/ PotentialTransition, /*Out*/ VisitedStateIndices);
}
// If transition is valid and not waiting on other conditions
if (PotentialTransition.IsValid())
{
bFoundValidTransition = true;
// let the latest transition know it has been interrupted
if ((ActiveTransitionArray.Num() > 0) && ActiveTransitionArray[ActiveTransitionArray.Num()-1].bActive)
{
Context.AnimInstance->AddAnimNotifyFromGeneratedClass(ActiveTransitionArray[ActiveTransitionArray.Num()-1].InterruptNotify);
}
const int32 PreviousState = CurrentState;
const int32 NextState = PotentialTransition.TargetState;
// Fire off Notifies for state transition
if (!bFirstUpdate)
{
Context.AnimInstance->AddAnimNotifyFromGeneratedClass(GetStateInfo(PreviousState).EndNotify);
Context.AnimInstance->AddAnimNotifyFromGeneratedClass(GetStateInfo(NextState).StartNotify);
}
// Get the current weight of the next state, which may be non-zero
const float ExistingWeightOfNextState = GetStateWeight(NextState);
// Push the transition onto the stack
const FAnimationTransitionBetweenStates& ReferenceTransition = GetTransitionInfo(PotentialTransition.TransitionRule->TransitionIndex);
FAnimationActiveTransitionEntry* NewTransition = new (ActiveTransitionArray) FAnimationActiveTransitionEntry(NextState, ExistingWeightOfNextState, PreviousState, ReferenceTransition);
NewTransition->InitializeCustomGraphLinks(Context, *(PotentialTransition.TransitionRule));
#if WITH_EDITORONLY_DATA
NewTransition->SourceTransitionIndices = PotentialTransition.SourceTransitionIndices;
#endif
if (!bFirstUpdate)
{
Context.AnimInstance->AddAnimNotifyFromGeneratedClass(NewTransition->StartNotify);
}
SetState(Context, NextState);
TransitionCountThisFrame++;
}
}
while (bFoundValidTransition && (TransitionCountThisFrame < MaxTransitionsPerFrame));
if (bFirstUpdate)
{
//Handle enter notify for "first" (after initial transitions) state
Context.AnimInstance->AddAnimNotifyFromGeneratedClass(GetStateInfo().StartNotify);
// in the first update, we don't like to transition from entry state
// so we throw out any transition data at the first update
ActiveTransitionArray.Reset();
bFirstUpdate = false;
}
StatesUpdated.Empty(StatesUpdated.Num());
// Tick the individual state/states that are active
if (ActiveTransitionArray.Num() > 0)
{
for (int32 Index = 0; Index < ActiveTransitionArray.Num(); ++Index)
{
// The custom graph will tick the needed states
bool bFinishedTrans = false;
// The custom graph will tick the needed states
ActiveTransitionArray[Index].Update(Context, CurrentState, /*out*/ bFinishedTrans);
if (bFinishedTrans)
{
// only play these events if it is the last transition (most recent, going to current state)
if (Index == (ActiveTransitionArray.Num() - 1))
{
Context.AnimInstance->AddAnimNotifyFromGeneratedClass(ActiveTransitionArray[Index].EndNotify);
Context.AnimInstance->AddAnimNotifyFromGeneratedClass(GetStateInfo().FullyBlendedNotify);
}
}
else
{
// transition is still active, so tick the required states
UpdateTransitionStates(Context, ActiveTransitionArray[Index]);
}
}
// remove finished transitions here, newer transitions ending means any older ones must complete as well
for (int32 Index = (ActiveTransitionArray.Num()-1); Index >= 0; --Index)
{
// if we find an inactive one, remove all older transitions and break out
if (!ActiveTransitionArray[Index].bActive)
{
ActiveTransitionArray.RemoveAt(0, Index+1);
break;
}
}
}
//@TODO: StatesUpdated.Contains is a linear search
// Update the only active state if there are no transitions still in flight
if (ActiveTransitionArray.Num() == 0 && !IsAConduitState(CurrentState) && !StatesUpdated.Contains(CurrentState))
{
StatePoseLinks[CurrentState].Update(Context);
}
ElapsedTime += Context.GetDeltaTime();
}
void TrafficLightPlugin::SignalStateCallback(const runtime_manager::traffic_light::ConstPtr& msg) {
StateInfo info = {};
GetStateInfo(static_cast<StateNumber>(msg->traffic_light), info);
SetStateInfo(info);
} // TrafficLightPlugin::SignalStateCallback()
