void ToasterHumanReader::humanJointStateCallBack(const toaster_msgs::HumanList::ConstPtr& msg) {
//std::cout << "[area_manager][DEBUG] new data for human received with time " << msg->humanList[0].meAgent.meEntity.time << std::endl;
Human * curHuman;
for (unsigned int i = 0; i < msg->humanList.size(); i++) {
// If this human is not assigned we have to allocate data.
if (lastConfig_.find(msg->humanList[i].meAgent.meEntity.id) == lastConfig_.end()) {
curHuman = new Human(msg->humanList[i].meAgent.meEntity.id);
} else
curHuman = lastConfig_[msg->humanList[i].meAgent.meEntity.id];
std::vector<double> humanOrientation;
bg::model::point<double, 3, bg::cs::cartesian> humanPosition;
Mobility curHumanMobility = FULL;
curHuman->setId(msg->humanList[i].meAgent.meEntity.id);
curHuman->setName(msg->humanList[i].meAgent.meEntity.name);
curHuman->setMobility(curHumanMobility);
curHuman->setTime(msg->humanList[i].meAgent.meEntity.time);
curHuman->busyHands_ = msg->humanList[i].meAgent.busyHands;
humanPosition.set<0>(msg->humanList[i].meAgent.meEntity.positionX);
humanPosition.set<1>(msg->humanList[i].meAgent.meEntity.positionY);
humanPosition.set<2>(msg->humanList[i].meAgent.meEntity.positionZ);
curHuman->setPosition(humanPosition);
humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationRoll);
humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationPitch);
humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationYaw);
curHuman->setOrientation(humanOrientation);
lastConfig_[curHuman->getId()] = curHuman;
//TODO: fullHuman
if (fullHuman_) {
Joint * curJnt;
for (unsigned int i_jnt = 0; i_jnt < msg->humanList[i].meAgent.skeletonNames.size(); i_jnt++) {
// If this joint is not assigned we have to allocate data.
if (lastConfig_[curHuman->getId()]->skeleton_[msg->humanList[i].meAgent.skeletonNames[i_jnt] ] == NULL) {
curJnt = new Joint(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.id, msg->humanList[i].meAgent.meEntity.id);
} else
curJnt = lastConfig_[curHuman->getId()]->skeleton_[msg->humanList[i].meAgent.skeletonNames[i_jnt] ];
std::vector<double> jointOrientation;
bg::model::point<double, 3, bg::cs::cartesian> jointPosition;
curJnt->setName(msg->humanList[i].meAgent.skeletonNames[i_jnt]);
curJnt->setAgentId(curHuman->getId());
curJnt->setTime(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.time);
jointPosition.set<0>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionX);
jointPosition.set<1>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionY);
jointPosition.set<2>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionZ);
curJnt->setPosition(jointPosition);
jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationRoll);
jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationPitch);
jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationYaw);
curJnt->setOrientation(jointOrientation);
lastConfig_[curHuman->getId()]->skeleton_[curJnt->getName()] = curJnt;
}
}
}
}
void WorldModel::updatePositions()
{
for (int i = 0; i < (int) ambulanceTeams.size(); i++)
{
Human *human = ambulanceTeams[i];
if (human->getMotionlessObject() == NULL || human->getMotionlessObject()->getId() != human->getPosition())
{
human->setMotionlessObject((MotionlessObject*) objects[human->getPosition()]);
LOG(Main, 1) << "update position of " << human->getId() << " : " << human->getMotionlessObject()->getId() << endl;
}
}
for (int i = 0; i < (int) humans.size(); i++)
{
Human *human = humans[i];
if (!human->isAmbulanceTeam())
{
bool flag = false;
for (int j = 0; j < (int) ambulanceTeams.size(); j++)
if (ambulanceTeams[j]->getId() == human->getPosition())
{
human->setMotionlessObject((MotionlessObject*) objects[ambulanceTeams[j]->getPosition()]);
LOG(Main, 1) << "update position of (loaded in ambulance) " << human->getId() << " : " << human->getMotionlessObject()->getId() << endl;
flag = true;
break;
}
if (human->getMotionlessObject() == NULL || (human->getMotionlessObject()->getId() != human->getPosition() && !flag))
{
human->setMotionlessObject((MotionlessObject*) objects[human->getPosition()]);
LOG(Main, 1) << "update position of " << human->getId() << " : " << human->getMotionlessObject()->getId() << endl;
}
}
if (human->getLastCycleUpdatedBySense() != world->getTime())
{
human->setX(human->getMotionlessObject()->getX());
human->setY(human->getMotionlessObject()->getY());
}
}
if (self->isPlatoon())
{
for (int i = 0; i < civilians.size(); i++)
{
if (((Platoon*) self)->getMotionlessObject()->motionlessIndex == civilians[i]->getMotionlessObject()->motionlessIndex && getTime() - civilians[i]->getLastCycleUpdated() > 1)
civilians[i]->isAvailable = false;
}
}
//set civilians to buildingBlocks
if (self->isPoliceForce())
{
for (int i = 0; i < buildingBlocks.size(); i++)
buildingBlocks[i]->civilians.clear();
for (int i = 0; i < civilians.size(); i++)
{
if (civilians[i]->getMotionlessObject()->isBuilding())
{
LOG(Main, 4) << "civ: " << civilians[i]->getId() << endl;
LOG(Main, 4) << "((Building*)civilians[i]->getMotionlessObject())->blockNum = " << ((Building*) civilians[i]->getMotionlessObject())->blockNum << endl;
buildingBlocks[((Building*) civilians[i]->getMotionlessObject())->blockNum]->civilians.push_back(civilians[i]);
}
}
}
if (self->isPlatoon())
((Platoon*) self)->getMotionlessObject()->iHaveBeenHere = true;
}
bool AmbulanceTeamAgent::simpleDecision()
{
LOG(Main, 1) << "Simple Decision begins" << endl;
// for (int i = 0; i < neededHelpHumans.size(); i++)
// setValue(neededHelpHumans[i]);
sort(neededHelpHumans.begin(), neededHelpHumans.end(), humanComparator);
Human* myTarget;
set<int> busyAgents;
for (int i = 0; i < neededHelpHumans.size(); i++)
{
int neededAmbulances = getNumOfNeededAmbulances(neededHelpHumans[i]);
LOG(Main, 1) << "neededAmbulances for " << neededHelpHumans[i]->getId() << " is " << neededAmbulances << endl;
for (int j = 0; j < neededAmbulances; j++)
{
int minDist = MAX_INT;
AmbulanceTeam* at = NULL;
for (int k = 0; k < world->ambulanceTeams.size(); k++)
{
if (busyAgents.find(world->ambulanceTeams[k]->ambulanceTeamIndex) == busyAgents.end() && worldGraph->isReachable(world->ambulanceTeams[k]->getRepresentiveNodeIndex(), neededHelpHumans[i]->getRepresentiveNodeIndex(), GM_DEFAULT_NOT_BLOCKED))
{
int dist = worldGraph->getDistance(world->ambulanceTeams[k]->getRepresentiveNodeIndex(), neededHelpHumans[i]->getRepresentiveNodeIndex(), GM_DEFAULT_NOT_BLOCKED);
// LOG(Main, 1) << "dist " << world->ambulanceTeams[k]->getId() << " from civ is " << dist << endl;
if (dist < minDist)
{
// LOG(Main, 1) << "in if" << endl;
minDist = dist;
at = world->ambulanceTeams[k];
// LOG(Main, 1) << "at->getId " << at->getId() << endl;
}
}
}
if (at != NULL)
{
if (at->ambulanceTeamIndex == self()->ambulanceTeamIndex)
{
myTarget = neededHelpHumans[i];
break;
}
else
{
LOG(Main, 1) << "ambulance " << at->getId() << " goes to " << neededHelpHumans[i]->getId() << endl;
busyAgents.insert(at->ambulanceTeamIndex);
}
}
}
if (myTarget != NULL)
break;
}
if (myTarget != NULL)
{
LOG(Main, 1) << "my Target: " << myTarget->getId() << " its postion is " << myTarget->getMotionlessObject()->getId() << endl;
if (self()->getMotionlessObject() != myTarget->getMotionlessObject())
{
LOG(Main, 1) << "move to myTarget " << myTarget->getId() << endl;
command->moveToMotionless(myTarget->getMotionlessObject()->motionlessIndex, GM_DEFAULT_NOT_BLOCKED);
return true;
}
else if (myTarget->getBuriedness() > 0)
{
LOG(Main, 1) << "I am rescuing my target " << endl;
command->rescue(*myTarget);
return true;
}
else
{
if (myTarget->isCivilian())
{
int minId = self()->getId();
for (int i = 0; i < world->ambulanceTeams.size(); i++)
if (world->ambulanceTeams[i]->getMotionlessObject() == myTarget->getMotionlessObject() && world->ambulanceTeams[i]->getId() < minId)
minId = world->ambulanceTeams[i]->getId();
if (minId == self()->getId())
{
LOG(Main, 1) << "I am loading my target" << endl;
command->load(*myTarget);
return true;
}
else
{
myTarget->isAvailable = false;
}
}
}
}
return false;
}