Esempio n. 1
0
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();
}
Esempio n. 7
0
void TrafficLightPlugin::SignalStateCallback(const runtime_manager::traffic_light::ConstPtr& msg) {
    StateInfo info = {};
    GetStateInfo(static_cast<StateNumber>(msg->traffic_light), info);
    SetStateInfo(info);
} // TrafficLightPlugin::SignalStateCallback()