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 = "head_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.0, 0.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);

                }

        }

};