commit 2aa96942ede4a1a40b446a7b36fb8e7cac169f5d
Author: sschulz <sschulz@techfak.uni-bielefeld.de>
Date:   Tue Jun 7 15:56:52 2016 +0200

    adding debugging

diff --git a/src/server/middleware_ros.cpp b/src/server/middleware_ros.cpp
index 3e312b8..a5fc261 100644
--- a/src/server/middleware_ros.cpp
+++ b/src/server/middleware_ros.cpp
@@ -27,6 +27,7 @@
 
 #include <boost/algorithm/string/classification.hpp>
 
+#include <std_msgs/Float32.h>
 #include <string>
 
 #include "humotion/server/middleware_ros.h"
@@ -35,8 +36,9 @@ using humotion::server::MiddlewareROS;
 
 //! constructor
 MiddlewareROS::MiddlewareROS(std::string scope, Controller *c)
-    : Middleware(scope, c) {
+    : Middleware(scope, c), nh_(scope + "/humotion") {
     ROS_DEBUG_STREAM_NAMED("MiddlewareROS", "will use ros middleware");
+    debug_initialized_ = false;
 
     // start ros core?
     if (ros::isInitialized()) {
@@ -58,14 +60,13 @@ MiddlewareROS::MiddlewareROS(std::string scope, Controller *c)
     }
 
     // create node handle
-    ros::NodeHandle n;
     ros::NodeHandle pnh("~");
 
     // set up subscribers
-    mouth_target_subscriber_ = n.subscribe(scope + "/humotion/mouth/target", 150,
+    mouth_target_subscriber_ = nh_.subscribe("mouth/target", 150,
                                           &MiddlewareROS::incoming_mouth_target ,
                                           this, ros::TransportHints().unreliable());
-    gaze_target_subscriber_  = n.subscribe(scope + "/humotion/gaze/target",  150,
+    gaze_target_subscriber_  = nh_.subscribe("gaze/target",  150,
                                           &MiddlewareROS::incoming_gaze_target,
                                           this, ros::TransportHints().unreliable());
 
@@ -79,12 +80,16 @@ void MiddlewareROS::dynamic_reconfigure_callback(const humotion::humotionConfig
     // fetch config
     humotion::server::Config *config = controller_->get_config();
 
+    // debug
+    config->publish_internals = dyn_config.publish_internals;
+
     // saccade detection thresholds
     config->threshold_velocity_eye_saccade = dyn_config.threshold_velocity_eye_saccade;
     config->threshold_angle_neck_saccade = dyn_config.threshold_angle_neck_saccade;
     config->threshold_angle_omr_limit = dyn_config.threshold_angle_omr_limit;
 
-    config->use_neck_target_instead_of_position_eye = dyn_config.use_neck_target_instead_of_position_eye;
+    config->use_neck_target_instead_of_position_eye =
+            dyn_config.use_neck_target_instead_of_position_eye;
 
     // neck motion generation configuration
     config->scale_velocity_neck = dyn_config.scale_velocity_neck;
@@ -141,6 +146,53 @@ void MiddlewareROS::tick() {
     }
 }
 
+//! init debug dataset publishers
+void MiddlewareROS::debug_initialize() {
+    if (debug_initialized_) {
+        return;
+    }
+
+    debug_initialized_ = true;
+}
+
+//! publish a debug dataset
+void MiddlewareROS::publish_debug_dataset(debug_data_t debug_data) {
+    if (!controller_->get_config()->publish_internals) {
+        // no debugging data enabled, return
+        return;
+    }
+
+    debug_initialize();
+
+    // iterate over all debug variables and publish them
+    debug_data_t::iterator it;
+    for (it = debug_data.begin(); it != debug_data.end(); it++) {
+        publish_debug_data(it->first, it->second);
+    }
+}
+
+void MiddlewareROS::publish_debug_data(std::string name, float value) {
+    debug_topic_map_t::iterator it = debug_topic_map.find(name);
+    if (it == debug_topic_map.end()) {
+        // we have no publisher for this dataset, create one:
+        ROS_DEBUG_STREAM("creating debug output stream " << name);
+        ros::Publisher pub =  nh_.advertise<std_msgs::Float32>("debug_data/" + name, 1000);
+
+        std::pair<debug_topic_map_t::iterator, bool> ret;
+        ret = debug_topic_map.insert(std::pair<std::string, ros::Publisher>(name, pub));
+        it = ret.first;
+    }
+
+    // in any case, it is now the publisher we want  so publish data now
+    std_msgs::Float32 msg;
+    msg.data = value;
+
+    // std::cout  << "DEBUG HUMOTION " << name << " " << value << std::endl;
+
+    // send it
+    it->second.publish(msg);
+}
+
 //! callback to handle incoming mouth  target
 void MiddlewareROS::incoming_mouth_target(const humotion::mouth::ConstPtr& msg) {
     // printf("> incoming mouth_target\n");
