hlrc

/*

* This file is part of hlrc

*

* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

* http://opensource.cit-ec.de/projects/hlrc

*

* This file may be licensed under the terms of the

* GNU General Public License Version 3 (the ``GPL''),

* or (at your option) any later version.

*

* Software distributed under the License is distributed

* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

* express or implied. See the GPL for the specific language

* governing rights and limitations.

*

* You should have received a copy of the GPL along with this

* program. If not, go to http://www.gnu.org/licenses/gpl.html

* or write to the Free Software Foundation, Inc.,

* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

*

* The development of this software was supported by the

* Excellence Cluster EXC 277 Cognitive Interaction Technology.

* The Excellence Cluster EXC 277 is a grant of the Deutsche

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*

*/

#ifdef RSB_SUPPORT

#include "MiddlewareRSB.h"

#include <rsb/converter/Repository.h>

#include <rsb/converter/ProtocolBufferConverter.h>

#include <rst/robot/EmotionState.pb.h>

#include <rst/robot/Animation.pb.h>

#include <rst/robot/GazeTarget.pb.h>

#include <rst/robot/MouthTarget.pb.h>

#include <rst/audition/Utterance.pb.h>

#include <boost/algorithm/string.hpp>

#include <boost/range/algorithm_ext/erase.hpp>

WARNING: RSB interface might be deprtecated and needs some backporting

from the ROS code. [todo]

using namespace std;

using namespace rsb;

using namespace rsb::patterns;

MiddlewareRSB::MiddlewareRSB(string scope) : Middleware(scope) {

    printf("> new MiddlewareRSB() on base scope '%s'\n",base_scope.c_str());

    init();

}

void MiddlewareRSB::init(void){

    printf("> MiddlewareRSB::init() registering converters\n");

    try{

        //converter for EmotionState

        rsb::converter::Converter<string>::Ptr emotionStateConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::EmotionState>());

        rsb::converter::converterRepository<string>()->registerConverter(emotionStateConverter);

        //converter for Utterance

        //rsb::converter::Converter<string>::Ptr UtteranceConverter(new rsb::converter::ProtocolBufferConverter<rst::audition::Utterance>());

        //rsb::converter::converterRepository<string>()->registerConverter(UtteranceConverter);

        //converter for GazeTarget

        rsb::converter::Converter<string>::Ptr gazeTargetConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::GazeTarget>());

        rsb::converter::converterRepository<string>()->registerConverter(gazeTargetConverter);

        //converter for MouthTarget

        rsb::converter::Converter<string>::Ptr mouthTargetConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::MouthTarget>());

        rsb::converter::converterRepository<string>()->registerConverter(mouthTargetConverter);

        //converter for Animation

        rsb::converter::Converter<string>::Ptr animationConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::Animation>());

        rsb::converter::converterRepository<string>()->registerConverter(animationConverter);

    }catch(std::invalid_argument e){

        printf("> converters already registered\n");

    }

    //first get a factory instance that is used to create RSB domain objects

    Factory &factory = getFactory();

    //get server

    string scope = base_scope + "/set/";

    hlrc_server = factory.createRemoteServer(scope);

    printf("> init done\n");

}

void MiddlewareRSB::publish_emotion(string scope_target, RobotEmotion e, bool blocking){

    boost::shared_ptr<rst::robot::EmotionState> request(new rst::robot::EmotionState());

    switch(e.value){

        default:

            printf("> WANRING: invalid emotion id %d. defaulting to NEUTRAL\n",e.value);

            //fall through:

        case(RobotEmotion::NEUTRAL):

            request->set_value(rst::robot::EmotionState::NEUTRAL);

            break;

        case(RobotEmotion::HAPPY):

            request->set_value(rst::robot::EmotionState::HAPPY);

            break;

        case(RobotEmotion::SAD):

            request->set_value(rst::robot::EmotionState::SAD);

            break;

        case(RobotEmotion::ANGRY):

            request->set_value(rst::robot::EmotionState::ANGRY);

            break;

        case(RobotEmotion::SURPRISED):

            request->set_value(rst::robot::EmotionState::SURPRISED);

            break;

        case(RobotEmotion::FEAR):

            request->set_value(rst::robot::EmotionState::FEAR);

            break;

    }

    request->set_duration(e.time_ms);

    if (blocking){

        hlrc_server->call<rst::robot::EmotionState>(scope_target, request);

    }else{

        hlrc_server->callAsync<rst::robot::EmotionState>(scope_target, request);

    }

}

