hlrc

#######################################

#allow forced disable of RSB

option(IGNORE_RSB "IGNORE_RSB" OFF)

#######################################

# check if we have RSB support:

IF (IGNORE_RSB)

    MESSAGE(INFO "RSB disabled per command line flag IGNORE_RSB")

ELSE (IGNORE_RSB)

FIND_PACKAGE(RSB 0.11 QUIET)

IF (RSB_FOUND)

    #RSB

    SET(CMAKE_INSTALL_RPATH "\$ORIGIN/../lib:\$ORIGIN/")

    FIND_PACKAGE(RSC 0.11 REQUIRED)

    FIND_PACKAGE(RSB 0.11 REQUIRED)

    #RST

    FIND_PACKAGE(RST REQUIRED COMPONENTS sandbox)

    INCLUDE_DIRECTORIES(BEFORE SYSTEM ${RST_INCLUDE_DIRS})

    ADD_DEFINITIONS(${RST_CFLAGS} ${RSTSANDBOX_CFLAGS})

    INCLUDE_DIRECTORIES(BEFORE SYSTEM ${RSB_INCLUDE_DIRS})

    LIST(INSERT CMAKE_MODULE_PATH 0 ${RSC_CMAKE_MODULE_PATH})

    #RSB

    SET(CMAKE_INSTALL_RPATH "\$ORIGIN/../lib:\$ORIGIN/")

    FIND_PACKAGE(RSC 0.11 REQUIRED)

    FIND_PACKAGE(RSB 0.11 REQUIRED)

    #RST

    FIND_PACKAGE(RST REQUIRED COMPONENTS sandbox)

    INCLUDE_DIRECTORIES(BEFORE SYSTEM ${RST_INCLUDE_DIRS})

    ADD_DEFINITIONS(${RST_CFLAGS} ${RSTSANDBOX_CFLAGS})

    INCLUDE_DIRECTORIES(BEFORE SYSTEM ${RSB_INCLUDE_DIRS})

    LIST(INSERT CMAKE_MODULE_PATH 0 ${RSC_CMAKE_MODULE_PATH})

    message(STATUS "RSB Support is ON")

    add_definitions(-DRSB_SUPPORT=1)

ELSE(RSB_FOUND)

    message(STATUS "RSB not found.")

ENDIF(RSB_FOUND)

ENDIF(IGNORE_RSB)

    src/MiddlewareRSB.cpp

target_link_libraries(${HLRC_CLIENT_LIBNAME} ${RSB_LIBRARIES} ${RST_LIBRARIES} ${catkin_LIBRARIES})

#TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${RSB_LIBRARIES} ${RST_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ${Boost_LIBS} ${BART_LIBRARIES} )

		printf("       <middleware> is either ROS or RSB\n");

	RobotController* robot_controller = new RobotController("RSB", "/flobi");

		printf("> ERROR: hlrc was compiled without ROS middleware support. Please use MiddlewareRSB() instead!\n\n");

/*

 * 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.

 *

 */

#pragma once

#include "Middleware.h"

#ifdef RSB_SUPPORT

#define BOOST_SIGNALS_NO_DEPRECATION_WARNING // mute warnings from RSB

#include <rsb/Factory.h>

#include <rsb/Informer.h>

#include <rsb/Listener.h>

#if USE_INPROTK_SYNTHESIS

#include <rst/communicationpatterns/TaskState.pb.h>

#endif

#endif

class MiddlewareRSB : public Middleware {

#ifndef RSB_SUPPORT

public:

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

		printf("> ERROR: hlrc was compiled without RSB middleware support. Please use MiddlewareROS() instead!\n\n");

		exit(EXIT_FAILURE);

	}

	void init(){};

	void publish_default_emotion(RobotEmotion e, bool blocking){};

	void publish_current_emotion(RobotEmotion e, bool blocking){};

	void publish_gaze_target(RobotGaze g, bool blocking){};

	void publish_lookat_target(float x, float y, float z, const std::string frame_id, bool blocking, float roll){};

	void publish_mouth_target(RobotMouth m, bool blocking){};

	void publish_head_animation(RobotHeadAnimation a, bool blocking){};

	void publish_speech(std::string text, bool blocking){};

#else

public:

	MiddlewareRSB(std::string scope);

protected:

	void init();

	void publish_current_emotion(RobotEmotion e, bool blocking);

	void publish_default_emotion(RobotEmotion e, bool blocking);

	void publish_gaze_target(RobotGaze target, bool blocking);

	void publish_lookat_target(float x, float y, float z, const std::string frame_id, bool blocking, float roll);

	void publish_mouth_target(RobotMouth target, bool blocking);

	void publish_head_animation(RobotHeadAnimation a, bool blocking);

	void publish_speech(std::string text, bool blocking);

private:

	void publish_emotion(std::string scope_target, RobotEmotion e, bool blocking);

	// rsb::patterns::RemoteServerPtr inprotk_server;

	rsb::patterns::RemoteServerPtr hlrc_server;

	// rsb::patterns::LocalServerPtr inprotk_server;

