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 ROS_SUPPORT

#include "MiddlewareROS.h"

#include <stdio.h>

#include <boost/algorithm/string.hpp>

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

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

using namespace std;

MiddlewareROS::MiddlewareROS(string scope) : Middleware(scope) {

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

    init();

}

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

        char buf[256];

        snprintf(buf, 256, "ERROR: action service %s not ready", scope.c_str());

        throw runtime_error(buf);

    }

    return ac;

}

void MiddlewareROS::init(void){

    printf("> MiddlewareROS::init()\n");

    string scope = base_scope + "/set";

    string node_name = "hlrc_client__"+ base_scope;

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

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

    if (!ros::isInitialized()){

        ros::M_string no_remapping;

        ros::init(no_remapping, node_name);

    }

    //create node handle

    //ros_node_handle = new ros::NodeHandle();

    //node_handle = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle());

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

    animation_ac = create_action_client<hlrc_server::animationAction>(scope + "/animation");

    utterance_ac = create_action_client<hlrc_server::utteranceAction>(scope + "/utterance");

    current_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/currentEmotion");

    default_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/defaultEmotion");

    gazetarget_ac = create_action_client<hlrc_server::gazetargetAction>(scope + "/gaze");

    lookattarget_ac = create_action_client<hlrc_server::lookattargetAction>(scope + "/lookat");

    mouthtarget_ac = create_action_client<hlrc_server::mouthtargetAction>(scope + "/mouth");

    speech_ac = create_action_client<hlrc_server::speechAction>(scope + "/speech");

    printf("> init done.\n");

}

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

    hlrc_server::emotionstateGoal goal;

    switch(e.value){

        default:

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

            //fall through:

        case(RobotEmotion::NEUTRAL):

            goal.value = hlrc_server::emotionstateGoal::NEUTRAL;

            break;

        case(RobotEmotion::HAPPY):

            goal.value = hlrc_server::emotionstateGoal::HAPPY;

            break;

        case(RobotEmotion::SAD):

            goal.value = hlrc_server::emotionstateGoal::SAD;

            break;

        case(RobotEmotion::ANGRY):

            goal.value = hlrc_server::emotionstateGoal::ANGRY;

            break;

        case(RobotEmotion::SURPRISED):

            goal.value = hlrc_server::emotionstateGoal::SURPRISED;

            break;

        case(RobotEmotion::FEAR):

            goal.value = hlrc_server::emotionstateGoal::FEAR;

            break;

    }

    goal.duration = e.time_ms;

    actionlib::SimpleActionClient<hlrc_server::emotionstateAction>  *ac;

    if (scope_target == "defaultEmotion"){

        ac = default_emotionstate_ac;

    }else{

        ac = current_emotionstate_ac;

    }

    //send

    ac->sendGoal(goal);

    if (blocking){

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

        if (!finished_before_timeout){

            printf("> ERROR: blocking call timed out!\n");

        }

    }

}

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

    publish_emotion("currentEmotion", e, blocking);

}

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

    publish_emotion("defaultEmotion", e, blocking);

}

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

    hlrc_server::gazetargetGoal goal;

    goal.pan = target.pan;

    goal.tilt = target.tilt;

    goal.roll = target.roll;

    goal.vergence = target.vergence;

    goal.tilt_offset = target.tilt_offset;

    goal.pan_offset = target.pan_offset;

    //timestamp:

    goal.gaze_timestamp.sec  = target.gaze_timestamp.sec;

    goal.gaze_timestamp.nsec = target.gaze_timestamp.nsec;

    if (target.gaze_type == RobotGaze::ABSOLUTE){

        goal.gaze_type = hlrc_server::gazetargetGoal::GAZETARGET_ABSOLUTE;

    }else{

        goal.gaze_type = hlrc_server::gazetargetGoal::GAZETARGET_RELATIVE;

    }

    //send

    gazetarget_ac->sendGoal(goal);

    if (blocking){

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

        if (!finished_before_timeout){

            printf("> ERROR: blocking call timed out!\n");

        }

    }

}

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

{

    hlrc_server::lookattargetGoal goal;

    goal.point.header.frame_id = frame_id;

    goal.point.header.stamp = ros::Time::now();

    geometry_msgs::Point &p = goal.point.point;

    p.x = x;

    p.y = y;

    p.z = z;

    goal.roll = roll;

    //send

    lookattarget_ac->sendGoal(goal);

    if (blocking){

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

        if (!finished_before_timeout){

            printf("> ERROR: blocking call timed out!\n");

        }

    }

}

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

    hlrc_server::mouthtargetGoal goal;

    goal.position_left   = target.position_left;

    goal.position_center = target.position_center;

    goal.position_right  = target.position_right;

    goal.opening_left   = target.opening_left;

    goal.opening_center = target.opening_center;

    goal.opening_right  = target.opening_right;

    //send

    mouthtarget_ac->sendGoal(goal);

    if (blocking){

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

        if (!finished_before_timeout){

            printf("> ERROR: blocking call timed out!\n");

        }

    }

}

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

    hlrc_server::animationGoal goal;

    switch(a.value){

        default:

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

            //fall through:

        case(RobotHeadAnimation::IDLE):

            goal.target = hlrc_server::animationGoal::IDLE;

            break;

        case(RobotHeadAnimation::HEAD_NOD):

            goal.target = hlrc_server::animationGoal::HEAD_NOD;

            break;

        case(RobotHeadAnimation::HEAD_SHAKE):

            goal.target = hlrc_server::animationGoal::HEAD_SHAKE;

            break;

        case(RobotHeadAnimation::EYEBLINK_L):

            goal.target = hlrc_server::animationGoal::EYEBLINK_L;

            break;

        case(RobotHeadAnimation::EYEBLINK_R):

            goal.target = hlrc_server::animationGoal::EYEBLINK_R;

            break;

        case(RobotHeadAnimation::EYEBLINK_BOTH):

            goal.target = hlrc_server::animationGoal::EYEBLINK_BOTH;

            break;

        case(RobotHeadAnimation::EYEBROWS_RAISE):

            goal.target = hlrc_server::animationGoal::EYEBROWS_RAISE;

            break;

        case(RobotHeadAnimation::EYEBROWS_LOWER):

            goal.target = hlrc_server::animationGoal::EYEBROWS_LOWER;

            break;

    }

    goal.repetitions = a.repetitions;

    goal.scale       = a.scale;

    goal.duration_each = a.time_ms;

    //send

    animation_ac->sendGoal(goal);

    if (blocking){

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

        if (!finished_before_timeout){

            printf("> ERROR: blocking call timed out!\n");

        }

    }

}

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

    hlrc_server::speechGoal goal;

    goal.text = text;

    //send

    speech_ac->sendGoal(goal);

    if (blocking){

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

        if (!finished_before_timeout){

            printf("> ERROR: blocking call timed out!\n");

        }

    }

}

#endif