void CPFA_qt_user_functions::DrawOnRobot(CFootBotEntity& entity) {
CPFA_controller& c = dynamic_cast<CPFA_controller&>(entity.GetControllableEntity().GetController());
if(c.IsHoldingFood() == true) {
DrawCylinder(CVector3(0.0, 0.0, 0.3), CQuaternion(), loopFunctions.FoodRadius, 0.025, CColor::BLACK);
}
if(loopFunctions.DrawIDs == 1) {
/* Disable lighting, so it does not interfere with the chosen text color */
glDisable(GL_LIGHTING);
/* Disable face culling to be sure the text is visible from anywhere */
glDisable(GL_CULL_FACE);
/* Set the text color */
CColor cColor(CColor::BLACK);
glColor3ub(cColor.GetRed(), cColor.GetGreen(), cColor.GetBlue());
/* The position of the text is expressed wrt the reference point of the footbot
* For a foot-bot, the reference point is the center of its base.
* See also the description in
* $ argos3 -q foot-bot
*/
// Disable for now
//GetOpenGLWidget().renderText(0.0, 0.0, 0.5, // position
// entity.GetId().c_str()); // text
/* Restore face culling */
glEnable(GL_CULL_FACE);
/* Restore lighting */
glEnable(GL_LIGHTING);
}
}
SInt32 CLandmarks::AtTarget(CFootBotEntity& c_fb) {
/*
* Go through the targets
* If the square distance between the foot-bot center and the target center
* is less than the target radius squared, return the id of the current target
* Otherwise, return -1
*/
for(size_t i = 0; i < TARGETS; ++i) {
if(SquareDistance(CVector2(c_fb.GetEmbodiedEntity().GetOriginAnchor().Position.GetX(),
c_fb.GetEmbodiedEntity().GetOriginAnchor().Position.GetY()),
m_vecTargets[i]) < TARGET_RADIUS2) {
return i;
}
}
return -1;
}
// draw function for adding graphics to a foot-bot
void iAnt_qt_user_functions::DrawFood(CFootBotEntity& entity) {
iAnt_controller& c = dynamic_cast<iAnt_controller&>
(entity.GetControllableEntity().GetController());
UpdateDrawInWorldData(c);
if(c.IsHoldingFood() == true) {
//#ifdef __APPLE__
//Edit here for drawing trails
DrawCylinder(CVector3(0.0f, 0.0f, 0.3f), CQuaternion(), 0.05f, 0.025f, CColor::BLACK);
//#else
// DrawCylinder(0.05f, 0.025f, CVector3(0.0f, 0.0f, 0.3f), CColor::BLACK);
//#endif
}
}
bool CLandmarks::InChain(CFootBotEntity& c_fb) {
/* Return value: true = robot is part of a chain, false otherwise */
bool bRetVal = false;
/* Get reference to robot controller */
CLuaController& cContr = dynamic_cast<CLuaController&>(c_fb.GetControllableEntity().GetController());
/* Get 'robot' table */
lua_getglobal(cContr.GetLuaState(), "robot");
/* Get 'in_chain' variable */
lua_getfield(cContr.GetLuaState(), -1, "in_chain");
if((! lua_isnil(cContr.GetLuaState(), -1)) && // 'in_chain' variable found
(lua_isnumber(cContr.GetLuaState(), -1))) { // its value is a number
/* Get value as a real number */
Real fInChain = lua_tonumber(cContr.GetLuaState(), -1);
/* If not zero, robot is in a chain */
if(fInChain != 0.0) {
bRetVal = true;
}
}
/* Clean up Lua stack */
lua_pop(cContr.GetLuaState(), 2);
/* Return result */
return bRetVal;
}
void CBuzzQT::Draw(CFootBotEntity& c_entity) {
Draw(dynamic_cast<CBuzzController&>(c_entity.GetControllableEntity().GetController()));
}
void CDynamics3DRemoveVisitor::Visit(CFootBotEntity& c_entity) {
m_cEngine.RemovePhysicsEntity(c_entity.GetId());
m_cEngine.RemoveControllableEntity(c_entity.GetId());
}
void CFootBotUN::ControlStep() {
printf("\n --------- UN %s - Simulation step %d -----------\n", GetRobot().GetRobotId().c_str(), CSimulator::GetInstance().GetSpace().GetSimulationClock());
m_pcLEDs->SetAllColors(CColor::BLUE);
/** Wifi communications section */
std::ostringstream str(ostringstream::out);
if (amIaGenerator) {
if (m_iSendOrNotSend % m_generatePacketInterval == 0) {
if (skipFirstSend) {
skipFirstSend = false;
} else {
sendPackets++;
str << "Hi I'm " << str_Me << " and I say \"Hello " << str_Dest << "\"";
str << ",Hi I'm " << str_Me << " and I say \"Hello " << str_Dest << "\"";
m_pcWifiActuator->SendMessageTo(str_Dest, str.str());
//std::cout << str_Me << " sending\n";
}
}
}
m_iSendOrNotSend++;
//searching for the received msgs
TMessageList t_incomingMsgs;
m_pcWifiSensor->GetReceivedMessages(t_incomingMsgs);
for (TMessageList::iterator it = t_incomingMsgs.begin(); it != t_incomingMsgs.end(); it++) {
receivedPackets++;
std::cout << str_Me << " received: " << it->Payload << " from " << it->Sender << " (total received=)" << receivedPackets << std::endl;
}
/** End of wifi*/
/** LCM for getting current positions and obtaining the next target point */
UInt8 robotID = atoi(GetRobot().GetRobotId().substr(3).c_str());
/* New target point from the COMMAND engine*/
if (lcmThreadCommand.getLcmHandler()->existNode(robotID)) {
Node nodeCommand = lcmThreadCommand.getLcmHandler()->getNodeById(robotID);
targetPosition.Set(nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
printf("ID %d - TARGET: (%f,%f)\n", robotID, nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
} else {
if (lcmThread.getLcmHandler()->existNode(robotID)) {
Node nodeCommand = lcmThread.getLcmHandler()->getNodeById(robotID);
targetPosition.Set(nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
printf("ID %d - TARGET: (%f,%f)\n", robotID, nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
}
}
if (lcmThread.getLcmHandler()->existNode(robotID)) {
//lcmThread.getLcmHandler()->printNodeListElements();
/** LCM related Node information */
//To get the other nodes locations through LCM
listNodeObstacles = lcmThread.getLcmHandler()->retrieveNodeList();
//Get myself
mySelf = lcmThread.getLcmHandler()->getNodeById(robotID);
printf("ID %d\n", mySelf.getId());
printf("POS (%f,%f)\n", mySelf.getPosition().GetX(), mySelf.getPosition().GetY());
printf("QUAT (%f,%f,%f,%f)\n", mySelf.getOrientation().GetW(), mySelf.getOrientation().GetX(), mySelf.getOrientation().GetY(), mySelf.getOrientation().GetZ());
printf("VEL %d\n", mySelf.getVelocity());
/** From the simulator */
CVector2 oldPosition = position;
CSpace& space = CSimulator::GetInstance().GetSpace();
const std::string stringIdA = GetRobot().GetRobotId();
CFootBotEntity *robotEntityA = static_cast<CFootBotEntity*>(&space.GetEntity(stringIdA));
CVector3 cFootBotPositionA = robotEntityA->GetEmbodiedEntity().GetPosition();
CQuaternion cFootBotOrientationA = robotEntityA->GetEmbodiedEntity().GetOrientation();
CVector3 axis;
CRadians oA;
cFootBotOrientationA.ToAngleAxis(oA, axis);
if (axis.GetZ() < 0)
oA = -oA;
// Position
position = CVector2(cFootBotPositionA.GetX(), cFootBotPositionA.GetY());
// Angle
angle = oA;
// Velocity
//velocity = CVector2(speed, 0);
velocity = CVector2(mySelf.getVelocity(), 0);
/** NAVIGATION AND AVOIDING COLLISION */
updateAgent(agent);
agent->clearObstacles();
addNodesAsObstacles(listNodeObstacles);
if ((targetPosition - position).Length() < m_targetMinPointDistance) { // 2 cm
state = ARRIVED_AT_TARGET;
printf("State: STOPPED\n");
} else {
state = MOVING;
printf("State: MOVING\n");
}
updateNavigation();
}
}
void CRecruitmentQTUserFunctions::Draw(CFootBotEntity& c_entity) {
CBTFootbotRandomForagingController& cController = dynamic_cast<CBTFootbotRandomForagingController&>(c_entity.GetControllableEntity().GetController());
}
