commit 0c8d22a524bacd2a0469cce769fa47d6c6f05a27
Author: sschulz <sschulz@techfak.uni-bielefeld.de>
Date:   Fri Feb 19 14:54:11 2016 +0100

    stylecheck based cleanup

diff --git a/src/server/controller.cpp b/src/server/controller.cpp
index c45a860..3cb1e23 100644
--- a/src/server/controller.cpp
+++ b/src/server/controller.cpp
@@ -25,17 +25,19 @@
 * Excellence Initiative.
 */
 
-#include "server/controller.h"
-#include "server/eye_motion_generator.h"
-#include "server/eyelid_motion_generator.h"
-#include "server/eyebrow_motion_generator.h"
-#include "server/neck_motion_generator.h"
-#include "server/mouth_motion_generator.h"
-#include "timestamp.h"
-
-using namespace std;
-using namespace humotion;
-using namespace humotion::server;
+#include "humotion/server/controller.h"
+#include "humotion/server/eye_motion_generator.h"
+#include "humotion/server/eyebrow_motion_generator.h"
+#include "humotion/server/eyelid_motion_generator.h"
+#include "humotion/server/mouth_motion_generator.h"
+#include "humotion/server/neck_motion_generator.h"
+#include "humotion/timestamp.h"
+
+// using namespace std;
+// using namespace humotion;
+// using namespace humotion::server;
+
+using humotion::server::Controller;
 
 //! constructor
 Controller::Controller(JointInterface *j) : activated(false) {
@@ -43,91 +45,104 @@ Controller::Controller(JointInterface *j) : activated(false) {
 }
 
 //! destructor
-Controller::~Controller(){
+Controller::~Controller() {
 }
 
 //! initialise motion generators
-void Controller::init_motion_generators(){
-    //NOTE: the order of these generators is important!
+void Controller::init_motion_generators() {
+    // NOTE: the order of these generators is important!
     //       (i.e. the neck generator must be added after the eye generator!)
 
-    //1) eye motion generation:
+    // eye motion generation:
     add_motion_generator(new EyeMotionGenerator(joint_interface));
 
-    //2) eyelid motion generator
+    // eyelid motion generator
     add_motion_generator(new EyelidMotionGenerator(joint_interface));
 
-    //3) neck motion generator
+    // neck motion generator
     add_motion_generator(new NeckMotionGenerator(joint_interface));
 
-    //4) mouth motion generator
+    // mouth motion generator
     add_motion_generator(new MouthMotionGenerator(joint_interface));
 
-    //5) eyebrow motion generator
+    // eyebrow motion generator
     add_motion_generator(new EyebrowMotionGenerator(joint_interface));
 }
 
 //! add a single motion genrator
-void Controller::add_motion_generator(MotionGenerator *m){
+void Controller::add_motion_generator(MotionGenerator *m) {
     motion_generator_vector.push_back(m);
 }
 
 //! calculate target angles for all motion generators:
-void Controller::calculate_targets(){
-    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){
+void Controller::calculate_targets() {
+    Controller::motion_generator_vector_t::iterator it;
+    for (it = motion_generator_vector.begin(); it < motion_generator_vector.end(); it++) {
         (*it)->calculate_targets();
     }
 }
 
 //! publish all target angles to the devices:
 //! NOTE: this is done in an extra loop to have a low delay between consequent sets:
-void Controller::publish_targets(){
-    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){
+void Controller::publish_targets() {
+    Controller::motion_generator_vector_t::iterator it;
+    for (it = motion_generator_vector.begin(); it < motion_generator_vector.end(); it++) {
         (*it)->publish_targets();
     }
 }
 
-GazeState Controller::relative_gaze_to_absolute_gaze(GazeState relative){
+humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeState relative) {
     double pan, tilt, roll;
-    GazeState absolute_gaze = relative;
+    humotion::GazeState absolute_gaze = relative;
 
-    //incoming gaze state wants to set a relative gaze angle
-    //in order to calc the new absolute gaze, we need to go back
-    //in time and find out where the head was pointing at that specific time:
+    // incoming gaze state wants to set a relative gaze angle
+    // in order to calc the new absolute gaze, we need to go back
+    // in time and find out where the head was pointing at that specific time:
     Timestamp relative_target_timestamp = relative.timestamp;
 
-    //check if this timestamp allows a valid conversion:
-    Timestamp history_begin = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();
-    // Timestamp history_end   = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();
-
-    //printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(), history_begin.to_seconds(), history_end.to_seconds());
-
-    //our history keeps the last n=100 elements in a timestamped list
-    if ((relative_target_timestamp < history_begin) || (history_begin.is_null())){
-        //when the incoming data is older than that it makes no sense
-        //to do any guesswork and try to calculate a valid absolute target
-        //therefore we will use the last known targets (see below)
-        //in case we did not see this timestamp before, show a warning:
-        if (last_known_absolute_timestamp != relative_target_timestamp){
-            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f [this should not happen]\n", relative_target_timestamp.to_seconds());
+    // check if this timestamp allows a valid conversion:
+    Timestamp history_begin =
+            joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();
+    // Timestamp history_end   =
+    //       joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();
+
+    // printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(),
+    // history_begin.to_seconds(), history_end.to_seconds());
+
+    // our history keeps the last n elements in a timestamped list
+    if ((relative_target_timestamp < history_begin) || (history_begin.is_null())) {
+        // when the incoming data is older than that it makes no sense
+        // to do any guesswork and try to calculate a valid absolute target
+        // therefore we will use the last known targets (see below)
+        // in case we did not see this timestamp before, show a warning:
+        if (last_known_absolute_timestamp != relative_target_timestamp) {
+            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f "
+                   "[this should not happen]\n", relative_target_timestamp.to_seconds());
             last_known_absolute_target_pan = 0.0;
             last_known_absolute_target_tilt = 0.0;
             last_known_absolute_target_roll = 0.0;
         }
-    }else{
-        //all fine, we can reconstruct the absolute target:
-        //fetch head / camera pose during that timestamp:
-        double neck_pan  = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);
-        double eye_l_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);
-        double eye_r_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);
+    } else {
+        // all fine, we can reconstruct the absolute target
+        // fetch head / camera pose during that timestamp
+        double neck_pan  = joint_interface->get_ts_position(
+                JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);
+        double eye_l_pan = joint_interface->get_ts_position(
+                JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);
+        double eye_r_pan = joint_interface->get_ts_position(
+                JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);
         last_known_absolute_target_pan       = neck_pan + (eye_l_pan + eye_r_pan)/2.0;
         //
-        double neck_tilt = joint_interface->get_ts_position(JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);
-        double eye_tilt  = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);
+        double neck_tilt = joint_interface->get_ts_position(
+                JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);
+        double eye_tilt  = joint_interface->get_ts_position(
+                JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);
         last_known_absolute_target_tilt      = neck_tilt + eye_tilt;
         //
-        last_known_absolute_target_roll      = joint_interface->get_ts_position(JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);
-        //safe this timestamp as known:
+        last_known_absolute_target_roll      = joint_interface->get_ts_position(
+                JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);
+
+        // safe this timestamp as known:
         last_known_absolute_timestamp = relative_target_timestamp;
     }
 
@@ -135,70 +150,71 @@ GazeState Controller::relative_gaze_to_absolute_gaze(GazeState relative){
     tilt = last_known_absolute_target_tilt;
     roll = last_known_absolute_target_roll;
 
-    //substract offsets:
+    // substract offsets
     pan  -= relative.pan_offset;
     tilt -= relative.tilt_offset;
     roll -= relative.roll_offset;
 
 
-    //build up absolute target:
+    // build up absolute target
     absolute_gaze.gaze_type  = GazeState::GAZETYPE_ABSOLUTE;
     absolute_gaze.pan   = pan + relative.pan;
     absolute_gaze.tilt  = tilt + relative.tilt;
     absolute_gaze.roll  = roll + relative.roll;
-    printf("pan  now = %4.1f, rel=%4.1f ===> %4.2f\n",pan,relative.pan,absolute_gaze.pan);
-    printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n",tilt,relative.tilt,absolute_gaze.tilt);
+    printf("pan  now = %4.1f, rel=%4.1f ===> %4.2f\n", pan, relative.pan, absolute_gaze.pan);
+    printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n", tilt, relative.tilt, absolute_gaze.tilt);
 
-    //FIXME: use ros TF for that calculation...
-    //see http://wiki.ros.org/tf/Tutorials/Time%20travel%20with%20tf%20%28C%2B%2B%29
-    //ros::Time past = now - ros::Duration(5.0);
-    //listener.waitForTransform("/turtle2", now,J "/turtle1", past, "/world", ros::Duration(1.0));
-    //listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);
-    //absolute_gaze.dump();
+    // FIXME: use ros TF for that calculation...
+    // see http://wiki.ros.org/tf/Tutorials/Time%20travel%20with%20tf%20%28C%2B%2B%29
+    // ros::Time past = now - ros::Duration(5.0);
+    // listener.waitForTransform("/turtle2", now,J "/turtle1", past, "/world", ros::Duration(1.0));
+    // listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);
+    // absolute_gaze.dump();
 
     return absolute_gaze;
 }
 
 //! activate controller
-void Controller::set_activated(void){
+void Controller::set_activated(void) {
     activated = true;
 }
 
 //! update gaze target:
 //! \param GazeState with target values for the overall gaze
-void Controller::set_gaze_target(GazeState new_gaze_target){
-    if (!activated){
-        //not yet initialized, ignore incoming targets
+void Controller::set_gaze_target(humotion::GazeState new_gaze_target) {
+    if (!activated) {
+        // not yet initialized, ignore incoming targets
         return;
     }
 
-    GazeState target_gaze;
-    //new_gaze_target.dump();
+    humotion::GazeState target_gaze;
+    // new_gaze_target.dump();
 
-    //relative or absolute gaze update?
-    if (new_gaze_target.gaze_type == GazeState::GAZETYPE_RELATIVE){
-        //relative gaze target -> calculate target angles
+    // relative or absolute gaze update?
+    if (new_gaze_target.gaze_type == GazeState::GAZETYPE_RELATIVE) {
+        // relative gaze target -> calculate target angles
         target_gaze = relative_gaze_to_absolute_gaze(new_gaze_target);
-    }else{
-        //already absolute gaze, set this
+    } else {
+        // already absolute gaze, set this
         target_gaze = new_gaze_target;
     }
 
-
-    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){
+    Controller::motion_generator_vector_t::iterator it;
+    for (it = motion_generator_vector.begin(); it < motion_generator_vector.end(); it++) {
         (*it)->set_gaze_target(target_gaze);
     }
 }
 
 //! update mouth state:
 //! \param MouthState with target values for the mouth joints
-void Controller::set_mouth_target(MouthState s){
-    if (!activated){
-        //not yet initialized, ignore incoming targets
+void Controller::set_mouth_target(MouthState s) {
+    if (!activated) {
+        // not yet initialized, ignore incoming targets
         return;
     }
 
-    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){
+    Controller::motion_generator_vector_t::iterator it;
+    for (it = motion_generator_vector.begin(); it < motion_generator_vector.end(); it++) {
         (*it)->set_mouth_target(s);
     }
 }
