// 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());
}
//------------------------------------------------------------------------------
// 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 );
}
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CAmbientGeneric::UpdateOnRemove( void )
{
if ( m_fActive )
{
// Stop the sound we're generating
SendSound( SND_STOP );
}
BaseClass::UpdateOnRemove();
}
//-----------------------------------------------------------------------------
// 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 );
}
//-----------------------------------------------------------------------------
// 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;
}
//------------------------------------------------------------------------------
// 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());
}
}
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;
}
//------------------------------------------------------------------------------
// 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 );
}
}
//-----------------------------------------------------------------------------
// 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 );
}
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;
}
//-----------------------------------------------------------------------------
// 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;
}
}