CPFA_loop_functions::CPFA_loop_functions() :
RNG(argos::CRandom::CreateRNG("argos")),
MaxSimTime(3600 * GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick()),
ResourceDensityDelay(0),
RandomSeed(GetSimulator().GetRandomSeed()),
SimCounter(0),
MaxSimCounter(1),
VariableFoodPlacement(0),
OutputData(0),
DrawDensityRate(4),
DrawIDs(1),
DrawTrails(1),
DrawTargetRays(1),
FoodDistribution(2),
FoodItemCount(256),
NumberOfClusters(4),
ClusterWidthX(8),
ClusterLengthY(8),
PowerRank(4),
ProbabilityOfSwitchingToSearching(0.0),
ProbabilityOfReturningToNest(0.0),
UninformedSearchVariation(0.0),
RateOfInformedSearchDecay(0.0),
RateOfSiteFidelity(0.0),
RateOfLayingPheromone(0.0),
RateOfPheromoneDecay(0.0),
FoodRadius(0.05),
FoodRadiusSquared(0.0025),
NestRadius(0.25),
NestRadiusSquared(0.0625),
NestElevation(0.01),
SearchRadiusSquared((4.0 * FoodRadius) * (4.0 * FoodRadius)), // We are looking at a 4 by 4 square (3 targets + 2*1/2 target gaps)
score(0),
PrintFinalScore(0)
{}
void CPFA_loop_functions::UpdatePheromoneList() {
// Return if this is not a tick that lands on a 0.5 second interval
if ((int)(GetSpace().GetSimulationClock()) % ((int)(GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick()) / 2) != 0) return;
std::vector<Pheromone> new_p_list;
argos::Real t = GetSpace().GetSimulationClock() / GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick();
//ofstream log_output_stream;
//log_output_stream.open("time.txt", ios::app);
//log_output_stream << t << ", " << GetSpace().GetSimulationClock() << ", " << GetSimulator().GetPhysicsEngine("default").GetInverseSimulationClockTick() << endl;
//log_output_stream.close();
for(size_t i = 0; i < PheromoneList.size(); i++) {
PheromoneList[i].Update(t);
if(PheromoneList[i].IsActive() == true) {
new_p_list.push_back(PheromoneList[i]);
}
}
PheromoneList = new_p_list;
}
void CFloorPowerFunctions::SetPovCamera()
{
m_Renderer = dynamic_cast<CQTOpenGLRender*>(&GetSimulator().GetVisualization());
m_Camera = &m_Renderer->GetMainWindow().GetOpenGLWidget().GetCamera();
m_CameraSettings = &m_Camera->GetActiveSettings();
m_SelectedEntity = dynamic_cast<CEFootBotEntity*>(m_Renderer->GetMainWindow().GetOpenGLWidget().GetSelectedEntity());
if(m_SelectedEntity != NULL){
// get robot position and orientation
CVector3 pos_vec = m_SelectedEntity->GetEmbodiedEntity().GetOriginAnchor().Position;
CQuaternion orientation = m_SelectedEntity->GetEmbodiedEntity().GetOriginAnchor().Orientation;
// get robot angle
CRadians cZAngle, cYAngle, cXAngle;
orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
// calculate camera direction vector
double x = pos_vec.GetX() + cos(cZAngle.GetValue());
double y = pos_vec.GetY() + sin(cZAngle.GetValue());
// fixate X so you don't get tilt
m_CameraSettings->Up.SetX(0);
// set position and viewpoint target
m_CameraSettings->Position.Set((double)(pos_vec.GetX()), (double)(pos_vec.GetY()), POV_HEIGHT);
m_CameraSettings->Target.Set(x, y, POV_HEIGHT);
}
}
/*****
* 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::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();
}
bool CLUTConverter::SetLUT(const string &s, bool isInputCurrent) {
_isInputCurrent = isInputCurrent;
ifstream inputFile(string(GetSimulator()->GetPathPrefix() + s).c_str());
if (!inputFile) {
return false;
}
_pLUT = new CLookUpTable(3, inputFile, false);
return true;
}
/*****
* 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();
}
argos::Real CPFA_loop_functions::getSimTimeInSeconds()
{
int ticks_per_second = GetSimulator().GetPhysicsEngine("Default").GetInverseSimulationClockTick();
float sim_time = GetSpace().GetSimulationClock();
return sim_time/ticks_per_second;
}
void RbFirmataController::ProcessIO()
{
try
{
m_bIOThreadProcessing = true;
std::cout << "Sending firmware version request\r\n";
//First lets flush the buffer.
_port.flush();
//First reset firmata
sendReset();
boost::this_thread::sleep(boost::posix_time::microseconds(10));
sendFirmwareVersionRequest();
//Loop through to do the innitial setup
int iSendFirmwareCount=0;
while(!(m_bStopIO || m_bSetupComplete))
{
if(!_firmwareReceived)
{
update();
//if(iSendFirmwareCount <= 0 && !_firmwareReceived)
//{
// //Then need to do this to init the pins, get the firmware version, and call setupArduino.
// //Will stay in update loop looking for signal. When it arrives Setup will be called
// //and we can start processing.
// iSendFirmwareCount = 1000;
//}
//iSendFirmwareCount--;
boost::this_thread::sleep(boost::posix_time::microseconds(300));
}
}
//Start the timer to measure motor send timing.
m_lMotorSendStart = GetSimulator()->GetTimerTick();
//Now that setup has compled lets do our main loop
while(!m_bStopIO)
{
if(m_bPauseIO || m_lpSim->Paused())
{
m_bIOPaused = true;
boost::this_thread::sleep(boost::posix_time::microseconds(1000));
}
else
{
m_bIOPaused = false;
//Update the firmata IO.
update();
//Do not try and step IO until it has been setup correctly.
StepIO();
if(_dynamixelMoveAdds > 0)
{
//Get the motor send time
m_fltMotorSendTime = GetSimulator()->TimerDiff_m(m_lMotorSendStart, GetSimulator()->GetTimerTick());
//Execute any synch moves that were setup for this IO loop in StepIO
//If none were setup it will ignore this call.
sendDynamixelSynchMoveExecute();
//Star the timer again.
m_lMotorSendStart = GetSimulator()->GetTimerTick();
}
}
}
}
catch(CStdErrorInfo oError)
{
m_bIOThreadProcessing = false;
}
catch(...)
{
m_bIOThreadProcessing = false;
}
m_bIOThreadProcessing = false;
}
