humotion

/*

* This file is part of humotion

*

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

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

*

* This file may be licensed under the terms of the

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

* 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 LGPL for the specific language

* governing rights and limitations.

*

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

* program. If not, go to http://www.gnu.org/licenses/lgpl.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 "client/middleware_ros.h"

#include "humotion/mouth.h"

#include "humotion/gaze.h"

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

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

using namespace std;

using namespace boost;

using namespace humotion;

using namespace humotion::client;

//! constructor

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

    //start ros core

    if (!ros::isInitialized()){

        tick_necessary = true;

        string node_name = "humotion_client__"+ base_scope;

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

        ros::M_string no_remapping;

        ros::init(no_remapping, node_name);

    }else{

        //another ros thread takes care of spinning

        tick_necessary = false;

    }

    //create node handle

    ros::NodeHandle n;

    //set up publishers:

    mouth_target_publisher = n.advertise<humotion::mouth>(base_scope + "/humotion/mouth/target", 100);

    gaze_target_publisher  = n.advertise<humotion::gaze>(base_scope + "/humotion/gaze/target", 100);

}

//! destructor

MiddlewareROS::~MiddlewareROS(){

}

//! connection ok?

//! \return true if conn is alive

bool MiddlewareROS::ok(){

    return ros::ok();

}

//! do a single tick

void MiddlewareROS::tick(){

    if (tick_necessary){

        ros::spinOnce();

    }

}

//! send mouth target to server

void MiddlewareROS::send_mouth_target(){

    //build target packet:

    humotion::mouth msg;

    //set timestamp

    msg.header.stamp = ros::Time::now();

    msg.position.left   = mouth_state.position_left;

    msg.position.center = mouth_state.position_center;

    msg.position.right  = mouth_state.position_right;

    msg.opening.left   = mouth_state.opening_left;

    msg.opening.center = mouth_state.opening_center;

    msg.opening.right  = mouth_state.opening_right;

    //add position to send queue

    mouth_target_publisher.publish(msg);

    //allow ros to handle data

    tick();

}

//! send mouth target to server

void MiddlewareROS::send_gaze_target(){

    //build target packet:

    humotion::gaze msg;

    //set timestamp

    msg.header.stamp = ros::Time::now();

    msg.pan   = gaze_state.pan;

    msg.tilt  = gaze_state.tilt;

    msg.roll  = gaze_state.roll;

    msg.vergence = gaze_state.vergence;

    msg.pan_offset = gaze_state.pan_offset;

    msg.tilt_offset = gaze_state.tilt_offset;

    msg.roll_offset = gaze_state.roll_offset;

    msg.eyelid_opening_upper = gaze_state.eyelid_opening_upper;

    msg.eyelid_opening_lower = gaze_state.eyelid_opening_lower;

    msg.eyebrow_left = gaze_state.eyebrow_left;

    msg.eyebrow_right = gaze_state.eyebrow_right;

    msg.eyeblink_request_left = gaze_state.eyeblink_request_left;

    msg.eyeblink_request_right = gaze_state.eyeblink_request_right;

    if (gaze_state.gaze_type == GazeState::GAZETYPE_ABSOLUTE){

        msg.gaze_type = humotion::gaze::GAZETYPE_ABSOLUTE;

    }else{

        msg.gaze_type = humotion::gaze::GAZETYPE_RELATIVE;

    }

    msg.gaze_timestamp.sec = gaze_state.timestamp.sec;

    msg.gaze_timestamp.nsec = gaze_state.timestamp.nsec;

    //add position to send queue

    gaze_target_publisher.publish(msg);

    //allow ros to handle data

    tick();

}