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 "server/middleware_ros.h"

#ifdef ROS_SUPPORT

#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::server;

//! constructor

MiddlewareROS::MiddlewareROS(string scope, Controller *c) : Middleware(scope, c){

    //start ros core?

    if (ros::isInitialized()){

       //oh another process is doing ros comm, fine, we do not need to call it

        tick_necessary = false;

    }else{

        //we have to take care of ros

        printf("> no active ros middleware, calling ros::init() and we will call tick() periodically!");

        string node_name = "humotion_server__"+ 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());

        ros::M_string no_remapping;

        ros::init(no_remapping, node_name);

        tick_necessary = true;

    }

    //create node handle

    ros::NodeHandle n;

    //set up subscribers:

    mouth_target_subscriber = n.subscribe(scope + "/humotion/mouth/target", 150, &MiddlewareROS::incoming_mouth_target , this, ros::TransportHints().unreliable());

    gaze_target_subscriber  = n.subscribe(scope + "/humotion/gaze/target",  150, &MiddlewareROS::incoming_gaze_target, this, ros::TransportHints().unreliable());

}

//! 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();

    }

}

//! callback to handle incoming mouth  target

void MiddlewareROS::incoming_mouth_target(const humotion::mouth::ConstPtr& msg){

    //printf("> incoming mouth_target\n");

    MouthState mouth_state;

    mouth_state.position_left   = msg->position.left;

    mouth_state.position_center = msg->position.center;

    mouth_state.position_right  = msg->position.right;

    mouth_state.opening_left   = msg->opening.left;

    mouth_state.opening_center = msg->opening.center;

    mouth_state.opening_right  = msg->opening.right;

    controller->set_mouth_target(mouth_state);

}

//! this provides a consistent transformation of ros ts to double

double MiddlewareROS::convert_ros_to_timestamp_ms(ros::Time t){

        return t.sec + t.nsec/1000000000.0;

}

//! callback to handle incoming gaze  target

void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg){

    ///printf("> incoming gaze_target [P:%3.1f, T:%3.1f, R:%3.1f] %s\n", msg->pan, msg->tilt, msg->roll, msg->type==humotion::gaze::ABSOLUTE?"ABSOLUTE":"RELATIVE");

    GazeState gaze_state;

    gaze_state.type = msg->type;

    gaze_state.pan  = msg->pan;

    gaze_state.tilt = msg->tilt;

    gaze_state.roll = msg->roll;

    gaze_state.vergence = msg->vergence;

    gaze_state.pan_offset  = msg->pan_offset;

    gaze_state.tilt_offset = msg->tilt_offset;

    gaze_state.roll_offset = msg->roll_offset;

    gaze_state.eyelid_opening_upper = msg->eyelid_opening_upper;

    gaze_state.eyelid_opening_lower = msg->eyelid_opening_lower;

    gaze_state.eyebrow_left = msg->eyebrow_left;

    gaze_state.eyebrow_right = msg->eyebrow_right;

    gaze_state.eyeblink_request_left = msg->eyeblink_request_left;

    gaze_state.eyeblink_request_right = msg->eyeblink_request_right;

    gaze_state.timestamp = convert_ros_to_timestamp_ms(msg->header.stamp);

    controller->set_gaze_target(gaze_state);

}

#endif