src/controllers/fitnessFramework/almEvolutionAgent.cpp

//#ifdef PARTNER_ALMENDE

#include <iostream>
#include <controllers/fitnessFramework/almEvolutionAgent.h>
#include <controllers/externalControllers/tcpip/nonEmulationMessage.h>

namespace srAlmende {

AlmEvolutionAgent::AlmEvolutionAgent(const std::string &name,
                srCore::RobotActorBase *robotActor):
                        srCore::ControllerComponentBase(name, robotActor)
{

}

AlmEvolutionAgent::~AlmEvolutionAgent() {

}

void AlmEvolutionAgent::ProcessMessage(const dtGame::Message& message) {
        if (message.GetMessageType() == srExternal::NonEmulationMessageType::TCPIP) {

                //std::cout << "AlmEvolutionAgent gets TCP/IP message" << std::endl;

                srExternal::NonEmulationMessage *neMsg = dynamic_cast<srExternal::NonEmulationMessage*>(
                                const_cast<dtGame::Message*>(&message));
                dtCore::UniqueId* busID = neMsg->getBusID();
                std::string msg = neMsg->getMessage();
                srExternal::NonEmulationMessageType::msgType type = neMsg->getMsgType();

                const char *arr_msg = msg.c_str();

                std::string rid = getBusID()->ToString();

                switch (type) {

                case srExternal::NonEmulationMessageType::POSITION:
                        //std::cout << "AlmEvolutionAgent " << rid << " gets TCP/IP POSITION message" << std::endl;

                        if (busID == getBusID()) {

//                              std::cout << "An AlmEvolutionAgent gets a TCP/IP POSITION message" << std::endl;
//                              printf("[0,1,2,3,4,5] = [%i,%i,%i,%i,%i,%i]\n",
//                                              arr_msg[0],arr_msg[1],arr_msg[2],arr_msg[3],arr_msg[4],arr_msg[5]);
                                int x = (arr_msg[0] << 8) + arr_msg[1];
                                int y = (arr_msg[2] << 8) + arr_msg[3];
                                int z = (arr_msg[4] << 8) + arr_msg[5];
//                              printf("  [x,y,z] = [%i,%i,%i]\n",x,y,z);
                                osg::Vec3 pos;
                                pos.set(x,y,z);
                                setRobotTranslation(pos);
                        }
                        break;
                default:
                        return;
                }
        }
}

} //end of namespace

//#endif

---