hlrc

/*

 * This file is part of hlrc_server

 *

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

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

 *

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

 *

 */

#include "MiddlewareROS.h"

#ifdef ROS_SUPPORT

#include <cstdio>

// CallbackWrapper:

#include <boost/range/algorithm/remove_if.hpp>

#include <boost/algorithm/string/classification.hpp>

#include <actionlib/client/simple_action_client.h>

#include "ROS/UtteranceROS.h"

using namespace std;

using namespace hlrc_server;

template <typename actionspec>

actionlib::SimpleActionClient<actionspec>* create_action_client(string scope) {

        printf("> starting SimpleActionClient on %s\n", scope.c_str());

        actionlib::SimpleActionClient<actionspec>* ac = new actionlib::SimpleActionClient<actionspec>(scope, true);

        if (!ac->waitForServer(ros::Duration(1))) {

                printf("> ERROR: action service %s not ready, wait timed out...\n", scope.c_str());

                return NULL;

        }

        return ac;

}

MiddlewareROS::MiddlewareROS(Arbiter* arbiter, std::string scope) : Middleware(arbiter, scope) {

        init();

}

MiddlewareROS::~MiddlewareROS() {

}

void MiddlewareROS::init() {

        string scope = base_scope + "/set/";

        string node_name = "hlrc_server__" + base_scope;

        node_name.erase(boost::remove_if(node_name, boost::is_any_of("/")), node_name.end());

        printf("> registering on ROS as node %s\n", node_name.c_str());

        if (ros::isInitialized()) {

                // ros is already active, no need to take care of that

                // FIXME: Instead of deciding based on ros::isInitialized() to tick or not,

                // we shoud use our own MessageQueue and AsyncSpinner.

                tick_necessary = false;

        }

        else {

                printf("> ROS not initialized, calling ros::init()\n");

                ros::M_string no_remapping;

                ros::init(no_remapping, node_name);

                tick_necessary = true;

        }

        // create node handle

        // printf("> creating ros node handle\n");

        // ros::NodeHa

        ros::NodeHandle n;

        printf("> setting up ROS services...\n");

        // ros::AsyncSpinner *spinner = new ros::AsyncSpinner(4); // Use 4 threads

        // spinner->start();

        tfListener.reset(new tf2_ros::TransformListener(tfBuffer));

        tfBuffer.clear();

        animation_action_server = new AnimationCallbackWrapper(this, scope, "animation");

        utterance_action_server = new UtteranceCallbackWrapper(this, scope, "utterance");

        current_emotion_action_server = new EmotionCallbackWrapper(this, scope, "currentEmotion");

        default_emotion_action_server = new EmotionCallbackWrapper(this, scope, "defaultEmotion");

        gaze_action_server = new GazeCallbackWrapper(this, scope, "gaze");

        lookat_action_server = new LookatCallbackWrapper(this, scope, "lookat", tfBuffer);

        mouth_action_server = new MouthCallbackWrapper(this, scope, "mouth");

        speech_action_server = new SpeechCallbackWrapper(this, scope, "speech");

        // create tts client

        tts_ac = create_action_client<hlrc_server::ttsAction>(base_scope + "/tts_provider");

        if (tts_ac == NULL) {

                printf("> tts action service not available, will not do any TTS !\n");

        }

        printf("> init done\n");

}

void MiddlewareROS::tick() {

        if (tick_necessary) {

                ros::spinOnce();

        }

}

// call a tts system to convert a string to an utterance

std::shared_ptr<Utterance> MiddlewareROS::tts_call(string text) {

        std::shared_ptr<Utterance> utterance(new Utterance());

        // double tts_timeout = 1.0; //seconds. DO NOT CHANGE THIS!

        if (tts_ac == NULL) {

                printf("> tts action service not available. will not speak '%s'\n", text.c_str());

                return utterance;

        }

        hlrc_server::ttsGoal goal;

        goal.text = text;

        // send

        tts_ac->sendGoal(goal);

        // call ros:

        bool finished_before_timeout = tts_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));

        if (!finished_before_timeout) {

                printf("> ERROR: NO REPLY to utterance action call received within %4.2f s\n", ROS_ACTION_CALL_TIMEOUT);

        }

        else {

                // done, return utterance ptr

                ttsResultConstPtr tts_res = tts_ac->getResult();

                std::shared_ptr<Utterance> utterance(new UtteranceROS(tts_res));

                printf("> done. got utterance (text=%s)\n", utterance->get_text().c_str());

                return utterance;

        }

        printf("> failed... got no utterance\n");

        return utterance;

}

#endif