C++ Human::getId方法代码示例

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