/*****
* This function resets all iAnts and restarts the simulation based on initial
* conditions set in the XML file.
*****/
void DSA_loop_functions::Reset() {
if(VariableSeed == 1) GetSimulator().SetRandomSeed(++RandomSeed);
//GetSimulator().Reset();
GetSpace().Reset();
SimTime = 0;
ResourceDensityDelay = 0;
FoodList.clear();
//PheromoneList.clear();
//FidelityList.clear();
TargetRayList.clear();
SetFoodDistribution();
size_t STOP = 0;
size_t robots = 0;
for(size_t i = 0; i < iAnts.size(); i++) {
iAnts[i]->Reset();
ReadFile();
iAnts[i]->SetStop(STOP);
STOP += 10;
if(robots <= N_robots)
{
iAnts[i]->GetPattern(robotPattern[robots]);
robots++;
}
}
}
void CPFA_loop_functions::Reset() {
if(VariableFoodPlacement == 0) {
RNG->Reset();
}
GetSpace().Reset();
GetSpace().GetFloorEntity().Reset();
MaxSimCounter = SimCounter;
SimCounter = 0;
score = 0.0;
FoodList.clear();
FoodColoringList.clear();
PheromoneList.clear();
FidelityList.clear();
TargetRayList.clear();
SetFoodDistribution();
argos::CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot");
argos::CSpace::TMapPerType::iterator it;
for(it = footbots.begin(); it != footbots.end(); it++) {
argos::CFootBotEntity& footBot = *argos::any_cast<argos::CFootBotEntity*>(it->second);
BaseController& c = dynamic_cast<BaseController&>(footBot.GetControllableEntity().GetController());
CPFA_controller& c2 = dynamic_cast<CPFA_controller&>(c);
MoveEntity(footBot.GetEmbodiedEntity(), c2.GetStartPosition(), argos::CQuaternion(), false);
c2.Reset();
}
}
void CPFA_loop_functions::Init(argos::TConfigurationNode &node) {
argos::CDegrees USV_InDegrees;
argos::TConfigurationNode CPFA_node = argos::GetNode(node, "CPFA");
argos::GetNodeAttribute(CPFA_node, "ProbabilityOfSwitchingToSearching", ProbabilityOfSwitchingToSearching);
argos::GetNodeAttribute(CPFA_node, "ProbabilityOfReturningToNest", ProbabilityOfReturningToNest);
argos::GetNodeAttribute(CPFA_node, "UninformedSearchVariation", USV_InDegrees);
argos::GetNodeAttribute(CPFA_node, "RateOfInformedSearchDecay", RateOfInformedSearchDecay);
argos::GetNodeAttribute(CPFA_node, "RateOfSiteFidelity", RateOfSiteFidelity);
argos::GetNodeAttribute(CPFA_node, "RateOfLayingPheromone", RateOfLayingPheromone);
argos::GetNodeAttribute(CPFA_node, "RateOfPheromoneDecay", RateOfPheromoneDecay);
argos::GetNodeAttribute(CPFA_node, "PrintFinalScore", PrintFinalScore);
UninformedSearchVariation = ToRadians(USV_InDegrees);
/****************************************************************************************************************************/
argos::TConfigurationNode settings_node = argos::GetNode(node, "settings");
argos::GetNodeAttribute(settings_node, "MaxSimTimeInSeconds", MaxSimTime);
MaxSimTime *= GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick();
argos::GetNodeAttribute(settings_node, "MaxSimCounter", MaxSimCounter);
argos::GetNodeAttribute(settings_node, "VariableFoodPlacement", VariableFoodPlacement);
argos::GetNodeAttribute(settings_node, "OutputData", OutputData);
argos::GetNodeAttribute(settings_node, "DrawIDs", DrawIDs);
argos::GetNodeAttribute(settings_node, "DrawTrails", DrawTrails);
argos::GetNodeAttribute(settings_node, "DrawTargetRays", DrawTargetRays);
argos::GetNodeAttribute(settings_node, "FoodDistribution", FoodDistribution);
argos::GetNodeAttribute(settings_node, "FoodItemCount", FoodItemCount);
argos::GetNodeAttribute(settings_node, "NumberOfClusters", NumberOfClusters);
argos::GetNodeAttribute(settings_node, "ClusterWidthX", ClusterWidthX);
argos::GetNodeAttribute(settings_node, "ClusterLengthY", ClusterLengthY);
argos::GetNodeAttribute(settings_node, "PowerRank", PowerRank);
argos::GetNodeAttribute(settings_node, "FoodRadius", FoodRadius);
argos::GetNodeAttribute(settings_node, "NestElevation", NestElevation);
FoodRadiusSquared = FoodRadius*FoodRadius;
// calculate the forage range and compensate for the robot's radius of 0.085m
argos::CVector3 ArenaSize = GetSpace().GetArenaSize();
argos::Real rangeX = (ArenaSize.GetX() / 2.0) - 0.085;
argos::Real rangeY = (ArenaSize.GetY() / 2.0) - 0.085;
ForageRangeX.Set(-rangeX, rangeX);
ForageRangeY.Set(-rangeY, rangeY);
// Send a pointer to this loop functions object to each controller.
argos::CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot");
argos::CSpace::TMapPerType::iterator it;
for(it = footbots.begin(); it != footbots.end(); it++) {
argos::CFootBotEntity& footBot = *argos::any_cast<argos::CFootBotEntity*>(it->second);
BaseController& c = dynamic_cast<BaseController&>(footBot.GetControllableEntity().GetController());
CPFA_controller& c2 = dynamic_cast<CPFA_controller&>(c);
c2.SetLoopFunctions(this);
}
SetFoodDistribution();
}
/*****
* Required by ARGoS. This function initializes global variables using
* values from the XML configuration file supplied when ARGoS is run.
*****/
void DSA_loop_functions::Init(TConfigurationNode& node) {
/* Temporary variables. */
CSimulator *simulator = &GetSimulator();
CPhysicsEngine *physicsEngine = &simulator->GetPhysicsEngine("default");
CVector3 ArenaSize = GetSpace().GetArenaSize();
CVector2 rangeX = CVector2(-ArenaSize.GetX()/2.0, ArenaSize.GetX()/2.0);
CVector2 rangeY = CVector2(-ArenaSize.GetY()/2.0, ArenaSize.GetY()/2.0);
CDegrees USV_InDegrees;
/* Get each tag in the loop functions section of the XML file. */
TConfigurationNode simNode = GetNode(node, "simulation");
TConfigurationNode random = GetNode(node, "_0_FoodDistribution_Random");
TConfigurationNode cluster = GetNode(node, "_1_FoodDistribution_Cluster");
TConfigurationNode powerLaw = GetNode(node, "_2_FoodDistribution_PowerLaw");
GetNodeAttribute(simNode, "MaxSimCounter", MaxSimCounter);
GetNodeAttribute(simNode, "VariableSeed", VariableSeed);
GetNodeAttribute(simNode, "OutputData", OutputData);
GetNodeAttribute(simNode, "NestPosition", NestPosition);
GetNodeAttribute(simNode, "NestRadius", NestRadius);
GetNodeAttribute(simNode, "FoodRadius", FoodRadius);
GetNodeAttribute(simNode, "FoodDistribution", FoodDistribution);
GetNodeAttribute(random, "FoodItemCount", FoodItemCount);
GetNodeAttribute(cluster, "NumberOfClusters", NumberOfClusters);
GetNodeAttribute(cluster, "ClusterWidthX", ClusterWidthX);
GetNodeAttribute(cluster, "ClusterLengthY", ClusterLengthY);
GetNodeAttribute(powerLaw, "PowerRank", PowerRank);
/* Convert and calculate additional values. */
NestRadiusSquared = (NestRadius) * (NestRadius);
FoodRadiusSquared = (FoodRadius + 0.04) * (FoodRadius + 0.04);
ForageRangeX.Set(rangeX.GetX() + (2.0 * FoodRadius),
rangeX.GetY() - (2.0 * FoodRadius));
ForageRangeY.Set(rangeY.GetX() + (2.0 * FoodRadius),
rangeY.GetY() - (2.0 * FoodRadius));
RNG = CRandom::CreateRNG("argos");
/* Store the iAnts in a more friendly, human-readable structure. */
CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot");
CSpace::TMapPerType::iterator it;
ReadFile();
size_t STOP = 0;
size_t robots = 0;
for(it = footbots.begin(); it != footbots.end(); it++)
{
CFootBotEntity& footBot = *any_cast<CFootBotEntity*>(it->second);
DSA_controller& c = dynamic_cast<DSA_controller&>(footBot.GetControllableEntity().GetController());
DSAnts.push_back(&c);
c.SetData(&data);
c.SetStop(STOP);
STOP += 10; /* adds 10 seconds extra pause for each robot */
if(robots <= N_robots)
{
c.GetPattern(robotPattern[robots]);
robots++;
//attempt to add mutiple colors for each robot
// if(robots==2)
// {
// DrawTargetRays = 2;
// }
// else if(robots ==3)
// {
// DrawTargetRays = 3;
// }
}
}
/* Set up the food distribution based on the XML file. */
SetFoodDistribution();
}
