src/controllers/fitnessFramework/choreoFitness.cpp

//#ifdef PARTNER_ALMENDE

#include <iostream>

#include <dtGame/gmcomponent.h>
#include <controllers/fitnessFramework/choreoFitness.h>
#include <controllers/fitnessFramework/almChoreoAgent.h>

namespace srAlmende {

ChoreoFitness::ChoreoFitness(const std::string &name) :
        dtGame::GMComponent(name),
        trial_length (300),
        trial_tick (0),
        enabled(false) {
}

unsigned char ChoreoFitness::CalcFitness(unsigned int time_tick,
                unsigned char actual_success, unsigned char perceived_success) {
        if (time_tick == 0) return 0xFF;
        int diff = (int)actual_success - (int)perceived_success;
        if (diff < 0) diff = 0 - diff;

        unsigned char periods = 5;

        unsigned char ratio = trial_length * periods / time_tick;

        return (ratio * diff) + (periods - ratio) * actual_success;
}

unsigned char ChoreoFitness::Synchronized(std::vector <osg::Vec3f> orientations) {
        std::vector<osg::Vec3f>::const_iterator i;
        osg::Vec3f start = *orientations.begin(), result;
        for( i = orientations.begin()+1; i < orientations.end(); ++i){
                result = *i - start;
        }
        return result.x() + result.y() + result.z();
}

unsigned char ChoreoFitness::Average(std::vector<unsigned char> values) {
        std::vector<unsigned char>::const_iterator i;
        unsigned int result = 0;
        for( i = values.begin(); i < values.end(); ++i){
                result += *i;
        }
        return (unsigned char)(result / values.size());
}

void ChoreoFitness::ProcessMessage(const dtGame::Message& message) {

        if ((message.GetMessageType() == dtGame::MessageType::TICK_LOCAL) && (enabled)) {

                std::vector< osg::Vec3f> positions, orientations;
                std::vector< unsigned char> successes;
                unsigned char success, synchronized;

                //std::cout << "Tick in ChoreoFitness component" << std::endl;

                //trial_tick++;
                if (trial_tick >= trial_length) {
                        std::vector< dtGame::GMComponent* > allComponents;
                        GetGameManager()->GetAllComponents(allComponents);
                        for (unsigned int i = 0; i < allComponents.size(); i++) {
                                std::string name = allComponents[i]->GetName();
                                if(name.find("almChoreoAgent",0) == std::string::npos) continue;
                                AlmChoreoAgent *agent = dynamic_cast<AlmChoreoAgent*>(allComponents[i]);
                                dtCore::RefPtr<srExternal::NonEmulationMessage> tcpipMsg =
                                        new srExternal::NonEmulationMessage();
                                GetGameManager()->GetMessageFactory().CreateMessage(
                                                srExternal::NonEmulationMessageType::TCPIP, tcpipMsg);
                                if (GetGameManager()->GetMessageFactory().IsMessageTypeSupported(
                                                tcpipMsg->GetMessageType())) {
                                        dtCore::UniqueId *id = agent->getBusID();
                                        tcpipMsg->setBusID(id);
                                        std::ostringstream msg_stream;
                                        unsigned char msg_size = 5;
                                        unsigned char origin = 0xFF; //comes from component
                                        msg_stream << msg_size;
                                        msg_stream << origin;
                                        msg_stream << fitness;
                                        tcpipMsg->setMessage(msg_stream.str());
                                        tcpipMsg->setMsgType(srExternal::NonEmulationMessageType::FITNESS);
                                        GetGameManager()->SendMessage(*tcpipMsg);
                                        std::cout << "Send a fitness message" << std::endl;
                                }
                        }
                        enabled = false;
                        fitness = 0;
                        trial_tick = 0;
                }

                positions.clear(); orientations.clear(); successes.clear();

                std::vector< dtGame::GMComponent* > allComponents;
                GetGameManager()->GetAllComponents(allComponents);
                for (unsigned int i = 0; i < allComponents.size(); i++) {
                        std::string name = allComponents[i]->GetName();
                        if(name.find("almChoreoAgent",0) == std::string::npos) continue;
                        //std::cout << "Found a ChoreoAgent" << std::endl;
                        AlmChoreoAgent *agent = dynamic_cast<AlmChoreoAgent*>(allComponents[i]);
                        successes.push_back(agent->getChoreoSignal());
                        positions.push_back(agent->getRobotPosition());
                        orientations.push_back(agent->getRobotRotation());
                }

                if (successes.empty()) {
                        std::cout << "Choreo Fitness module can't find any Choreo Agents." << std::endl;
                        std::cout << "   It will disable itself." << std::endl;
                        enabled = false;
                        return;
                }
                //std::cout << "Calculate synchronization achievement." << std::endl;
                synchronized = Synchronized(orientations);
                //std::cout << "Calculate average." << std::endl;
                success = Average(successes);
                fitness += CalcFitness(trial_tick, synchronized, success);
                //              if (!(trial_tick % (trial_length / 5)))
                //                      std::cout << "Fitness becomes: " << fitness << std::endl;
        }

        if (message.GetMessageType() == srExternal::NonEmulationMessageType::TCPIP) {
                srExternal::NonEmulationMessage *neMsg = dynamic_cast<srExternal::NonEmulationMessage*>(
                                const_cast<dtGame::Message*>(&message));
                srExternal::NonEmulationMessageType::msgType type = neMsg->getMsgType();
                switch(type) {
                case srExternal::NonEmulationMessageType::POSITION: {
//                      std::cout << "Received a position message" << std::endl;
                        trial_tick = 0;
                        enabled = true;
                        break;
                }
                case srExternal::NonEmulationMessageType::ACTUATOR: {
//                      std::cout << "Received an actuator message" << std::endl;
                        trial_tick++;
                        break;
                }
                default: break;
                }

        }

}

} //end of namespace

//#endif

---