void mtsTeleOperationECM::TransitionEnabled(void) { std::string armState; // check ECM state mECM.GetCurrentState(armState); if (armState != "READY") { mInterface->SendWarning(this->GetName() + ": ECM state has changed to [" + armState + "]"); mTeleopState.SetDesiredState("DISABLED"); } // check mtml state mMTML.GetCurrentState(armState); if (armState != "READY") { mInterface->SendWarning(this->GetName() + ": MTML state has changed to [" + armState + "]"); mTeleopState.SetDesiredState("DISABLED"); } // check mtmr state mMTMR.GetCurrentState(armState); if (armState != "READY") { mInterface->SendWarning(this->GetName() + ": MTMR state has changed to [" + armState + "]"); mTeleopState.SetDesiredState("DISABLED"); } if (mTeleopState.DesiredStateIsNotCurrent()) { SetFollowing(false); mTeleopState.SetCurrentState(mTeleopState.DesiredState()); } }
// This function lets a slave refollow his master bool CreatureLinkingHolder::TryFollowMaster(Creature* pCreature) { CreatureLinkingInfo const* pInfo = sCreatureLinkingMgr.GetLinkedTriggerInformation(pCreature); if (!pInfo || !(pInfo->linkingFlag & FLAG_FOLLOW)) return false; Creature* pMaster = NULL; if (pInfo->mapId != INVALID_MAP_ID) // entry case { BossGuidMapBounds finds = m_masterGuid.equal_range(pInfo->masterId); for (BossGuidMap::iterator itr = finds.first; itr != finds.second; ++itr) { pMaster = pCreature->GetMap()->GetCreature(itr->second); if (pMaster && IsSlaveInRangeOfBoss(pCreature, pMaster, pInfo->searchRange)) break; } } else // guid case { CreatureData const* masterData = sObjectMgr.GetCreatureData(pInfo->masterDBGuid); CreatureInfo const* cInfo = ObjectMgr::GetCreatureTemplate(masterData->id); pMaster = pCreature->GetMap()->GetCreature(ObjectGuid(cInfo->GetHighGuid(), cInfo->Entry, pInfo->masterDBGuid)); } if (pMaster && pMaster->isAlive()) { SetFollowing(pCreature, pMaster); return true; } return false; }
// Helper function, to process a single slave void CreatureLinkingHolder::ProcessSlave(CreatureLinkingEvent eventType, Creature* pSource, uint32 flag, Creature* pSlave, Unit* pEnemy) { switch (eventType) { case LINKING_EVENT_AGGRO: if (flag & FLAG_AGGRO_ON_AGGRO) { if (pSlave->IsControlledByPlayer()) return; if (pSlave->isInCombat()) pSlave->SetInCombatWith(pEnemy); else pSlave->AI()->AttackStart(pEnemy); } break; case LINKING_EVENT_EVADE: if (flag & FLAG_DESPAWN_ON_EVADE && pSlave->isAlive()) pSlave->ForcedDespawn(); if (flag & FLAG_RESPAWN_ON_EVADE && !pSlave->isAlive()) pSlave->Respawn(); break; case LINKING_EVENT_DIE: if (flag & FLAG_SELFKILL_ON_DEATH && pSlave->isAlive()) pSlave->DealDamage(pSlave, pSlave->GetHealth(), NULL, DIRECT_DAMAGE, SPELL_SCHOOL_MASK_NORMAL, NULL, false); if (flag & FLAG_DESPAWN_ON_DEATH && pSlave->isAlive()) pSlave->ForcedDespawn(); if (flag & FLAG_RESPAWN_ON_DEATH && !pSlave->isAlive()) pSlave->Respawn(); break; case LINKING_EVENT_RESPAWN: if (flag & FLAG_RESPAWN_ON_RESPAWN) { // Additional check to prevent endless loops (in case whole group respawns on first respawn) if (!pSlave->isAlive() && pSlave->GetRespawnTime() > time(NULL)) pSlave->Respawn(); } else if (flag & FLAG_DESPAWN_ON_RESPAWN && pSlave->isAlive()) pSlave->ForcedDespawn(); if (flag & FLAG_FOLLOW && pSlave->isAlive() && !pSlave->isInCombat()) SetFollowing(pSlave, pSource); break; case LINKING_EVENT_DESPAWN: if (flag & FLAG_DESPAWN_ON_DESPAWN && !pSlave->IsDespawned()) pSlave->ForcedDespawn(); break; } }
// This function lets a slave refollow his master bool CreatureLinkingHolder::TryFollowMaster(Creature* pCreature) { CreatureLinkingInfo const* pInfo = sCreatureLinkingMgr.GetLinkedTriggerInformation(pCreature); if (!pInfo || !(pInfo->linkingFlag & FLAG_FOLLOW)) return false; BossGuidMapBounds finds = m_masterGuid.equal_range(pInfo->masterId); for (BossGuidMap::iterator itr = finds.first; itr != finds.second; ++itr) { Creature* pMaster = pCreature->GetMap()->GetCreature(itr->second); if (pMaster && pMaster->isAlive() && IsSlaveInRangeOfBoss(pCreature, pMaster, pInfo->searchRange)) { SetFollowing(pCreature, pMaster); return true; } } return false; }
void mtsTeleOperationECM::Clutch(const bool & clutch) { // if the teleoperation is activated if (clutch) { SetFollowing(false); mInterface->SendStatus(this->GetName() + ": console clutch pressed"); // set MTMs in effort mode, no force applied but gravity and locked orientation prmForceCartesianSet wrench; mMTML.SetWrenchBody(wrench); mMTML.SetGravityCompensation(true); mMTML.LockOrientation(mMTML.PositionCartesianCurrent.Position().Rotation()); mMTMR.SetWrenchBody(wrench); mMTMR.SetGravityCompensation(true); mMTMR.LockOrientation(mMTMR.PositionCartesianCurrent.Position().Rotation()); } else { mIsClutched = false; mInterface->SendStatus(this->GetName() + ": console clutch released"); mTeleopState.SetCurrentState("SETTING_ARMS_STATE"); } }
// Function to process actions for linked NPCs void CreatureLinkingHolder::DoCreatureLinkingEvent(CreatureLinkingEvent eventType, Creature* pSource, Unit* pEnemy /* = NULL*/) { // This check will be needed in reload case if (!sCreatureLinkingMgr.IsLinkedEventTrigger(pSource)) return; // Ignore atypic behaviour if (pSource->IsControlledByPlayer()) return; if (eventType == LINKING_EVENT_AGGRO && !pEnemy) return; uint32 eventFlagFilter = 0; uint32 reverseEventFlagFilter = 0; switch (eventType) { case LINKING_EVENT_AGGRO: eventFlagFilter = EVENT_MASK_ON_AGGRO; reverseEventFlagFilter = FLAG_TO_AGGRO_ON_AGGRO; break; case LINKING_EVENT_EVADE: eventFlagFilter = EVENT_MASK_ON_EVADE; reverseEventFlagFilter = FLAG_TO_RESPAWN_ON_EVADE; break; case LINKING_EVENT_DIE: eventFlagFilter = EVENT_MASK_ON_DIE; reverseEventFlagFilter = 0; break; case LINKING_EVENT_RESPAWN: eventFlagFilter = EVENT_MASK_ON_RESPAWN; reverseEventFlagFilter = FLAG_FOLLOW; break; } // Process Slaves (by entry) HolderMapBounds bounds = m_holderMap.equal_range(pSource->GetEntry()); for (HolderMap::iterator itr = bounds.first; itr != bounds.second; ++itr) ProcessSlaveGuidList(eventType, pSource, itr->second.linkingFlag & eventFlagFilter, itr->second.searchRange, itr->second.linkedGuids, pEnemy); // Process Slaves (by guid) bounds = m_holderGuidMap.equal_range(pSource->GetGUIDLow()); for (HolderMap::iterator itr = bounds.first; itr != bounds.second; ++itr) ProcessSlaveGuidList(eventType, pSource, itr->second.linkingFlag & eventFlagFilter, itr->second.searchRange, itr->second.linkedGuids, pEnemy); // Process Master if (CreatureLinkingInfo const* pInfo = sCreatureLinkingMgr.GetLinkedTriggerInformation(pSource)) { if (pInfo->linkingFlag & reverseEventFlagFilter) { Creature* pMaster = NULL; if (pInfo->mapId != INVALID_MAP_ID) // entry case { BossGuidMapBounds finds = m_masterGuid.equal_range(pInfo->masterId); for (BossGuidMap::iterator itr = finds.first; itr != finds.second; ++itr) { pMaster = pSource->GetMap()->GetCreature(itr->second); if (pMaster && IsSlaveInRangeOfBoss(pSource, pMaster, pInfo->searchRange)) break; } } else // guid case { CreatureData const* masterData = sObjectMgr.GetCreatureData(pInfo->masterDBGuid); CreatureInfo const* cInfo = ObjectMgr::GetCreatureTemplate(masterData->id); pMaster = pSource->GetMap()->GetCreature(ObjectGuid(cInfo->GetHighGuid(), cInfo->Entry, pInfo->masterDBGuid)); } if (pMaster) { switch (eventType) { case LINKING_EVENT_AGGRO: if (pMaster->IsControlledByPlayer()) return; if (pMaster->isInCombat()) pMaster->SetInCombatWith(pEnemy); else pMaster->AI()->AttackStart(pEnemy); break; case LINKING_EVENT_EVADE: if (!pMaster->isAlive()) pMaster->Respawn(); break; case LINKING_EVENT_RESPAWN: if (pMaster->isAlive()) SetFollowing(pSource, pMaster); } } } } }
void mtsTeleOperationECM::EnterEnabled(void) { // set cartesian effort parameters mMTML.SetWrenchBodyOrientationAbsolute(true); mMTML.LockOrientation(mMTML.PositionCartesianCurrent.Position().Rotation()); mMTMR.SetWrenchBodyOrientationAbsolute(true); mMTMR.LockOrientation(mMTMR.PositionCartesianCurrent.Position().Rotation()); // initial state for MTM force feedback // -1- initial distance between MTMs vct3 vectorLR; vectorLR.DifferenceOf(mMTMR.PositionCartesianCurrent.Position().Translation(), mMTML.PositionCartesianCurrent.Position().Translation()); // -2- mid-point, aka center of image mInitial.C.SumOf(mMTMR.PositionCartesianCurrent.Position().Translation(), mMTML.PositionCartesianCurrent.Position().Translation()); mInitial.C.Multiply(0.5); // -3- image up vector mInitial.Up.CrossProductOf(vectorLR, mInitial.C); mInitial.Up.NormalizedSelf(); // -4- width of image, depth of arms wrt image plan vct3 side; side.CrossProductOf(mInitial.C, mInitial.Up); side.NormalizedSelf(); mInitial.w = 0.5 * vctDotProduct(side, vectorLR); mInitial.d = 0.5 * vctDotProduct(mInitial.C.Normalized(), vectorLR); // projections mInitial.Lr.Assign(mInitial.C[0], 0, mInitial.C[2]); mInitial.Lr.NormalizedSelf(); mInitial.Ud.Assign(0, mInitial.C[1], mInitial.C[2]); mInitial.Ud.NormalizedSelf(); mInitial.Cw.Assign(mInitial.Up[0], mInitial.Up[1], 0); mInitial.Cw.NormalizedSelf(); mInitial.ECMPositionJoint = mECM.StateJointDesired.Position(); // -5- store current rotation matrix for MTML, MTMR, and ECM vctEulerZYXRotation3 eulerAngles; eulerAngles.Assign(mInitial.ECMPositionJoint[3], mInitial.ECMPositionJoint[0], mInitial.ECMPositionJoint[1]); vctEulerToMatrixRotation3(eulerAngles, mInitial.ECMRotEuler); mInitial.MTMLRot = mMTML.PositionCartesianCurrent.Position().Rotation(); mInitial.MTMRRot = mMTMR.PositionCartesianCurrent.Position().Rotation(); #if 0 std::cerr << CMN_LOG_DETAILS << std::endl << "L: " << mMTML.PositionCartesianCurrent.Position().Translation() << std::endl << "R: " << mMTMR.PositionCartesianCurrent.Position().Translation() << std::endl << "C: " << mInitial.C << std::endl << "Up: " << mInitial.Up << std::endl << "w: " << mInitial.w << std::endl << "d: " << mInitial.d << std::endl << "Si: " << side << std::endl; #endif mInitial.ECMPositionJoint = mECM.StateJointDesired.Position(); // check if by any chance the clutch pedal is pressed if (mIsClutched) { Clutch(true); } else { SetFollowing(true); } }
void mtsTeleOperationECM::RunAllStates(void) { mtsExecutionResult executionResult; // get MTML Cartesian position/velocity executionResult = mMTML.GetPositionCartesian(mMTML.PositionCartesianCurrent); if (!executionResult.IsOK()) { CMN_LOG_CLASS_RUN_ERROR << "Run: call to MTML.GetPositionCartesian failed \"" << executionResult << "\"" << std::endl; mInterface->SendError(this->GetName() + ": unable to get cartesian position from MTML"); this->SetDesiredState("DISABLED"); } executionResult = mMTML.GetVelocityCartesian(mMTML.VelocityCartesianCurrent); if (!executionResult.IsOK()) { CMN_LOG_CLASS_RUN_ERROR << "Run: call to MTML.GetVelocityCartesian failed \"" << executionResult << "\"" << std::endl; mInterface->SendError(this->GetName() + ": unable to get cartesian velocity from MTML"); this->SetDesiredState("DISABLED"); } // get MTMR Cartesian position executionResult = mMTMR.GetPositionCartesian(mMTMR.PositionCartesianCurrent); if (!executionResult.IsOK()) { CMN_LOG_CLASS_RUN_ERROR << "Run: call to MTMR.GetPositionCartesian failed \"" << executionResult << "\"" << std::endl; mInterface->SendError(this->GetName() + ": unable to get cartesian position from MTMR"); this->SetDesiredState("DISABLED"); } executionResult = mMTMR.GetVelocityCartesian(mMTMR.VelocityCartesianCurrent); if (!executionResult.IsOK()) { CMN_LOG_CLASS_RUN_ERROR << "Run: call to MTMR.GetVelocityCartesian failed \"" << executionResult << "\"" << std::endl; mInterface->SendError(this->GetName() + ": unable to get cartesian velocity from MTMR"); this->SetDesiredState("DISABLED"); } // get ECM Cartesian position for GUI executionResult = mECM.GetPositionCartesian(mECM.PositionCartesianCurrent); if (!executionResult.IsOK()) { CMN_LOG_CLASS_RUN_ERROR << "Run: call to ECM.GetPositionCartesian failed \"" << executionResult << "\"" << std::endl; mInterface->SendError(this->GetName() + ": unable to get cartesian position from ECM"); this->SetDesiredState("DISABLED"); } // for motion computation executionResult = mECM.GetStateJointDesired(mECM.StateJointDesired); if (!executionResult.IsOK()) { CMN_LOG_CLASS_RUN_ERROR << "Run: call to ECM.GetStateJointDesired failed \"" << executionResult << "\"" << std::endl; mInterface->SendError(this->GetName() + ": unable to get joint state from ECM"); this->SetDesiredState("DISABLED"); } // check if anyone wanted to disable anyway if ((mTeleopState.DesiredState() == "DISABLED") && (mTeleopState.CurrentState() != "DISABLED")) { SetFollowing(false); mTeleopState.SetCurrentState("DISABLED"); return; } // monitor state of arms if needed if ((mTeleopState.CurrentState() != "DISABLED") && (mTeleopState.CurrentState() != "SETTING_ARMS_STATE")) { std::string armState; mECM.GetDesiredState(armState); if (armState != "READY") { this->SetDesiredState("DISABLED"); mInterface->SendError(this->GetName() + ": ECM is not in state \"READY\" anymore"); } mMTML.GetDesiredState(armState); if (armState != "READY") { this->SetDesiredState("DISABLED"); mInterface->SendError(this->GetName() + ": MTML is not in state \"READY\" anymore"); } mMTMR.GetDesiredState(armState); if (armState != "READY") { this->SetDesiredState("DISABLED"); mInterface->SendError(this->GetName() + ": MTMR is not in state \"READY\" anymore"); } } }
void mtsTeleOperationECM::Startup(void) { CMN_LOG_CLASS_INIT_VERBOSE << "Startup" << std::endl; SetScale(mScale); SetFollowing(false); }