commit 2cf3c285f046b19e13b492d8821c74e681177c6f
Author: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date:   Thu Jun 2 16:40:09 2016 +0200

    server: implemented lookat()

diff --git a/client/cpp/CMakeLists.txt b/client/cpp/CMakeLists.txt
index c660676..ea69fb9 100644
--- a/client/cpp/CMakeLists.txt
+++ b/client/cpp/CMakeLists.txt
@@ -125,7 +125,7 @@ SET_TARGET_PROPERTIES(${HLRC_CLIENT_LIBNAME} PROPERTIES VERSION ${HLRC_CLIENT_VE
 INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/include)
 LINK_DIRECTORIES(${LIBDIR})
 IF (ROS_FOUND)
-add_dependencies(${HLRC_CLIENT_LIBNAME} ${catkin_EXPORTED_TARGETS} hlc_server_gencpp)
+    add_dependencies(${HLRC_CLIENT_LIBNAME} ${catkin_EXPORTED_TARGETS} hlrc_server_gencpp)
 ENDIF (ROS_FOUND)
 
 target_link_libraries(${HLRC_CLIENT_LIBNAME} ${RSB_LIBRARIES} ${RST_LIBRARIES} ${catkin_LIBRARIES})
diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt
index 2b093e2..a6a5072 100644
--- a/server/CMakeLists.txt
+++ b/server/CMakeLists.txt
@@ -55,7 +55,9 @@ find_package(catkin)
 IF (catkin_FOUND)
     SET(ROS_FOUND 1)
 
-    find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs sensor_msgs message_generation genmsg actionlib_msgs actionlib)
+    find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs sensor_msgs message_generation genmsg actionlib_msgs actionlib tf2_ros tf2_eigen angles)
+    find_package(PkgConfig REQUIRED)
+    pkg_search_module(EIGEN3 REQUIRED eigen3)
 
     message(STATUS "ROS Support is ON")
     add_definitions(-DROS_SUPPORT=1)
@@ -111,7 +113,6 @@ IF (humotion_FOUND)
         message("> using humotion includes from " ${humotion_INCLUDE_DIRS})
 ELSE (humotion_FOUND)
         message(FATAL_ERROR "> error: can not find libhumotion")
-
 endif (humotion_FOUND)
 
 ###################################
@@ -127,7 +128,7 @@ ELSE (AO_FOUND)
 ENDIF (AO_FOUND)
 
 #build 
-include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${AO_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS})
+include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${AO_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS})
 link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${AO_LIBRARY_DIRS} ${humotion_LIBRARY_DIRS})
 
 #build dummy list of header files
diff --git a/server/include/MiddlewareROS.h b/server/include/MiddlewareROS.h
index 5eabc4a..3b1e1c5 100644
--- a/server/include/MiddlewareROS.h
+++ b/server/include/MiddlewareROS.h
@@ -31,6 +31,7 @@
 #ifdef ROS_SUPPORT
     #include "ros/ros.h"
     #include <actionlib/client/simple_action_client.h>
+    #include <tf2_ros/transform_listener.h>
 
     //messages
     #include "hlrc_server/phoneme.h"
@@ -81,12 +82,11 @@ private:
     //boost::shared_ptr<ros::NodeHandle> node_handle;
     ros::NodeHandle *ros_node_handle;
     bool tick_necessary;
-    /*ros::ServiceServer animation_service;
-    ros::ServiceServer utterance_service;
-    ros::ServiceServer default_emotion_service;
-    ros::ServiceServer current_emotion_service;
-    ros::ServiceServer gaze_service;
-    ros::ServiceServer tts_service;*/
+    //listen to tf tree
+    tf2_ros::Buffer tfBuffer;
+    boost::shared_ptr<tf2_ros::TransformListener> tfListener;
+
+    //FIXME: These pointers are never destroyed. Shouldn't they be shared_ptrs?
     AnimationCallbackWrapper *animation_action_server;
     UtteranceCallbackWrapper *utterance_action_server;
     EmotionCallbackWrapper *current_emotion_action_server;
@@ -98,7 +98,3 @@ private:
     actionlib::SimpleActionClient<hlrc_server::ttsAction> *tts_ac;
 #endif
 };
