/ - Diff - humotion - Research for Cognitive Interaction

     gen = ParameterGenerator()
     debug_group = gen.add_group("debug")
     add_entry_bool(debug_group, "publish_internals", "publish debugging data on different topics")
     general_group = gen.add_group("thresholds")
     add_entry(general_group, "threshold_velocity_eye_saccade", "velocity threshold for eye saccade detection (in deg/s)", 1.0, 30.0)
     add_entry(general_group, "threshold_angle_neck_saccade", "magnitude of gaze change that triggers neck saccade (in deg)", 1.0, 30.0)

     #ifndef INCLUDE_HUMOTION_SERVER_CONFIG_H_
     #define INCLUDE_HUMOTION_SERVER_CONFIG_H_
     #include <map>
     #include <string>
     namespace humotion {
     namespace server {
     typedef std::map<std::string, float> debug_data_t;
     class Config {
      public:
         Config();
-...
         // NOTE: See config.cpp for explanations and default values
         // ********************************************************
         // publish debug information
         bool publish_internals;
         // saccade detection thresholds
         float threshold_velocity_eye_saccade;
         float threshold_angle_neck_saccade;

include/humotion/server/controller.h
48	48	void init_motion_generators();
49	49	void calculate_targets();
50	50	void publish_targets();
	51	debug_data_t get_debug_data();
51	52
52	53	void set_gaze_target(GazeState s);
53	54	void set_mouth_target(MouthState s);

     #include <cstdint>
     #include <cstdio>
     #include <map>
     #include <string>
     #include <utility>
     #include "humotion/gaze_state.h"
     #include "humotion/mouth_state.h"
-...
         virtual bool ok() = 0;
         virtual void tick() = 0;
         virtual void publish_debug_dataset(debug_data_t) = 0;
      protected:
         Controller *controller_;
         std::string base_scope_;

     #include <dynamic_reconfigure/server.h>
     #include <ros/ros.h>
     #include <map>
     #include <string>
     #include <utility>
     #include "humotion/gaze.h"
     #include "humotion/humotionConfig.h"
-...
         void tick();
         void dynamic_reconfigure_callback(const humotion::humotionConfig &config, uint32_t level);
         void publish_debug_dataset(debug_data_t);
      private:
         void publish_debug_data(std::string name, float value);
         void incoming_mouth_target(const humotion::mouth::ConstPtr& msg);
         void incoming_gaze_target(const humotion::gaze::ConstPtr& msg);
         void attach_to_reconfiguration_server(ros::NodeHandle priv_nodehandle);
         void debug_initialize();
         bool tick_necessary_;
         bool debug_initialized_;
         ros::Subscriber mouth_target_subscriber_;
         ros::Subscriber gaze_target_subscriber_;
         ros::NodeHandle nh_;
         typedef std::map<std::string, ros::Publisher> debug_topic_map_t;
         debug_topic_map_t debug_topic_map;
         dynamic_reconfigure::Server<humotion::humotionConfig> *reconf_server_;
     };

     #include <boost/date_time/posix_time/posix_time.hpp>
     #include <map>
     #include <string>
     #include "humotion/server/config.h"
-...
         virtual void set_gaze_target(GazeState s);
         virtual void set_mouth_target(MouthState s);
         debug_data_t get_debug_data();
      protected:
         Config *config;
         debug_data_t debug_data;
         void store_debug_data(std::string name, float value);
         float get_current_position(int joint_id);
         float get_current_speed(int joint_id);
         humotion::Timestamp get_timestamped_state(int joint_id, float *position, float *velocity);
         float limit_target(int joint_id, float val);
         bool mouth_target_input_active();
         bool gaze_target_input_active();

+    }
     void Config::init_defaults() {
         // publish internal debug data as topics?
         publish_internals = false;
         // saccade detection thresholds:
         // 1) velocity threshold, values above this value trigger an eye saccade
         //    value is given in deg/s

     using humotion::server::Controller;
     using humotion::server::Config;
     using humotion::server::debug_data_t;
     //! constructor
     Controller::Controller(JointInterface *j) {
-...
     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();
             MotionGenerator *mg = *it;
             // calculate targets
             mg->calculate_targets();
+        }
+    }
     debug_data_t Controller::get_debug_data() {
         debug_data_t debug_data;
         Controller::motion_generator_vector_t::iterator it;
         for (it = motion_generator_vector_.begin(); it < motion_generator_vector_.end(); it++) {
             MotionGenerator *mg = *it;
             // fetch and append debug data
             debug_data_t dataset = mg->get_debug_data();
             debug_data.insert(dataset.begin(), dataset.end());
+        }
         return debug_data;
+    }
     //! 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() {

         joint_interface_->set_target(JointInterface::ID_EYES_BOTH_UD,
                                     reflexxes_position_output->NewPositionVector->VecData[2],
                                     reflexxes_position_output->NewVelocityVector->VecData[2]);
         // store debug data:
         store_debug_data("eye/neck_pan_now", neck_pan_now);
         store_debug_data("eye/neck_tilt_now", neck_tilt_now);
         store_debug_data("eye/eye_pan_r_target", eye_pan_r_target);
         store_debug_data("eye/eye_pan_l_target", eye_pan_l_target);
         store_debug_data("eye/eye_tilt_target", eye_tilt_target);
         store_debug_data("eye/gaze_target_pan", requested_gaze_state_.pan);
         store_debug_data("eye/gaze_target_tilt", requested_gaze_state_.tilt);
         store_debug_data("eye/eye_pan_r_now", eye_pan_r_now);
         store_debug_data("eye/eye_pan_l_now", eye_pan_l_now);
         store_debug_data("eye/eye_tilt_now", eye_tilt_now);
         store_debug_data("eye/output_l_lr", reflexxes_position_output->NewPositionVector->VecData[0]);
         store_debug_data("eye/output_r_lr", reflexxes_position_output->NewPositionVector->VecData[1]);
         store_debug_data("eye/output_ud",   reflexxes_position_output->NewPositionVector->VecData[2]);
+    }

     #include <boost/algorithm/string/classification.hpp>
     #include <std_msgs/Float32.h>
     #include <string>
     #include "humotion/server/middleware_ros.h"