void MiddlewareRSB::publish_current_emotion(RobotEmotion e, bool blocking){

    publish_emotion("currentEmotion", e, blocking);

}

void MiddlewareRSB::publish_default_emotion(RobotEmotion e, bool blocking){

    publish_emotion("defaultEmotion", e, blocking);

}

void MiddlewareRSB::publish_gaze_target(RobotGaze target, bool blocking){

    boost::shared_ptr<rst::robot::GazeTarget> request(new rst::robot::GazeTarget());

    request->set_pan(target.pan);

    request->set_tilt(target.tilt);

    request->set_roll(target.roll);

    request->set_vergence(target.vergence);

    request->set_pan_offset(target.pan_offset);

    request->set_tilt_offset(target.tilt_offset);

    if (blocking){

        hlrc_server->call<rst::robot::GazeTarget>("gaze", request);

    }else{

        hlrc_server->callAsync<rst::robot::GazeTarget>("gaze", request);

    }

}

void MiddlewareRSB::publish_mouth_target(RobotMouth target, bool blocking){

    boost::shared_ptr<rst::robot::MouthTarget> request(new rst::robot::MouthTarget());

    request->set_position_left(  target.position_left);

    request->set_position_center(target.position_center);

    request->set_position_right( target.position_right);

    request->set_opening_left(  target.opening_left);

    request->set_opening_center(target.opening_center);

    request->set_opening_right( target.opening_right);

    if (blocking){

        hlrc_server->call<rst::robot::MouthTarget>("mouth", request);

    }else{

        hlrc_server->callAsync<rst::robot::MouthTarget>("mouth", request);

    }

}

void MiddlewareRSB::publish_head_animation(RobotHeadAnimation a, bool blocking){

    boost::shared_ptr<rst::robot::Animation> request(new rst::robot::Animation());

    switch(a.value){

        default:

            printf("> WANRING: invalid animation id %d. defaulting to IDLE",a.value);

            //fall through:

        case(RobotHeadAnimation::IDLE):

            request->set_target(rst::robot::Animation::IDLE);

            break;

        case(RobotHeadAnimation::HEAD_NOD):

            request->set_target(rst::robot::Animation::HEAD_NOD);

            break;

        case(RobotHeadAnimation::HEAD_SHAKE):

            request->set_target(rst::robot::Animation::HEAD_SHAKE);

            break;

        case(RobotHeadAnimation::EYEBLINK_L):

            request->set_target(rst::robot::Animation::EYEBLINK_L);

            break;

        case(RobotHeadAnimation::EYEBLINK_R):

            request->set_target(rst::robot::Animation::EYEBLINK_R);

            break;

        case(RobotHeadAnimation::EYEBLINK_BOTH):

            request->set_target(rst::robot::Animation::EYEBLINK_BOTH);

            break;

        case(RobotHeadAnimation::EYEBROWS_RAISE):

            request->set_target(rst::robot::Animation::EYEBROWS_RAISE);

            break;

        case(RobotHeadAnimation::EYEBROWS_LOWER):

            request->set_target(rst::robot::Animation::EYEBROWS_LOWER);

            break;

    }

    request->set_repetitions(a.repetitions);

    request->set_scale(a.scale);

    request->set_duration_each(a.time_ms);

    if (blocking){

        hlrc_server->call<rst::robot::Animation>("animation", request);

    }else{

        hlrc_server->callAsync<rst::robot::Animation>("animation", request);

    }

}

void MiddlewareRSB::publish_speech(string text, bool blocking){

    //say it

    boost::shared_ptr<std::string> request(new string(text));

    if (blocking){

        hlrc_server->call<std::string>("speech", request);

    }else{

        hlrc_server->callAsync<std::string>("speech", request);

    }

}

#endif