-
-
-
-
diff --git a/server/include/ROS/LookatCallbackWrapperROS.h b/server/include/ROS/LookatCallbackWrapperROS.h
index 5ff88e1..99ed480 100644
--- a/server/include/ROS/LookatCallbackWrapperROS.h
+++ b/server/include/ROS/LookatCallbackWrapperROS.h
@@ -29,42 +29,77 @@
 #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="BASE_LINK";
+    Eigen::Affine3d eig_base_to_eyes; // base to center of eyes
+    double eye_distance; // distance from center of eyes to eye frame
 
 public:
 
-    LookatCallbackWrapper(Middleware *mw, std::string scope, std::string name)
+    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)
     {
-        //
+        eye_distance = 0.15 / 2;
     }
 
     void call(const GoalConstPtr &goal){
         feedback.result = 1;
 
         hlrc_server::lookattargetGoalConstPtr request = goal;
+        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
         humotion::GazeState gaze_state;
+        double distance = target.norm();
+        if (distance > 1e-3) {
+            target /= distance;
 
-        // fill in lookat
-        gaze_state.pan   = 20;
-        gaze_state.tilt  = 30;
-        gaze_state.roll  = 10;
+            // 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(request->header.stamp.sec, request->header.stamp.nsec);
         gaze_state.gaze_type = humotion::GazeState::GAZETYPE_ABSOLUTE;
 
-        gaze_state.vergence  = 0;
-        gaze_state.pan_offset = 0;
-        gaze_state.tilt_offset = 0;
-
         // processing
-        feedback.result = 1;
         as_.publishFeedback(feedback);
 
         // send to application:
diff --git a/server/package.xml b/server/package.xml
index 57e1618..352511f 100644
--- a/server/package.xml
+++ b/server/package.xml
@@ -36,10 +36,15 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>actionlib_msgs</build_depend>
   <build_depend>humotion</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_eigen</build_depend>
+  <build_depend>eigen</build_depend>
+  <build_depend>angles</build_depend>
   <!-- Use run_depend for packages you need at runtime: -->
   <run_depend>std_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>actionlib_msgs</run_depend>
+  <run_depend>tf2_ros</run_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
 
diff --git a/server/src/MiddlewareROS.cpp b/server/src/MiddlewareROS.cpp
index d94d5ba..7c1ac3c 100644
--- a/server/src/MiddlewareROS.cpp
+++ b/server/src/MiddlewareROS.cpp
@@ -72,6 +72,8 @@ void MiddlewareROS::init(){
     printf("> registering on ROS as node %s\n",node_name.c_str());
     if (ros::isInitialized()){
         //ros is already active, no need to take care of that
+        //FIXME: Instead of deciding based on ros::isInitialized() to tick or not,
+        //we shoud use our own MessageQueue and AsyncSpinner.
         tick_necessary = false;
     }else{
         printf("> ROS not initialized, calling ros::init()\n");
@@ -86,17 +88,18 @@ void MiddlewareROS::init(){
     ros::NodeHandle n;
 
     printf("> setting up ROS services...\n");
-    //FIXME//init_callback("speech",    LocalServer::CallbackPtr(new SpeechCallbackWrapper(this)));
-
     //ros::AsyncSpinner *spinner = new ros::AsyncSpinner(4); // Use 4 threads
     //spinner->start();
 
+    tfListener.reset(new tf2_ros::TransformListener(tfBuffer));
+    tfBuffer.clear();
+
     animation_action_server = new AnimationCallbackWrapper(this, scope, "animation");
     utterance_action_server = new UtteranceCallbackWrapper(this, scope, "utterance");
     current_emotion_action_server = new EmotionCallbackWrapper(this, scope, "currentEmotion");
     default_emotion_action_server = new EmotionCallbackWrapper(this, scope, "defaultEmotion");
     gaze_action_server = new GazeCallbackWrapper(this, scope, "gaze");
-    lookat_action_server = new LookatCallbackWrapper(this, scope, "lookat");
+    lookat_action_server = new LookatCallbackWrapper(this, scope, "lookat", tfBuffer);
     mouth_action_server = new MouthCallbackWrapper(this, scope, "mouth");
     speech_action_server = new SpeechCallbackWrapper(this, scope, "speech");
 
@@ -106,8 +109,6 @@ void MiddlewareROS::init(){
         printf("> tts action service not available, will not do any TTS !\n");
     }
 
-
-
     printf("> init done\n");
 }
 
