commit f0dcf4ca8de3b7324986e739257f63cd092f6e3e
Author: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date:   Sat Jun 4 01:47:44 2016 +0200

    cpp client: lookat() implementation

diff --git a/client/CMakeLists.txt b/client/CMakeLists.txt
index 1f82af1..80d5952 100644
--- a/client/CMakeLists.txt
+++ b/client/CMakeLists.txt
@@ -5,5 +5,3 @@ SET(CMAKE_BUILD_TYPE distribution)
 
 add_subdirectory(cpp)
 add_subdirectory(python)
-
-
diff --git a/client/cpp/include/Middleware.h b/client/cpp/include/Middleware.h
index db7d119..84f344d 100644
--- a/client/cpp/include/Middleware.h
+++ b/client/cpp/include/Middleware.h
@@ -46,6 +46,8 @@ public:
     void set_default_emotion(RobotEmotion e, bool blocking);
     void set_current_emotion(RobotEmotion e, bool blocking);
     void set_gaze_target(RobotGaze target, bool blocking);
+    void set_lookat_target(float x, float y, float z, const std::string frame_id,
+                           bool blocking, float roll);
     void set_mouth_target(RobotMouth target, bool blocking);
     void set_head_animation(RobotHeadAnimation a, bool blocking);
     void set_speak(std::string text, bool blocking);
@@ -59,6 +61,8 @@ protected:
     virtual void publish_default_emotion(RobotEmotion e, bool blocking) = 0;
     virtual void publish_current_emotion(RobotEmotion e, bool blocking) = 0;
     virtual void publish_gaze_target(RobotGaze g, bool blocking) = 0;
+    virtual void publish_lookat_target(float x, float y, float z, const std::string frame_id,
+                                       bool blocking, float roll) = 0;
     virtual void publish_mouth_target(RobotMouth m, bool blocking) = 0;
     virtual void publish_head_animation(RobotHeadAnimation a, bool blocking) = 0;
     virtual void publish_speech(std::string text, bool blocking) = 0;
diff --git a/client/cpp/include/MiddlewareROS.h b/client/cpp/include/MiddlewareROS.h
index 901d74c..f726354 100644
--- a/client/cpp/include/MiddlewareROS.h
+++ b/client/cpp/include/MiddlewareROS.h
@@ -35,6 +35,7 @@
     #include <hlrc_server/phoneme.h>
     #include <hlrc_server/soundchunk.h>
     #include <hlrc_server/gazetargetAction.h>
+    #include <hlrc_server/lookattargetAction.h>
     #include <hlrc_server/animationAction.h>
     #include <hlrc_server/mouthtargetAction.h>
     #include <hlrc_server/utteranceAction.h>