-...
     //! 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()) {
-...
+        }
         // 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());
-...
         // 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;
-...
+        }
+    }
     //! 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");

     */
     #include <boost/date_time/posix_time/posix_time.hpp>
     #include <utility>
     #include "humotion/server/motion_generator.h"
     using humotion::server::MotionGenerator;
     using humotion::server::Config;
     using humotion::server::debug_data_t;
     //! constructor
     MotionGenerator::MotionGenerator(JointInterface *j, Config *cfg) {
-...
     MotionGenerator::~MotionGenerator() {
+    }
     //! return all accumulated debug data
     debug_data_t MotionGenerator::get_debug_data() {
         return debug_data;
+    }
     //! store debug data
     void MotionGenerator::store_debug_data(std::string name, float value) {
         debug_data[name] = value;
+    }
     //! fetch the latest position and velocity and return the timestamp of that dataset
     //! \param joint id
     //! \param pointer to position variable
     //! \param pointer to veolocity variable
     //! \param pointer to velocity variable
     //! \return Timestamp of this dataset
     humotion::Timestamp MotionGenerator::get_timestamped_state(int joint_id,
                                                                float *position, float *velocity) {
         humotion::Timestamp stamp = joint_interface_->get_ts_position(joint_id).get_last_timestamp();
         humotion::Timestamp stamp_p = joint_interface_->get_ts_position(joint_id).get_last_timestamp();
         humotion::Timestamp stamp_v = joint_interface_->get_ts_speed(joint_id).get_last_timestamp();
         humotion::Timestamp stamp;
         if (stamp_v < stamp_p) {
             // right now there is no velocity with that timestamp yet, therefore use the velocity ts
             // for both:
             stamp = stamp_v;
         } else {
             // both are available at the position ts, use that one
             stamp = stamp_p;
+        }
         // fetch data
         *position = joint_interface_->get_ts_position(joint_id).get_interpolated_value(stamp);
         *velocity = joint_interface_->get_ts_speed(joint_id).get_interpolated_value(stamp);
         return stamp;

                 get_current_gaze().pan,
                 (get_current_position(JointInterface::ID_EYES_LEFT_LR) + get_current_position(JointInterface::ID_EYES_RIGHT_LR))/2.0,
                 get_current_position(JointInterface::ID_NECK_PAN));*/
         // store debug data:
         store_debug_data("neck/neck_pan_now", neck_pan_now);
         store_debug_data("neck/neck_tilt_now", neck_tilt_now);
         store_debug_data("neck/neck_pan_target", neck_pan_target);
         store_debug_data("neck/neck_tilt_target", neck_tilt_target);
         store_debug_data("neck/gaze_target_pan", requested_gaze_state_.pan);
         store_debug_data("neck/gaze_target_tilt", requested_gaze_state_.tilt);
         store_debug_data("neck/output_pan", reflexxes_position_output->NewPositionVector->VecData[0]);
         store_debug_data("neck/output_tilt", reflexxes_position_output->NewPositionVector->VecData[1]);
+    }
     //! publish targets to motor boards:

             // mw tick
             middleware_->tick();
             // do some logging
             // controller->dump_angles();
             // calculate all targets
             controller_->calculate_targets();
             // do some logging
             middleware_->publish_debug_dataset(controller_->get_debug_data());
             // publish data to joints
             controller_->publish_targets();

humotion

Revision 2aa96942