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

#include "CallbackWrapperROS.h"

#include <tf2_ros/buffer.h>

#include <tf2_eigen/tf2_eigen.h>

#include <angles/angles.h>

//callback handler incoming lookat requests:

class LookatCallbackWrapper : CallbackWrapper<hlrc_server::lookattargetAction>{

protected:

    hlrc_server::lookattargetFeedback feedback;

    hlrc_server::lookattargetResult result;

    const tf2_ros::Buffer& tfBuffer;

    const std::string FLOBI_BASE_LINK="floka_BASE_LINK";

    // pan/tilt will be computed w.r.t. to virtual frame at height of eyes

    Eigen::Affine3d eig_eyes_to_base;

    double eye_distance; // distance from center of eyes to eye frame

public:

    LookatCallbackWrapper(Middleware *mw, std::string scope, std::string name, const tf2_ros::Buffer &tfBuffer)

       : CallbackWrapper<hlrc_server::lookattargetAction>(mw, scope, name, boost::bind(&LookatCallbackWrapper::call, this, _1))

       , tfBuffer(tfBuffer)

    {

        // eyes frame has z-axis pointing along view dir (90° rotation w.r.t. base)

        // and z-position is shifted to height of eyes

        eig_eyes_to_base = (Eigen::Translation3d(0.078, 0, 0.188)

                * Eigen::AngleAxisd(angles::from_degrees(90), Eigen::Vector3d::UnitY())

                * Eigen::AngleAxisd(angles::from_degrees(180), Eigen::Vector3d::UnitZ())).inverse();

        //eig_eyes_to_base = Eigen::Translation3d(0.070621, 0, 0.2006).inverse();

        //std::cout << "MATRIX" << eig_eyes_to_base << std::endl;

                //* Eigen::AngleAxisd(angles::from_degrees(90), Eigen::Vector3d::UnitY())

                //* Eigen::AngleAxisd(angles::from_degrees(180), Eigen::Vector3d::UnitZ())).inverse();

        eye_distance = 0.069;

    }

    void call(const GoalConstPtr &goal){

        feedback.result = 1;

        hlrc_server::lookattargetGoalConstPtr request = goal;

        const geometry_msgs::PointStamped &p = request->point;

        Eigen::Vector3d target(p.point.x, p.point.y, p.point.z);

        try {

            if (!p.header.frame_id.empty()) { // only consider tf, when frame_id is non-empty

                geometry_msgs::TransformStamped base_to_source =

                    // lookupTransform can throw

                    tfBuffer.lookupTransform(FLOBI_BASE_LINK, p.header.frame_id,

                                             p.header.stamp, ros::Duration(0.1));

                target = eig_eyes_to_base * (tf2::transformToEigen(base_to_source) * target);                        

            } else {

                // otherwise we interpret target directly w.r.t. eye center

                ROS_WARN("NO Frame provided with target!");

            }

        } catch (const std::exception &e) {

            // Failed to resolve tf

            ROS_WARN_STREAM_THROTTLE(2, e.what());

            result.result = 0;

            as_.setAborted(result, e.what());

            return;

        }

        // compute pan + tilt: let point z-axis of eyes frames towards target

        // alternatively, compute inverse kinematics of pan-tilt chain

        // as here: https://github.com/pal-robotics/head_action

        humotion::GazeState gaze_state;

        double distance = target.norm();

        if (distance > 1e-3) {

            // normalize vector from eyes-center to target

            target /= distance;

            // pan = rotation about x-axis: angle of projection of target onto yz-plane to z-axis

            gaze_state.pan = (90 - angles::to_degrees(atan2(target.z(), target.y())));

            // tilt = rotation about y-axis: angle between target and yz-plane

            Eigen::Vector2d projection(target.y(), target.z());

            gaze_state.tilt = angles::to_degrees(atan2(target.x(), projection.norm()));

        }

        // fill in rest of gaze_state

        gaze_state.roll  = request->roll;

        gaze_state.vergence  = -angles::to_degrees(atan2(eye_distance, distance));

        // use timestamp from request

        gaze_state.timestamp.set(p.header.stamp.sec, p.header.stamp.nsec);

        gaze_state.gaze_type = humotion::GazeState::GAZETYPE_ABSOLUTE;

        // processing

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

        }

    }

};