#if USE_INPROTK_SYNTHESIS

	rsb::Informer<rst::communicationpatterns::TaskState>::Ptr inprotk_informer;

	rsb::ListenerPtr inprotk_listener;

	rsb::ListenerPtr hack_listener;

	std::mutex pending_tasks_mutex;

	std::vector<std::shared_ptr<rst::communicationpatterns::TaskState> > pending_tasks;

	unsigned int say_task_active;

	unsigned int say_task_done;

	std::mutex current_saytask_mutex;

	std::string current_saytask;

	std::string get_current_saytask();

	void set_current_saytask(std::string text);

	void check_for_inprotk();

	void incoming_hack(std::shared_ptr<std::string> finished_task);

#endif

#endif

};

/*

 * 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/animation/EmotionExpression.pb.h>

#include <rst/animation/BinocularHeadGaze.pb.h>

#include <rst/animation/HeadAnimation.pb.h>

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

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

#include <boost/algorithm/string.hpp>

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

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::animation::EmotionExpression>());

		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::animation::BinocularHeadGaze>());

		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::animation::HeadAnimation>());

		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) {

	std::shared_ptr<rst::animation::EmotionExpression> request(new rst::animation::EmotionExpression());

	switch (e.value) {

		default:

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

			// fall through:

		case (RobotEmotion::NEUTRAL):

			request->set_emotion(rst::animation::EmotionExpression::NEUTRAL);

			break;

		case (RobotEmotion::HAPPY):

			request->set_emotion(rst::animation::EmotionExpression::HAPPY);

			break;

		case (RobotEmotion::SAD):

			request->set_emotion(rst::animation::EmotionExpression::SAD);

			break;

		case (RobotEmotion::ANGRY):

			request->set_emotion(rst::animation::EmotionExpression::ANGRY);

			break;

		case (RobotEmotion::SURPRISED):

			request->set_emotion(rst::animation::EmotionExpression::SURPRISED);

			break;

		case (RobotEmotion::FEAR):

			request->set_emotion(rst::animation::EmotionExpression::FEAR);

			break;

	}

	request->set_duration(e.time_ms);

	if (blocking) {

		hlrc_server->call<rst::animation::EmotionExpression>(scope_target, request);

	}

	else {

		hlrc_server->callAsync<rst::animation::EmotionExpression>(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 incoming_target, bool blocking) {

	std::shared_ptr<rst::animation::BinocularHeadGaze> request(new rst::animation::BinocularHeadGaze());

	std::shared_ptr<rst::geometry::SphericalDirectionFloat> target(new rst::geometry::SphericalDirectionFloat());

	target->set_azimuth(incoming_target.pan);

	target->set_elevation(incoming_target.tilt);

	request->set_allocated_target(target.get());

	request->set_eye_vergence(incoming_target.vergence);

	std::shared_ptr<rst::geometry::SphericalDirectionFloat> offset(new rst::geometry::SphericalDirectionFloat());

	offset->set_azimuth(incoming_target.pan_offset);

	offset->set_elevation(incoming_target.tilt_offset);

	request->set_allocated_offset(offset.get());

	if (blocking) {

		hlrc_server->call<rst::animation::BinocularHeadGaze>("gaze", request);

	}

	else {

		hlrc_server->callAsync<rst::animation::BinocularHeadGaze>("gaze", request);

	}

}

void MiddlewareRSB::publish_lookat_target(float x, float y, float z, const std::string frame_id, bool blocking, float roll) {

	std::cerr << "not yet implemented" << std::endl;

}

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

	/*

	    std::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);

	    }

	*/

	printf("> ERROR: mouth targets not yet implemented in RSB middleware!\n");

}

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

	std::shared_ptr<rst::animation::HeadAnimation> request(new rst::animation::HeadAnimation());

	switch (a.value) {

		default:

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

			// fall through:

		case (RobotHeadAnimation::IDLE):

			request->set_animation(rst::animation::HeadAnimation::IDLE);

			break;

		case (RobotHeadAnimation::HEAD_NOD):

			request->set_animation(rst::animation::HeadAnimation::HEAD_NOD);

			break;

		case (RobotHeadAnimation::HEAD_SHAKE):

			request->set_animation(rst::animation::HeadAnimation::HEAD_SHAKE);

			break;

		case (RobotHeadAnimation::EYEBLINK_L):

			request->set_animation(rst::animation::HeadAnimation::EYEBLINK_LEFT);

			break;

		case (RobotHeadAnimation::EYEBLINK_R):

			request->set_animation(rst::animation::HeadAnimation::EYEBLINK_RIGHT);

			break;

		case (RobotHeadAnimation::EYEBLINK_BOTH):

			request->set_animation(rst::animation::HeadAnimation::EYEBLINK_BOTH);

			break;

		case (RobotHeadAnimation::EYEBROWS_RAISE):

			request->set_animation(rst::animation::HeadAnimation::EYEBROWS_RAISE);

			break;

		case (RobotHeadAnimation::EYEBROWS_LOWER):

			request->set_animation(rst::animation::HeadAnimation::EYEBROWS_LOWER);

			break;

		case (RobotHeadAnimation::ENGAGEMENT_LEFT):

			request->set_animation(rst::animation::HeadAnimation::ENGAGEMENT_LEFT);

			break;

		case (RobotHeadAnimation::ENGAGEMENT_RIGHT):

			request->set_animation(rst::animation::HeadAnimation::ENGAGEMENT_RIGHT);

			break;

	}

	request->set_repetitions(a.repetitions);

	request->set_emphasis_scale(a.scale);

	request->set_duration_each(a.time_ms);

	if (blocking) {

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

	}

	else {

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

	}

}

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

	// say it

	std::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

#include "MiddlewareRSB.h"