@@ -50,7 +51,7 @@
 class MiddlewareROS : public Middleware{
 #ifndef ROS_SUPPORT
 public:
-    MiddlewareRSB(std::string scope) : Middleware(scope){
+    MiddlewareROS(std::string scope) : Middleware(scope){
         printf("> ERROR: hlrc was compiled without ROS middleware support. Please use MiddlewareRSB() instead!\n\n");
         exit(EXIT_FAILURE);
     }
@@ -59,12 +60,12 @@ public:
     void publish_default_emotion(RobotEmotion e, bool blocking){};
     void publish_current_emotion(RobotEmotion e, bool blocking){};
     void publish_gaze_target(RobotGaze g, bool blocking){};
+    void publish_lookat_target(float x, float y, float z, const std::string frame_id,
+                               bool blocking, float roll){};
     void publish_mouth_target(RobotMouth m, bool blocking){};
     void publish_head_animation(RobotHeadAnimation a, bool blocking) {};
     void publish_speech(std::string text, bool blocking) {};
 
-
-
 #else
 public:
     MiddlewareROS(std::string scope);
@@ -74,6 +75,8 @@ protected:
     void publish_current_emotion(RobotEmotion e, bool blocking);
     void publish_default_emotion(RobotEmotion e, bool blocking);
     void publish_gaze_target(RobotGaze target, bool blocking);
+    void publish_lookat_target(float x, float y, float z, const std::string frame_id,
+                               bool blocking, float roll);
     void publish_mouth_target(RobotMouth target, bool blocking);
     void publish_head_animation(RobotHeadAnimation a, bool blocking);
     void publish_speech(std::string text, bool blocking);
@@ -87,7 +90,8 @@ private:
     actionlib::SimpleActionClient<hlrc_server::emotionstateAction>  *default_emotionstate_ac;
     actionlib::SimpleActionClient<hlrc_server::emotionstateAction>  *current_emotionstate_ac;
     actionlib::SimpleActionClient<hlrc_server::gazetargetAction>    *gazetarget_ac;
-    actionlib::SimpleActionClient<hlrc_server::mouthtargetAction>    *mouthtarget_ac;
-    actionlib::SimpleActionClient<hlrc_server::speechAction>     *speech_ac;
+    actionlib::SimpleActionClient<hlrc_server::lookattargetAction>  *lookattarget_ac;
+    actionlib::SimpleActionClient<hlrc_server::mouthtargetAction>   *mouthtarget_ac;
+    actionlib::SimpleActionClient<hlrc_server::speechAction>        *speech_ac;
 #endif
 };
diff --git a/client/cpp/include/MiddlewareRSB.h b/client/cpp/include/MiddlewareRSB.h
index e05d23f..a742f13 100644
--- a/client/cpp/include/MiddlewareRSB.h
+++ b/client/cpp/include/MiddlewareRSB.h
@@ -51,6 +51,8 @@ public:
     void publish_default_emotion(RobotEmotion e, bool blocking){};
     void publish_current_emotion(RobotEmotion e, bool blocking){};
     void publish_gaze_target(RobotGaze g, bool blocking){};
+    void publish_lookat_target(float x, float y, float z, const std::string frame_id,
+                               bool blocking, float roll){};
     void publish_mouth_target(RobotMouth m, bool blocking){};
     void publish_head_animation(RobotHeadAnimation a, bool blocking) {};
     void publish_speech(std::string text, bool blocking) {};
@@ -66,6 +68,8 @@ protected:
     void publish_current_emotion(RobotEmotion e, bool blocking);
     void publish_default_emotion(RobotEmotion e, bool blocking);
     void publish_gaze_target(RobotGaze target, bool blocking);
+    void publish_lookat_target(float x, float y, float z, const std::string frame_id,
+                               bool blocking, float roll);
     void publish_mouth_target(RobotMouth target, bool blocking);
     void publish_head_animation(RobotHeadAnimation a, bool blocking);
     void publish_speech(std::string text, bool blocking);
diff --git a/client/cpp/include/RobotController.h b/client/cpp/include/RobotController.h
index 43c0af2..552ff32 100644
--- a/client/cpp/include/RobotController.h
+++ b/client/cpp/include/RobotController.h
@@ -44,6 +44,8 @@ public:
 
     void set_gaze_target(RobotGaze target, bool blocking=false);
     RobotGaze get_gaze_target();
+    void set_lookat_target(float x, float y, float z, const std::string frame_id="",
+                           bool blocking=false, float roll=0.0);
 
     void set_mouth_target(RobotMouth target, bool blocking=false);
 
diff --git a/client/cpp/src/Middleware.cpp b/client/cpp/src/Middleware.cpp
index ccbb7f9..b27a269 100644
--- a/client/cpp/src/Middleware.cpp
+++ b/client/cpp/src/Middleware.cpp
@@ -58,6 +58,11 @@ void Middleware::set_gaze_target(RobotGaze target, bool blocking){
     publish_gaze_target(gazeTarget, blocking);
 }
 
+void Middleware::set_lookat_target(float x, float y, float z, const string frame_id, bool blocking, float roll)
+{
+    publish_lookat_target(x,y,z, frame_id, blocking, roll);
+}
+
 void Middleware::set_mouth_target(RobotMouth target, bool blocking){
     mouthTarget = target;
     publish_mouth_target(mouthTarget, blocking);
diff --git a/client/cpp/src/MiddlewareROS.cpp b/client/cpp/src/MiddlewareROS.cpp
index 8af1fa5..aad481e 100644
--- a/client/cpp/src/MiddlewareROS.cpp
+++ b/client/cpp/src/MiddlewareROS.cpp
@@ -78,6 +78,7 @@ void MiddlewareROS::init(void){
     current_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/currentEmotion");
     default_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/defaultEmotion");
     gazetarget_ac = create_action_client<hlrc_server::gazetargetAction>(scope + "/gaze");
+    lookattarget_ac = create_action_client<hlrc_server::lookattargetAction>(scope + "/lookat");
     mouthtarget_ac = create_action_client<hlrc_server::mouthtargetAction>(scope + "/mouth");
     speech_ac = create_action_client<hlrc_server::speechAction>(scope + "/speech");
 
@@ -173,6 +174,29 @@ void MiddlewareROS::publish_gaze_target(RobotGaze target, bool blocking){
     }
 }
 
+void MiddlewareROS::publish_lookat_target(float x, float y, float z, const string frame_id, bool blocking, float roll)
+{
+    hlrc_server::lookattargetGoal goal;
+    goal.point.header.frame_id = frame_id;
+    goal.point.header.stamp = ros::Time::now();
+
+    geometry_msgs::Point &p = goal.point.point;
+    p.x = x;
+    p.y = y;
+    p.z = z;
+    goal.roll = roll;
+
+    //send
+    lookattarget_ac->sendGoal(goal);
+
+    if (blocking){
+        bool finished_before_timeout = gazetarget_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
+        if (!finished_before_timeout){
+            printf("> ERROR: blocking call timed out!\n");
+        }
+    }
+}
+
 
 void MiddlewareROS::publish_mouth_target(RobotMouth target, bool blocking){
     hlrc_server::mouthtargetGoal goal;
diff --git a/client/cpp/src/MiddlewareRSB.cpp b/client/cpp/src/MiddlewareRSB.cpp
index 17fe017..23cc5b3 100644
--- a/client/cpp/src/MiddlewareRSB.cpp
+++ b/client/cpp/src/MiddlewareRSB.cpp
@@ -156,6 +156,11 @@ void MiddlewareRSB::publish_gaze_target(RobotGaze incoming_target, bool blocking
     }
 }
 
+void MiddlewareRSB::publish_lookat_target(float x, float y, float z, const std::string frame_id,
+                                          bool blocking, float roll){
+    std::cerr << "not yet implemented" << std::endl;
+}
+
 void MiddlewareRSB::publish_mouth_target(RobotMouth target, bool blocking){
 /*
     boost::shared_ptr<rst::robot::MouthTarget> request(new rst::robot::MouthTarget());
diff --git a/client/cpp/src/RobotController.cpp b/client/cpp/src/RobotController.cpp
index fa60d35..0d81f1c 100644
--- a/client/cpp/src/RobotController.cpp
+++ b/client/cpp/src/RobotController.cpp
@@ -64,6 +64,11 @@ RobotGaze RobotController::get_gaze_target(){
     return middleware->get_gaze_target();
 }
 
+void RobotController::set_lookat_target(float x, float y, float z, const std::string frame_id,
+                                        bool blocking, float roll){
+    middleware->set_lookat_target(x, y, z, frame_id, blocking, roll);
+}
+
 void RobotController::set_mouth_target(RobotMouth target, bool blocking){
     middleware->set_mouth_target(target, blocking);
 }
diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt
index a6a5072..b19b433 100644
--- a/server/CMakeLists.txt
+++ b/server/CMakeLists.txt
@@ -96,7 +96,7 @@ IF (catkin_FOUND)
 
     catkin_package(
         INCLUDE_DIRS include
-        LIBRARIES hlrc_server
+        LIBRARIES
         CATKIN_DEPENDS actionlib_msgs
         #CATKIN_DEPENDS message_runtime
         #DEPENDS system_lib
