hlrc

#include <tf2_ros/buffer.h>

#include <tf2_eigen/tf2_eigen.h>

#include <angles/angles.h>

    const tf2_ros::Buffer& tfBuffer;

    const std::string FLOBI_BASE_LINK="BASE_LINK";

    Eigen::Affine3d eig_base_to_eyes; // base to center of eyes

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

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

       , tfBuffer(tfBuffer)

        eye_distance = 0.15 / 2;

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

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

        try {

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

                geometry_msgs::TransformStamped source_to_base =

                    // lookupTransform can throw

                    tfBuffer.lookupTransform(FLOBI_BASE_LINK, request->header.frame_id,

                                             request->header.stamp, ros::Duration(0.01));

                Eigen::Affine3d eig_source_to_eyes;

                eig_source_to_eyes = tf2::transformToEigen(source_to_base) * eig_base_to_eyes;

                target = eig_source_to_eyes * target;

            } else {

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

            }

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

            // Failed to resolve tf?

            result.result = 0;

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

            return;

        }

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

        double distance = target.norm();

        if (distance > 1e-3) {

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