// System Start
void GlobalPlanner::Start()
{
    m_start_time = ros::Time::now();
    SendSound("mario_lets_go.wav");
    std::stringstream ss;
    ss << "Starting global planner";
    SendText(ss.str());
}
예제 #2
0
//------------------------------------------------------------------------------
// Purpose:
//------------------------------------------------------------------------------
void CAmbientGeneric::Activate( void )
{
	BaseClass::Activate();

	// Initialize sound source.  If no source was given, or source can't be found
	// then this is the source
	if (m_hSoundSource == NULL)
	{
		if (m_sSourceEntName != NULL_STRING)
		{
			m_hSoundSource = gEntList.FindEntityByName( NULL, m_sSourceEntName );
			if ( m_hSoundSource != NULL )
			{
				m_nSoundSourceEntIndex = m_hSoundSource->entindex();
			}
		}

		if (m_hSoundSource == NULL)
		{
			m_hSoundSource = this;
			m_nSoundSourceEntIndex = entindex();
		}
		else
		{
			if ( !FBitSet( m_spawnflags, SF_AMBIENT_SOUND_EVERYWHERE ) )
			{
				AddEFlags( EFL_FORCE_CHECK_TRANSMIT );
			}
		}
	}

	// If active start the sound
	if ( m_fActive )
	{
		int flags = SND_SPAWNING;
		// If we are loading a saved game, we can't write into the init/signon buffer here, so just issue
		//  as a regular sound message...
		if ( gpGlobals->eLoadType == MapLoad_Transition ||
			 gpGlobals->eLoadType == MapLoad_LoadGame || 
			 g_pGameRules->InRoundRestart() )
		{
			flags = SND_NOFLAGS;
		}
	
		// Tracker 76119:  8/12/07 ywb: 
		//  Make sure pitch and volume are set up to the correct value (especially after restoring a .sav file)
		flags |= ( SND_CHANGE_PITCH | SND_CHANGE_VOL );  

		// Don't bother sending over to client if volume is zero, though
		if ( m_dpv.vol > 0 )
		{
			SendSound( (SoundFlags_t)flags );
		}

		SetNextThink( gpGlobals->curtime + 0.1f );
	}
}
예제 #3
0
//-----------------------------------------------------------------------------
// Purpose: 
//-----------------------------------------------------------------------------
void CAmbientGeneric::UpdateOnRemove( void )
{
	if ( m_fActive )
	{
		// Stop the sound we're generating
		SendSound( SND_STOP );
	}

	BaseClass::UpdateOnRemove();
}
예제 #4
0
//-----------------------------------------------------------------------------
// Purpose: Input handler for changing volume.
// Input  : Float new volume, from 0 - 10.
//-----------------------------------------------------------------------------
void CAmbientGeneric::InputVolume( inputdata_t &inputdata )
{
	//
	// Multiply the input value by ten since volumes are expected to be from 0 - 100.
	//
	m_dpv.vol = clamp( RoundFloatToInt( inputdata.value.Float() * 10.f ), 0, 100 );
	m_dpv.volfrac = m_dpv.vol << 8;

	SendSound( SND_CHANGE_VOL );
}
예제 #5
0
//-----------------------------------------------------------------------------
// Purpose: Think at 5hz if we are dynamically modifying pitch or volume of the
//			playing sound.  This function will ramp pitch and/or volume up or
//			down, modify pitch/volume with lfo if active.
//-----------------------------------------------------------------------------
void CAmbientGeneric::RampThink( void )
{
	int pitch = m_dpv.pitch; 
	int vol = m_dpv.vol;
	int flags = 0;
	int fChanged = 0;		// false if pitch and vol remain unchanged this round
	int	prev;

	if (!m_dpv.spinup && !m_dpv.spindown && !m_dpv.fadein && !m_dpv.fadeout && !m_dpv.lfotype)
		return;						// no ramps or lfo, stop thinking

	// ==============
	// pitch envelope
	// ==============
	if (m_dpv.spinup || m_dpv.spindown)
	{
		prev = m_dpv.pitchfrac >> 8;

		if (m_dpv.spinup > 0)
			m_dpv.pitchfrac += m_dpv.spinup;
		else if (m_dpv.spindown > 0)
			m_dpv.pitchfrac -= m_dpv.spindown;

		pitch = m_dpv.pitchfrac >> 8;
		
		if (pitch > m_dpv.pitchrun)
		{
			pitch = m_dpv.pitchrun;
			m_dpv.spinup = 0;				// done with ramp up
		}

		if (pitch < m_dpv.pitchstart)
		{
			pitch = m_dpv.pitchstart;
			m_dpv.spindown = 0;				// done with ramp down

			// shut sound off
			SendSound( SND_STOP );
			
			// return without setting m_flNextThink
			return;
		}

		if (pitch > 255) pitch = 255;
		if (pitch < 1) pitch = 1;

		m_dpv.pitch = pitch;

		fChanged |= (prev != pitch);
		flags |= SND_CHANGE_PITCH;
	}
예제 #6
0
//------------------------------------------------------------------------------
// Purpose:
//------------------------------------------------------------------------------
void CAmbientGeneric::Activate( void )
{
	BaseClass::Activate();

	// Initialize sound source.  If no source was given, or source can't be found
	// then this is the source
	if (m_hSoundSource == NULL)
	{
		if (m_sSourceEntName != NULL_STRING)
		{
			m_hSoundSource = gEntList.FindEntityByName( NULL, m_sSourceEntName );
			if ( m_hSoundSource != NULL )
			{
				m_nSoundSourceEntIndex = m_hSoundSource->entindex();
			}
		}

		if (m_hSoundSource == NULL)
		{
			m_hSoundSource = this;
			m_nSoundSourceEntIndex = entindex();
		}
		else
		{
			if ( !FBitSet( m_spawnflags, SF_AMBIENT_SOUND_EVERYWHERE ) )
			{
				AddEFlags( EFL_FORCE_CHECK_TRANSMIT );
			}
		}
	}

	// If active start the sound
	if ( m_fActive )
	{
		SoundFlags_t flags = SND_SPAWNING;
		// If we are loading a saved game, we can't write into the init/signon buffer here, so just issue
		//  as a regular sound message...
		if ( gpGlobals->eLoadType == MapLoad_Transition ||
			 gpGlobals->eLoadType == MapLoad_LoadGame || 
			 g_pGameRules->InRoundRestart() )
		{
			flags = SND_NOFLAGS;
		}
	
		SendSound( flags );

		SetNextThink( gpGlobals->curtime + 0.1f );
	}
}
// System finished
void GlobalPlanner::Finished()
{
    SendSound("mario_world_clear.wav");
    std::stringstream ss;
    ss << "Global Planner finished";
    SendText(ss.str());

    Display();

    // TODO: Wrap up statistics here.
    ROS_INFO("Global Planner finished in : %g seconds", TimeSinceStart() );

    // Print out waypoint assignment table
    ROS_INFO_STREAM("Robots - Waypoint Assignment Table:");
    for (std::map<int, std::map<int, double> >::iterator robot_it = robot_waypoint_times.begin();
         robot_it != robot_waypoint_times.end();
         ++robot_it)
    {
        std::stringstream ss;
        for (std::map<int, double>::iterator waypoint_it = robot_it->second.begin(); waypoint_it != robot_it->second.end(); ++waypoint_it)
        {
            if (m_tm.IsWaypoint(waypoint_it->first))
            {
                ss << "[WP " << waypoint_it->first << ": " << waypoint_it->second << "s] ";
            }
        }
        ROS_INFO_STREAM(m_robots[robot_it->first]->GetName() << "(" << robot_it->first << ") : " << ss.str());
    }

    // Print out goal assignment table
    ROS_INFO_STREAM("Robots - Goal Assignment Table:");
    for (std::map<int, std::map<int, double> >::iterator robot_it = robot_goal_times.begin();
         robot_it != robot_goal_times.end();
         ++robot_it)
    {
        std::stringstream ss;
        for (std::map<int, double>::iterator goal_it = robot_it->second.begin(); goal_it != robot_it->second.end(); ++goal_it)
        {
            ss << "[GOAL " << goal_it->first << ": " << goal_it->second << "s] ";
        }
        ROS_INFO_STREAM(m_robots[robot_it->first]->GetName() << "(" << robot_it->first << ") : " << ss.str());
    }
}
예제 #8
0
uint32 CreatureTextMgr::SendChat(Creature* source, uint8 textGroup, uint64 whisperGuid /*= 0*/, ChatMsg msgType /*= CHAT_MSG_ADDON*/, Language language /*= LANG_ADDON*/, TextRange range /*= TEXT_RANGE_NORMAL*/, uint32 sound /*= 0*/, Team team /*= TEAM_OTHER*/, bool gmOnly /*= false*/, Player* srcPlr /*= NULL*/)
{
    if (!source)
        return 0;

    CreatureTextMap::const_iterator sList = mTextMap.find(source->GetEntry());
    if (sList == mTextMap.end())
    {
        sLog->outErrorDb("CreatureTextMgr: Could not find Text for Creature(%s) Entry %u in 'creature_text' table. Ignoring.", source->GetName(), source->GetEntry());
        return 0;
    }

    CreatureTextHolder const& textHolder = sList->second;
    CreatureTextHolder::const_iterator itr = textHolder.find(textGroup);
    if (itr == textHolder.end())
    {
        sLog->outErrorDb("CreatureTextMgr: Could not find TextGroup %u for Creature(%s) GuidLow %u Entry %u. Ignoring.", uint32(textGroup), source->GetName(), source->GetGUIDLow(), source->GetEntry());
        return 0;
    }

    CreatureTextGroup const& textGroupContainer = itr->second;  //has all texts in the group
    CreatureTextRepeatIds repeatGroup = GetRepeatGroup(source, textGroup);//has all textIDs from the group that were already said
    CreatureTextGroup tempGroup;//will use this to talk after sorting repeatGroup

    for (CreatureTextGroup::const_iterator giter = textGroupContainer.begin(); giter != textGroupContainer.end(); ++giter)
        if (std::find(repeatGroup.begin(), repeatGroup.end(), giter->id) == repeatGroup.end())
            tempGroup.push_back(*giter);

    if (tempGroup.empty())
    {
        CreatureTextRepeatMap::iterator mapItr = mTextRepeatMap.find(source->GetGUID());
        if (mapItr != mTextRepeatMap.end())
        {
            CreatureTextRepeatGroup::iterator groupItr = mapItr->second.find(textGroup);
            groupItr->second.clear();
        }

        tempGroup = textGroupContainer;
    }

    uint8 count = 0;
    float lastChance = -1;
    bool isEqualChanced = true;

    float totalChance = 0;

    for (CreatureTextGroup::const_iterator iter = tempGroup.begin(); iter != tempGroup.end(); ++iter)
    {
        if (lastChance >= 0 && lastChance != iter->probability)
            isEqualChanced = false;

        lastChance = iter->probability;
        totalChance += iter->probability;
        ++count;
    }

    int32 offset = -1;
    if (!isEqualChanced)
    {
        for (CreatureTextGroup::const_iterator iter = tempGroup.begin(); iter != tempGroup.end(); ++iter)
        {
            uint32 chance = uint32(iter->probability);
            uint32 r = urand(0, 100);
            ++offset;
            if (r <= chance)
                break;
        }
    }

    uint32 pos = 0;
    if (isEqualChanced || offset < 0)
        pos = urand(0, count - 1);
    else if (offset >= 0)
        pos = offset;

    CreatureTextGroup::const_iterator iter = tempGroup.begin() + pos;

    ChatMsg finalType = (msgType == CHAT_MSG_ADDON) ? iter->type : msgType;
    Language finalLang = (language == LANG_ADDON) ? iter->lang : language;
    uint32 finalSound = sound ? sound : iter->sound;

    if (finalSound)
        SendSound(source, finalSound, finalType, whisperGuid, range, team, gmOnly);

    Unit* finalSource = source;
    if (srcPlr)
        finalSource = srcPlr;

    if (iter->emote)
        SendEmote(finalSource, iter->emote);

    CreatureTextBuilder builder(finalSource, finalType, iter->group, iter->id, finalLang, whisperGuid);
    SendChatPacket(finalSource, builder, finalType, whisperGuid, range, team, gmOnly);
    if (isEqualChanced || (!isEqualChanced && totalChance == 100.0f))
        SetRepeatId(source, textGroup, iter->id);

    return iter->duration;
}
예제 #9
0
//------------------------------------------------------------------------------
// Purpose:
//------------------------------------------------------------------------------
void CAmbientGeneric::Activate( void )
{
	BaseClass::Activate();

	// Initialize sound source.  If no source was given, or source can't be found
	// then this is the source
	if (m_hSoundSource == NULL)
	{
		if (m_sSourceEntName != NULL_STRING)
		{
			m_hSoundSource = gEntList.FindEntityByName( NULL, m_sSourceEntName );
			if ( m_hSoundSource != NULL )
			{
				m_nSoundSourceEntIndex = m_hSoundSource->entindex();
			}
		}

		if (m_hSoundSource == NULL)
		{
			m_hSoundSource = this;
			m_nSoundSourceEntIndex = entindex();
		}
		else
		{
			if ( !FBitSet( m_spawnflags, SF_AMBIENT_SOUND_EVERYWHERE ) )
			{
				AddEFlags( EFL_FORCE_CHECK_TRANSMIT );
			}
		}
	}

#ifdef PORTAL
		// This is the only way we can silence the radio sound from the first room without touching them map -- jdw
		if ( PortalGameRules() && PortalGameRules()->ShouldRemoveRadio() )
		{		
			if ( V_strcmp( STRING( gpGlobals->mapname ), "testchmb_a_00" ) == 0 || 
			    V_strcmp( STRING( gpGlobals->mapname ), "testchmb_a_11" ) == 0 || 
			    V_strcmp( STRING( gpGlobals->mapname ), "testchmb_a_14" ) == 0 )
			{
				if ( V_strcmp( STRING( GetEntityName() ), "radio_sound" ) == 0 )
				{
					UTIL_Remove( this );
					return;
				}
			}
		}
#endif // PORTAL

	// If active start the sound
	if ( m_fActive )
	{
		int flags = SND_SPAWNING;
		// If we are loading a saved game, we can't write into the init/signon buffer here, so just issue
		//  as a regular sound message...
		if ( gpGlobals->eLoadType == MapLoad_Transition ||
			 gpGlobals->eLoadType == MapLoad_LoadGame || 
			 g_pGameRules->InRoundRestart() )
		{
			flags = SND_NOFLAGS;
		}
	
		// Tracker 76119:  8/12/07 ywb: 
		//  Make sure pitch and volume are set up to the correct value (especially after restoring a .sav file)
		flags |= ( SND_CHANGE_PITCH | SND_CHANGE_VOL );  

		// Don't bother sending over to client if volume is zero, though
		if ( m_dpv.vol > 0 )
		{
			SendSound( (SoundFlags_t)flags );
		}

		SetNextThink( gpGlobals->curtime + 0.1f );
	}
}
예제 #10
0
//-----------------------------------------------------------------------------
// Purpose: Input handler for changing pitch.
// Input  : Float new pitch from 0 - 255 (100 = as recorded).
//-----------------------------------------------------------------------------
void CAmbientGeneric::InputPitch( inputdata_t &inputdata )
{
	m_dpv.pitch = clamp( FastFloatToSmallInt( inputdata.value.Float() ), 0, 255 );

	SendSound( SND_CHANGE_PITCH );
}
예제 #11
0
uint32 CreatureTextMgr::SendChat(Creature* source, uint8 textGroup, uint64 whisperGuid, ChatType msgtype, Language language, TextRange range, uint32 sound, Team team, bool gmOnly, Player* srcPlr)
{
    if (!source)
        return 0;
    CreatureTextMap::const_iterator sList = mTextMap.find(source->GetEntry());
    if (sList == mTextMap.end())
    {
        sLog->outErrorDb("CreatureTextMgr: Could not find Text for Creature(%s) Entry %u in 'creature_text' table. Ignoring.", source->GetName(), source->GetEntry());
        return 0;
    }
    CreatureTextHolder TextHolder = (*sList).second;
    CreatureTextHolder::const_iterator itr = TextHolder.find(textGroup);
    if (itr == TextHolder.end())
    {
        sLog->outErrorDb("CreatureTextMgr: Could not find TextGroup %u for Creature(%s) GuidLow %u Entry %u. Ignoring.", uint32(textGroup), source->GetName(), source->GetGUIDLow(), source->GetEntry());
        return 0;
    }
    CreatureTextGroup TextGroup = (*itr).second;//has all texts in the group
    CreatureTextRepeatIds repeatGroup = GetRepeatGroup(source, textGroup);//has all textIDs from the group that were already said
    CreatureTextGroup tempGroup;//will use this to talk after sorting repeatGroup

    for (CreatureTextGroup::const_iterator giter = TextGroup.begin(); giter != TextGroup.end(); ++giter)
    {
        if (std::find(repeatGroup.begin(), repeatGroup.end(), (*giter).id) == repeatGroup.end())
            tempGroup.push_back((*giter));
    }
    if (tempGroup.empty())
    {
        CreatureTextRepeatMap::iterator mapItr = mTextRepeatMap.find(source->GetGUID());
        if (mapItr != mTextRepeatMap.end())
        {
            CreatureTextRepeatGroup::iterator groupItr = (*mapItr).second.find(textGroup);
            (*groupItr).second.clear();
        }
        tempGroup = TextGroup;
    }
    uint8 count = 0;
    float lastChance = -1;
    bool isEqualChanced = true;

    float totalChance = 0;

    for (CreatureTextGroup::const_iterator iter = tempGroup.begin(); iter != tempGroup.end(); ++iter)
    {
        if (lastChance >= 0 && lastChance != (*iter).probability)
            isEqualChanced = false;

        lastChance = (*iter).probability;
        totalChance += (*iter).probability;
        count++;
    }
    int32 offset = -1;
    if (!isEqualChanced)
    {
        for (CreatureTextGroup::const_iterator iter = tempGroup.begin(); iter != tempGroup.end(); ++iter)
        {
            uint32 chance = uint32((*iter).probability);
            uint32 r = urand(0, 100);
            offset++;
            if (r <= chance)
                break;
        }
    }
    uint32 pos = 0;
    if (isEqualChanced || offset < 0)
        pos = urand(0, count - 1);
    else if (offset >= 0)
        pos = offset;
    CreatureTextGroup::const_iterator iter = tempGroup.begin() + pos;

    ChatType finalType = (msgtype == CHAT_TYPE_END) ? (*iter).type : msgtype;
    Language finalLang = (language == LANG_ADDON) ? (*iter).lang : language;
    uint32 finalSound = sound ? sound : (*iter).sound;

    if (finalSound)
        SendSound(source, finalSound, finalType, whisperGuid, range, team, gmOnly);

    if ((*iter).emote)
        SendEmote(srcPlr ? srcPlr->ToUnit() : source, (*iter).emote);

    SendChatString(srcPlr ? srcPlr->ToUnit() : source, (*iter).text.c_str(), finalType, finalLang, whisperGuid, range, team, gmOnly);
    if (isEqualChanced || (!isEqualChanced && totalChance == 100.0f))
        SetRepeatId(source, textGroup, (*iter).id);

    return (*iter).duration;
}
예제 #12
0
//-----------------------------------------------------------------------------
// Purpose: Input handler for changing pitch.
// Input  : Float new pitch from 0 - 255 (100 = as recorded).
//-----------------------------------------------------------------------------
void CAmbientGeneric::InputPitch( inputdata_t &inputdata )
{
	m_dpv.pitch = (int)clamp( inputdata.value.Float(), 0, 255 );

	SendSound( SND_CHANGE_PITCH );
}
uint32 CreatureTextMgr::SendChat(Creature* source, uint8 textGroup, WorldObject const* whisperTarget /*= nullptr*/, ChatMsg msgType /*= CHAT_MSG_ADDON*/, Language language /*= LANG_ADDON*/, CreatureTextRange range /*= TEXT_RANGE_NORMAL*/, uint32 sound /*= 0*/, Team team /*= TEAM_OTHER*/, bool gmOnly /*= false*/, Player* srcPlr /*= nullptr*/)
{
    if (!source)
        return 0;

    CreatureTextMap::const_iterator sList = mTextMap.find(source->GetEntry());
    if (sList == mTextMap.end())
    {
        TC_LOG_ERROR("sql.sql.creaturetextmgr", "CreatureTextMgr: Could not find Text for Creature %s (Entry %u, spawnId %u) in 'creature_text' table. Ignoring.", source->GetName().c_str(), source->GetEntry(), source->GetSpawnId());
        return 0;
    }

    CreatureTextHolder const& textHolder = sList->second;
    CreatureTextHolder::const_iterator itr = textHolder.find(textGroup);
    if (itr == textHolder.end())
    {
        TC_LOG_ERROR("sql.sql.creaturetextmgr", "CreatureTextMgr: Could not find TextGroup %u for Creature %s (Entry %u, spawnId %u) in 'creature_text' table. Ignoring.", uint32(textGroup), source->GetName().c_str(), source->GetEntry(), source->GetSpawnId());
        return 0;
    }

    CreatureTextGroup const& textGroupContainer = itr->second;  //has all texts in the group
    CreatureTextRepeatIds repeatGroup = source->GetTextRepeatGroup(textGroup);//has all textIDs from the group that were already said
    CreatureTextGroup tempGroup;//will use this to talk after sorting repeatGroup

    for (CreatureTextGroup::const_iterator giter = textGroupContainer.begin(); giter != textGroupContainer.end(); ++giter)
        if (std::find(repeatGroup.begin(), repeatGroup.end(), giter->id) == repeatGroup.end())
            tempGroup.push_back(*giter);

    if (tempGroup.empty())
    {
        source->ClearTextRepeatGroup(textGroup);
        tempGroup = textGroupContainer;
    }

    auto iter = Trinity::Containers::SelectRandomWeightedContainerElement(tempGroup, [](CreatureTextEntry const& t) -> double
    {
        return t.probability;
    });

    ChatMsg finalType = (msgType == CHAT_MSG_ADDON) ? iter->type : msgType;
    Language finalLang = (language == LANG_ADDON) ? iter->lang : language;
    uint32 finalSound = sound ? sound : iter->sound;

    if (range == TEXT_RANGE_NORMAL)
        range = iter->TextRange;

    if (finalSound)
        SendSound(source, finalSound, finalType, whisperTarget, range, team, gmOnly);

    Unit* finalSource = source;
    if (srcPlr)
        finalSource = srcPlr;

    if (iter->emote)
        SendEmote(finalSource, iter->emote);

    if (srcPlr)
    {
        PlayerTextBuilder builder(source, finalSource, finalSource->getGender(), finalType, iter->groupId, iter->id, finalLang, whisperTarget);
        SendChatPacket(finalSource, builder, finalType, whisperTarget, range, team, gmOnly);
    }
    else
    {
        CreatureTextBuilder builder(finalSource, finalSource->getGender(), finalType, iter->groupId, iter->id, finalLang, whisperTarget);
        SendChatPacket(finalSource, builder, finalType, whisperTarget, range, team, gmOnly);
    }

    source->SetTextRepeatId(textGroup, iter->id);
    return iter->duration;
}
/***********************************************************************
 *  Method: RobotController::StateExecute
 *  Params: void *args
 * Returns: void
 * Effects: specifies what to run while in a state, and transition if
 *  appropriate
 ***********************************************************************/
void RobotController::StateExecute()
{
    // WHILE in WAITING:
    //      IF received a waypoint: transition(NAVIGATING)
    //      IF received a dump message: transition(DUMPING)
    // WHILE in NAVIGATING (waypoint navigation):
    //      IF received a (different) waypoint: change the pose to the new one
    //      IF received a dump message: transition(DUMPING)
    //      IF received a stop/cancel/estop: send waypoint result message (forced_stop) -> transition(WAITING)
    //      IF reached final pose of the waypoint, send waypoint result message (succeed) -> transition(WAITING)
    // WHILE in DUMPING (going to dump):
    //      IF received a (different) waypoint: change the pose to the new one
    //      IF received a dump message: transition(DUMPING)
    //      IF received a stop/cancel/estop: send waypoint result message (forced_stop) -> transition(WAITING)
    //      IF reached final pose of the waypoint, send waypoint result message (succeed) -> transition(WAITING)
    // ...
    bool execResult = false;
    // ROS_INFO_STREAM_THROTTLE(1.0, "STATE: "<<RobotState::ToString(m_status.GetState()));
    switch(m_status.GetState())
    {
        case RobotState::WAITING:
            if (m_tagProcessor->ShouldPause())
            {
                SendText("should pause - Waiting");
                // SendSound("mario_pause.wav");
                Transition(RobotState::WAITING_TAG_SPOTTED);
            }
        break;

        //In this state, the robot should now be stopping and getting a more accurate view of the tag
        case RobotState::WAITING_TAG_SPOTTED:
            ros::spinOnce();
            execResult = m_tagProcessor->Execute();
            // ROS_DEBUG_STREAM_THROTTLE(0.5, "Result from execute: "<<execResult);
            if (m_tagProcessor->ShouldResume())
            {
                //Transition back to the navigating state, using the same goal as before
                SendText("should resume - WAITING_TAG_SPOTTED");
                ROS_INFO("Resuming robot");
                Transition(RobotState::WAITING_TAG_FINISHED);
                break;
            }
            else
            {
                ROS_INFO_STREAM_THROTTLE(1, "Waiting on the OK to resume from the tag processor");
            }

            if (ros::Time::now() - m_timeEnteringState > ros::Duration(5))
            {
                ROS_ERROR_STREAM("ERROR: could not find any tags while stopped.  we are going to just resume WAITING");
                Transition(RobotState::NAVIGATING);
            }
        break;

        case RobotState::WAITING_TAG_FINISHED:
            if (ros::Time::now() - m_timeEnteringState > ros::Duration(1.0))
            {
                SendSound("mario_pause.wav");
                SendText("resuming - WAITING_TAG_FINISHED");
                Transition(RobotState::WAITING);
                m_tagProcessor->SetShouldPause(false);
            }
            break;

        case RobotState::NAVIGATING:
            if (m_tagProcessor->ShouldPause())
            {
                Transition(RobotState::NAVIGATING_TAG_SPOTTED);
                // SendSound("mario_pause.wav");
            }
            else
            {
                actionlib::SimpleClientGoalState::StateEnum result = action_client_ptr->getState().state_;
                switch (result)
                {
                    case actionlib::SimpleClientGoalState::SUCCEEDED:
                        ROS_INFO_STREAM("Movebase reached target (task id = "<<m_status.GetTaskID()<<")");
                        SendWaypointFinished(TaskResult::SUCCESS);
                        if (m_status.GetTaskID() < WAYPOINT_START_ID)
                        {
                            m_status.IncrementStorageUsed();
                        }
                        Transition(RobotState::WAITING);
                        break;
                    case actionlib::SimpleClientGoalState::ABORTED:
                    case actionlib::SimpleClientGoalState::REJECTED:
                    case actionlib::SimpleClientGoalState::LOST:
                    case actionlib::SimpleClientGoalState::RECALLED:
                    case actionlib::SimpleClientGoalState::PREEMPTED:
                        ROS_ERROR_STREAM("Navigation Failed: " << action_client_ptr->getState().toString() );
                        SendWaypointFinished(TaskResult::FAILURE);
                        Transition(RobotState::WAITING);
                        break;

                    case actionlib::SimpleClientGoalState::ACTIVE:
                    case actionlib::SimpleClientGoalState::PENDING:
                    default:
                        ROS_INFO_STREAM_THROTTLE(10, "Navigation state = " << action_client_ptr->getState().toString() );
                        break;
                }
            }
        break;

        //In this state, the robot should now be stopping and getting a more accurate view of the tag
        case RobotState::NAVIGATING_TAG_SPOTTED:
            ros::spinOnce();
            execResult = m_tagProcessor->Execute();
            // ROS_DEBUG_STREAM_THROTTLE(0.5, "Result from execute: "<<execResult);
            if (m_tagProcessor->ShouldResume())
            {
                //Transition back to the navigating state, using the same goal as before
                SendText("Resuming robot");
                ROS_INFO("Resuming robot");
                SendText("should resume- NAVIGATING_TAG_SPOTTED");
                Transition(RobotState::NAVIGATING_TAG_FINISHED);
            }
            else
            {
                ROS_INFO_STREAM_THROTTLE(1, "Waiting on the OK to resume from the tag processor");
            }

            if (ros::Time::now() - m_timeEnteringState > ros::Duration(5))
            {
                ROS_ERROR_STREAM("Could not find any tags while stopped.  we are going to just resume NAVIGATING");
                Transition(RobotState::NAVIGATING);
                m_tagProcessor->SetShouldPause(false);
            }
        break;

        case RobotState::NAVIGATING_TAG_FINISHED:
            if (ros::Time::now() - m_timeEnteringState > ros::Duration(2))
            {
                SendSound("mario_pause.wav");
                SendText("resuming - NAVIGATING_TAG_FINISHED");
                ROS_INFO_STREAM("Transitioning back to navigating");
                Transition(RobotState::NAVIGATING);
            }
        break;

        case RobotState::DUMPING:
        {
            // Check if reached dump site yet
            actionlib::SimpleClientGoalState::StateEnum result = action_client_ptr->getState().state_;
            switch (result)
            {
                case actionlib::SimpleClientGoalState::SUCCEEDED:
                    ROS_INFO_STREAM("Movebase reached target.");
                    SendDumpFinished(TaskResult::SUCCESS);
                    Transition(RobotState::DUMPING_FINISHED);
                    break;
                case actionlib::SimpleClientGoalState::ABORTED:
                case actionlib::SimpleClientGoalState::REJECTED:
                case actionlib::SimpleClientGoalState::LOST:
                case actionlib::SimpleClientGoalState::RECALLED:
                case actionlib::SimpleClientGoalState::PREEMPTED:
                    ROS_ERROR_STREAM("Navigation Failed: " << action_client_ptr->getState().toString() );
                    SendDumpFinished(TaskResult::FAILURE);
                    Transition(RobotState::WAITING);
                    break;

                case actionlib::SimpleClientGoalState::ACTIVE:
                case actionlib::SimpleClientGoalState::PENDING:
                default:
                    ROS_INFO_STREAM_THROTTLE(10, "Navigation state = " << action_client_ptr->getState().toString() );
                    break;
            }
        break;
        }

        case RobotState::DUMPING_FINISHED:
        break;

        default:
            ROS_ERROR_STREAM_THROTTLE(5, "Unexpected State:" << RobotState::ToString(m_status.GetState()) );
            break;
    }
}