src/controllers/fitnessFramework/almChoreoAgent.cpp

//#ifdef PARTNER_ALMENDE

#include <controllers/fitnessFramework/almChoreoAgent.h>
#include <controllers/fitnessFramework/almPuppet.h>
#include "basic/robotActorBase.h"

namespace srAlmende {

AlmChoreoAgent::AlmChoreoAgent(const std::string &name,
                srCore::RobotActorBase *robotActor):
                        AlmPuppet(name, robotActor),
                        success (0),
                        rightVel(0),
                        leftVel(0) {
}

AlmChoreoAgent::~AlmChoreoAgent() {

}

void AlmChoreoAgent::ProcessMessage(const dtGame::Message& message) {
        AlmPuppet::ProcessMessage(message);

        //ProcessSensorMessage();
}

void AlmChoreoAgent::ProcessSensorMessage() {
        unsigned long sensorLeft, sensorRight;
        getSensor("Front Left DistanceSensor")->evaluate(srCore::MAKE_PARAMETER_VALUE(&sensorLeft));
        getSensor("Front Right DistanceSensor")->evaluate(srCore::MAKE_PARAMETER_VALUE(&sensorRight));
        sensorLeft = sensorLeft >> 16;
        sensorRight = sensorRight >> 16;
//      std::cout << "Will send sensor values: " << (int)sensorLeft << " & "
//                        << (int)sensorRight << std::endl;
        dtCore::RefPtr<srExternal::NonEmulationMessage> tcpipMsg = new srExternal::NonEmulationMessage();
        GetGameManager()->GetMessageFactory().CreateMessage(
                        srExternal::NonEmulationMessageType::TCPIP, tcpipMsg);
        if (GetGameManager()->GetMessageFactory().IsMessageTypeSupported(
                        tcpipMsg->GetMessageType())) {
                dtCore::UniqueId *id = getBusID();
                tcpipMsg->setBusID(id);
                std::ostringstream msg_stream;
                unsigned char msg_size = 8;
                unsigned char origin = 0;
                msg_stream << msg_size;
                msg_stream << origin;
                msg_stream << static_cast<unsigned int>(sensorLeft);
                msg_stream << static_cast<unsigned int>(sensorRight);
                tcpipMsg->setMessage(msg_stream.str());
                tcpipMsg->setMsgType(srExternal::NonEmulationMessageType::SENSOR);
//              std::cout << "Message will be send via the GameManager from [" << id->ToString() << "]" << std::endl;
                GetGameManager()->SendMessage(*tcpipMsg);
                //std::cout << "Send a sensor message" << std::endl;
        }
}

void AlmChoreoAgent::ProcessActuatorMessage(srExternal::NonEmulationMessage *neMsg) {
        std::string msg = neMsg->getMessage();
        const char *arr_msg = msg.c_str();
        rightVel = arr_msg[0];
        leftVel = arr_msg[1];
        setActuatorVelocity(getActuator(1), rightVel);
        setActuatorVelocity(getActuator(2), leftVel);
//      std::cout << "Did set actuator velocities: " << (int)leftVel << " m/s & "
//                        << (int)rightVel << " m/s."<< std::endl;
        ProcessSensorMessage(); //only send sensor msg when actuator msg received
}

unsigned char AlmChoreoAgent::getChoreoSignal() {
        return success;
}

} //end of namespace

//#endif

---