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.

*

*/

#pragma once

#include "hlrc_server/gazetargetAction.h"

#include "CallbackWrapperROS.h"

//callback handler incoming gaze requests:

class GazeCallbackWrapper : CallbackWrapper<hlrc_server::gazetargetAction>{

protected:

    hlrc_server::gazetargetFeedback feedback;

    hlrc_server::gazetargetResult result;

public:

    GazeCallbackWrapper(Middleware *mw, std::string scope, std::string name) : CallbackWrapper<hlrc_server::gazetargetAction>(mw, scope, name, boost::bind(&GazeCallbackWrapper::call, this, _1)){

        //

    };

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

    double convert_ros_to_timestamp_ms(ros::Time t){

            return t.sec + t.nsec/1000000000.0;

    }

    void call(const GoalConstPtr &goal){

        feedback.result = 1;

        hlrc_server::gazetargetGoalConstPtr request = goal;

        //printf("> incoming gaze (p=%3.1f t=%3.1f r=%3.1f / v=%3.1f)\n", request->pan, request->tilt, request->roll, request->vergence);

        humotion::GazeState gaze_state;

        //fill in gaze

        gaze_state.pan   = request->pan;

        gaze_state.tilt  = request->tilt;

        gaze_state.roll  = request->roll;

        //use timestamp from request

        gaze_state.timestamp = convert_ros_to_timestamp_ms(request->timestamp); //get_timestamp();

        if (request->type == hlrc_server::gazetargetGoal::GAZETARGET_RELATIVE){

            gaze_state.type = humotion::GazeState::RELATIVE;

        }else{

            gaze_state.type = humotion::GazeState::ABSOLUTE;

        }

        //default:

        gaze_state.vergence  = request->vergence;

        gaze_state.pan_offset = request->pan_offset;

        gaze_state.tilt_offset = request->tilt_offset;

        //processing

        feedback.result = 1;

        as_.publishFeedback(feedback);

        //send to application:

        mw->gaze_callback(gaze_state);

        if (feedback.result){

            result.result = feedback.result;

            as_.setSucceeded(result);

        }else{

            as_.setAborted(result);

        }

    }

};