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