void CFootBotUN::addNodesAsObstacles(map<UInt8, Node> listNodeObstacles) {
//Aux variables
CVector2 auxPosition;
CVector2 auxVelocity;
//Create a list of Nodes
for (map<UInt8, Node>::iterator it = listNodeObstacles.begin(); it != listNodeObstacles.end(); it++) {
//printf("Node %d\n", (it->second).getId());
// I'm not and obstacle for myself!
if ((it->second).getId() != mySelf.getId()) {
//printf("Agent \n");
auxPosition.Set((it->second).getPosition().GetX(), (it->second).getPosition().GetY());
auxVelocity.Set((it->second).getVelocity(), 0);
// For a dynamic obstacle we need to use the addObstacleAtPoint(position,velocity,radius)
//agent->addObstacleAtPoint(auxPosition, OBSTACLE_RADIUS);
agent->addObstacleAtPoint(auxPosition, auxVelocity, OBSTACLE_RADIUS);
printf("Adding obstacle : %d\n", (it->second).getId());
}
}
}
void CGroundSensorEquippedEntity::AddSensorRing(const CVector2& c_center,
Real f_radius,
const CRadians& c_start_angle,
ESensorType e_type,
UInt32 un_num_sensors,
const SAnchor& s_anchor) {
CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
CRadians cAngle;
CVector2 cOffset;
for(UInt32 i = 0; i < un_num_sensors; ++i) {
cAngle = c_start_angle + i * cSensorSpacing;
cAngle.SignedNormalize();
cOffset.Set(f_radius, 0.0f);
cOffset.Rotate(cAngle);
cOffset += c_center;
AddSensor(cOffset, e_type, s_anchor);
}
}
bool CVector2::FromString(Engine::Containers::CString str, CVector2& a)
{
if (str.Length() <= 0 || // Needs a string :3
str[0] != '(' || str[str.Length() - 1] != ')' || // Needs the braces around it.
str.Count(',') != 1) // Needs exactly 1 seperating comma.
return false;
str = str.Trim("() ");
s32 idx = str.IndexOf(',');
Engine::Containers::CString xs = str.SubString(0, idx).Trim();
Engine::Containers::CString ys = str.SubString(idx + 1).Trim();
a.Set(xs.ToFloat(), ys.ToFloat());
return true;
}
virtual void SaveAsImage(const std::string& str_path) {
fipImage cImage(FIT_BITMAP, m_unPixelsPerMeter * m_cHalfArenaSize.GetX()*2, m_unPixelsPerMeter * m_cHalfArenaSize.GetY()*2, 24);
Real fFactor = 1.0f / static_cast<Real>(m_unPixelsPerMeter);
CVector2 cFloorPos;
CColor cARGoSPixel;
RGBQUAD tFIPPixel;
for(UInt32 y = 0; y < cImage.getHeight(); ++y) {
for(UInt32 x = 0; x < cImage.getWidth(); ++x) {
cFloorPos.Set(x * fFactor, y * fFactor);
cFloorPos -= m_cHalfArenaSize;
cARGoSPixel = m_cLoopFunctions.GetFloorColor(cFloorPos);
tFIPPixel.rgbRed = cARGoSPixel.GetRed();
tFIPPixel.rgbGreen = cARGoSPixel.GetGreen();
tFIPPixel.rgbBlue = cARGoSPixel.GetBlue();
cImage.setPixelColor(x, y, &tFIPPixel);
}
}
if(!cImage.save(str_path.c_str())) {
THROW_ARGOSEXCEPTION("Cannot save image \"" << str_path << "\" for floor entity.");
}
}
void CRecruitmentLoopFunctions::PreStep() {
/* Logic to pick and drop food items */
/*
* If a robot is in the nest, drop the food item
* If a robot is on a food item, pick it
* Each robot can carry only one food item per time
*/
/* Check whether a robot is on a food item */
//CSpace::TMapPerType& m_cFootbots = m_cSpace.GetEntitiesByType("foot-bot");
CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("foot-bot");
for(CSpace::TMapPerType::iterator it = m_cFootbots.begin();
it != m_cFootbots.end();
++it) {
/* Get handle to foot-bot entity and controller */
CFootBotEntity& cFootBot = *any_cast<CFootBotEntity*>(it->second);
CBTFootbotRecruitmentController& cController = dynamic_cast<CBTFootbotRecruitmentController&>(cFootBot.GetControllableEntity().GetController());
/* Get the position of the foot-bot on the ground as a CVector2 */
CVector2 cPos;
cPos.Set(cFootBot.GetEmbodiedEntity().GetPosition().GetX(),
cFootBot.GetEmbodiedEntity().GetPosition().GetY());
/* Get state data */
CBTFootbotRecruitmentController::SStateData* sStateData = &cController.GetStateData();
sStateData->CurrentPosition = cPos;
/* Get food data */
CBTFootbotRecruitmentController::SFoodData* sFoodData = &cController.GetFoodData();
/* The foot-bot has a food item */
if(sFoodData->HasFoodItem) {
/* Check whether the foot-bot is in the nest */
if(InNest(cPos)) {
/* Place a new food item on the ground */
CVector2 tmp = sFoodData->LastFoodPosition;
m_cFoodPos[sFoodData->FoodItemIdx].Set(tmp.GetX(), tmp.GetY());
//m_cFoodPos[sFoodData->FoodItemIdx].Set(m_pcRNG->Uniform(m_cForagingArenaSideX), m_pcRNG->Uniform(m_cForagingArenaSideY));
/* Drop the food item */
sFoodData->HasFoodItem = false;
sFoodData->FoodItemIdx = 0;
++sFoodData->TotalFoodItems;
m_nbCollectedFood++;
/* The floor texture must be updated */
m_pcFloor->SetChanged();
}
}
else {
/* The foot-bot has no food item */
/* Check whether the foot-bot is out of the nest */
if(!InNest(cPos)) {
/* Check whether the foot-bot is on a food item */
bool bDone = false;
for(size_t i = 0; i < m_cFoodPos.size() && !bDone; ++i) {
if((cPos - m_cFoodPos[i]).SquareLength() < m_fFoodSquareRadius) {
/* If so, we move that item out of sight */
m_cFoodPos[i].Set(100.0f, 100.f);
/* The foot-bot is now carrying an item */
sFoodData->HasFoodItem = true;
sFoodData->FoodItemIdx = i;
sFoodData->LastFoodPosition = cPos;
/* The floor texture must be updated */
m_pcFloor->SetChanged();
/* We are done */
bDone = true;
}
}
}
}
}
if(GetSpace().GetSimulationClock() % 1000 == 0){
m_cOutput << GetSpace().GetSimulationClock() << "\t"
<< m_nbCollectedFood << "\t" << m_nbCollectedFood / (GetSpace().GetSimulationClock()/1000)<< "\n";
}
}
void CCombinedLoopFunctions::PreStep() {
if(--foodClock == 0){
AddOneFood();
foodClock = renewalRate;
}
/* Logic to pick and drop food items */
/*
* If a robot is in the nest, drop the food item
* If a robot is on a food item, pick it
* Each robot can carry only one food item per time
*/
/* Check whether a robot is on a food item */
CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("foot-bot");
for(CSpace::TMapPerType::iterator it = m_cFootbots.begin(); it != m_cFootbots.end(); ++it) {
/* Get handle to foot-bot entity and controller */
CFootBotEntity& cFootBot = *any_cast<CFootBotEntity*>(it->second);
CBTFootbotCombinedController& cController = dynamic_cast<CBTFootbotCombinedController&>(cFootBot.GetControllableEntity().GetController());
CBTFootbotCombinedRootBehavior* pcRootBehavior = cController.GetRootBehavior();
/* Get the position of the foot-bot on the ground as a CVector2 */
CVector2 cPos;
cPos.Set(cFootBot.GetEmbodiedEntity().GetPosition().GetX(),
cFootBot.GetEmbodiedEntity().GetPosition().GetY());
/* Get state data */
CBTFootbotCombinedRootBehavior::SStateData* sStateData = &(*pcRootBehavior).GetStateData();
sStateData->CurrentPosition = cPos;
/* Get food data */
CBTFootbotCombinedRootBehavior::SFoodData* sFoodData = &(*pcRootBehavior).GetFoodData();
/* The foot-bot has a food item */
if(sFoodData->HasFoodItem) {
/* Check whether the foot-bot is in the nest */
if(InNest(cPos)) {
/* Drop the food item */
//foodPatches[sFoodData->FoodPatchIdx].erase(foodPatches[sFoodData->FoodPatchIdx].begin() + sFoodData->FoodItemIdx);
sFoodData->HasFoodItem = false;
sFoodData->FoodItemIdx = 0;
sFoodData->FoodPatchIdx = 0;
++sFoodData->TotalFoodItems;
m_nbCollectedFood++;
}
}
else {
/* The foot-bot has no food item */
/* Check whether the foot-bot is out of the nest */
if(!InNest(cPos)) {
/* Check whether the foot-bot is on a food item */
bool bDone = false;
for(UInt32 j = 0; j < foodPatches.size(); j++)
for(size_t i = 0; i < foodPatches[j].size() && !bDone; ++i) {
if((cPos - foodPatches[j][i]).SquareLength() < m_fFoodSquareRadius) {
/* If so, we move that item out of sight */
foodPatches[j][i].Set(1000.0f, 1000.f);
/* The foot-bot is now carrying an item */
sFoodData->HasFoodItem = true;
sFoodData->FoodItemIdx = i;
sFoodData->FoodPatchIdx = j;
sFoodData->LastFoodPosition = cPos;
/* We are done */
bDone = true;
foodPatches[sFoodData->FoodPatchIdx].erase(foodPatches[j].begin() + i);
/* The floor texture must be updated */
m_pcFloor->SetChanged();
}
}
}
}
}
if(GetSpace().GetSimulationClock() % 1000 == 0){
int nbFoodSolitary = 0;
int nbFoodRecruiter = 0;
int nbFoodRecruitee = 0;
int type = 0;
CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("foot-bot");
for(CSpace::TMapPerType::iterator it = m_cFootbots.begin(); it != m_cFootbots.end(); ++it) {
CFootBotEntity& cFootBot = *any_cast<CFootBotEntity*>(it->second);
CBTFootbotCombinedController& cController = dynamic_cast<CBTFootbotCombinedController&>(cFootBot.GetControllableEntity().GetController());
CBTFootbotCombinedRootBehavior* pcRootBehavior = cController.GetRootBehavior();
/* Get food data */
CBTFootbotCombinedRootBehavior::SFoodData* sFoodData = &(*pcRootBehavior).GetFoodData();
type = pcRootBehavior->GetRobotType();
switch(type){
case SOLITARY: {nbFoodSolitary += sFoodData->TotalFoodItems; break;}
case RECRUITER: {nbFoodRecruiter += sFoodData->TotalFoodItems; break;}
case RECRUITEE: {nbFoodRecruitee += sFoodData->TotalFoodItems; break;}
default: { THROW_ARGOSEXCEPTION("unrecognized type when forming statistics"); }
}
}
m_cOutput << GetSpace().GetSimulationClock() << ","
<< nbFoodSolitary << ","
<< nbFoodRecruiter << ","
<< nbFoodRecruitee
<< std::endl;
}
if(GetSpace().GetSimulationClock() % 10000 == 0){
LOG << "Clock at: " << GetSpace().GetSimulationClock() << std::endl;
}
}
