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;
}
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();
}
}
