commit 0d2003bb64cfe389df616ef23a90c76151e9acfe
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Wed Jun 24 10:49:11 2015 +0200

    added korg demo

diff --git a/examples/demo_korg_slider/CMakeLists.txt b/examples/demo_korg_slider/CMakeLists.txt
new file mode 100644
index 0000000..14aad42
--- /dev/null
+++ b/examples/demo_korg_slider/CMakeLists.txt
@@ -0,0 +1,54 @@
+SET( PROJECT_NAME demo_korg_slider )
+PROJECT( ${PROJECT_NAME} )
+# set (CMAKE_VERBOSE_MAKEFILE true)
+#profiling
+#set(CMAKE_CXX_FLAGS "-Wall -g -pg")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+
+
+set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} /homes/sschulz/src/ros_workspace/install/share/humotion/cmake/)
+# set(CMAKE_LIBRARY_PATH ${CMAKE_LIBRARY_PATH} /homes/sschulz/src/ros_workspace/install/lib/pkg_config)
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+INCLUDE(FindPkgConfig)
+
+# search for Boost version 1.34
+find_package( Boost 1.40 COMPONENTS thread program_options system filesystem regex )
+link_directories ( ${Boost_LIBRARY_DIRS}  )
+include_directories ( ${Boost_INCLUDE_DIRS} )
+# message(${Boost_LIBRARIES})
+
+
+
+SET(PREFIX ${CMAKE_INSTALL_PREFIX})
+SET(BINDIR "${PREFIX}/bin")
+SET(INCLUDEDIR "${PREFIX}/include")
+SET(MANDIR "${PREFIX}/man")
+SET(LIBDIR "${PREFIX}/lib")
+SET(DATADIR "${PREFIX}/share/${NAME}")
+
+find_package(humotion 0.0.1)
+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)
+
+SET(SRC_FILES
+	korg_input.cpp
+)
+
+LINK_DIRECTORIES(${LIBDIR} ${humotion_LIBRARY_DIRS} )
+INCLUDE_DIRECTORIES( ${humotion_INCLUDE_DIRS} )
+
+ADD_EXECUTABLE( ${PROJECT_NAME} main.cpp ${SRC_FILES})
+TARGET_LINK_LIBRARIES(  ${PROJECT_NAME} ${Boost_LIBRARIES} ${Boost_LIBS}  ${humotion_LIBRARIES} portmidi porttime) 
+
+
+#FILE(GLOB conf_files "${CMAKE_CURRENT_SOURCE_DIR}/*.mcfg")
+#install (FILES ${conf_files} DESTINATION etc/xsc2)
+
+install (TARGETS ${PROJECT_NAME} DESTINATION bin )
+set_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/lib)
+set_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH_USE_LINK_PATH TRUE)
+
diff --git a/examples/demo_korg_slider/korg_input.cpp b/examples/demo_korg_slider/korg_input.cpp
new file mode 100644
index 0000000..94db141
--- /dev/null
+++ b/examples/demo_korg_slider/korg_input.cpp
@@ -0,0 +1,282 @@
+#include "korg_input.h"
+#include <assert.h>
+#include <stdlib.h>
+
+#define DEBUG_SHOW_MIDI_DEVICES 1
+#define DEBUG_INCOMING_DATA 1
+
+//defines for portmidi
+#define INPUT_BUFFER_SIZE 100
+#define OUTPUT_BUFFER_SIZE 0
+#define DRIVER_INFO NULL
+#define TIME_PROC ((int32_t (*)(void *)) Pt_Time)
+#define TIME_INFO NULL
+#define TIME_START Pt_Start(1, 0, 0) /* timer started w/millisecond accuracy */
+
+//! constructor
+KorgInput::KorgInput(){
+	midi = NULL;
+	//first try old device:
+	device_open = open_device();
+	for(int i=0; i<256; i++){
+		data[i] = 64;
+		data_new[i] = false;
+	}
+}
+
+//! destructor
+KorgInput::~KorgInput(){
+	if (device_open){
+		//close
+		Pm_Close(midi);
+	}
+}
+
+//! fetch device id
+//! \param char array with device name
+//! \return id of device, or -1 if not found
+int KorgInput::fetch_device_id(const char *name, bool output=false){
+	//fetch device id:
+	int midi_device_id = -1;
+	
+	for (int i = 0; i < Pm_CountDevices(); i++) {
+		const PmDeviceInfo *info = Pm_GetDeviceInfo(i);
+		if (DEBUG_SHOW_MIDI_DEVICES) printf("%d: %s, %s\n", i, info->interf, info->name);
+		
+		if (output){
+			if (info->output) {
+				if (strcmp(info->name, name) == 0){
+					midi_device_id = i;
+				}
+			}
+		}else{
+			if (info->input) {
+				if (strcmp(info->name, name) == 0){
+					midi_device_id = i;
+				}
+			}
+		}
+	}
+	
+	return midi_device_id;
+}
+
+//! open device
+//! \return true if device is found, false otherwise
+bool KorgInput::open_device(){
+	PmEvent buffer[1];
+	
+	//standard is type1:
+	device_type = NANO_KONTROL;
+
+	int midi_device_id = fetch_device_id("nanoKONTROL MIDI 1");
+	int midi_device_id_out = fetch_device_id("nanoKONTROL MIDI 1", true);
+	if (midi_device_id == -1){
+		device_type = NANO_KONTROL2;
+		midi_device_id = fetch_device_id("nanoKONTROL2 MIDI 1");
+		midi_device_id_out = fetch_device_id("nanoKONTROL2 MIDI 1", true);
+	}
+	
+	
+	if (midi_device_id == -1){
+		printf("> can not open device (input). Error: device not found\n");
+		return false;
+	}
+	
+	if (midi_device_id_out == -1){
+		printf("> can not open output device\n");
+		return false;
+	}
+	
+	//success, found device
+	printf("> found korg controller with id %d (%s)\n",midi_device_id,device_type==NANO_KONTROL?"nanoKONTROL":"nanoKONTROL2");
+	printf("> if setting leds on/off does not work, make sure to set led mode to external with the korg windows software\n");
+	
+	//open input
+	Pm_OpenInput(&midi, midi_device_id, DRIVER_INFO, INPUT_BUFFER_SIZE, TIME_PROC, TIME_INFO);
+	
+	//open output
+	Pm_OpenOutput(&midi_out, midi_device_id_out, DRIVER_INFO, OUTPUT_BUFFER_SIZE, NULL, NULL, 0); 
+ 
+
+
+
+	//set filter
+	Pm_SetFilter(midi, PM_FILT_ACTIVE | PM_FILT_CLOCK | PM_FILT_SYSEX);
+	Pm_SetFilter(midi_out, PM_FILT_ACTIVE | PM_FILT_CLOCK | PM_FILT_SYSEX);
+	
+	//clear buffer with unfiltered messages
+	//printf("> clearing buffer\n");
+	//while (Pm_Poll(midi)) {
+		//Pm_Read(midi, buffer, 1);
+	//}
+	printf("> init done \n");
+	
+	//disable all leds:
+// 	for(unsigned char id = 0; id<127; id++){
+// 		Pm_WriteShort(midi_out, TIME_PROC(TIME_INFO), Pm_Message(0xB0, id, 0));
+// 		usleep(10*1000);
+// 	}
+	
+	//fancy animation:
+	for(unsigned char id = 0; id<80+4; id++){
+		if (id<127) Pm_WriteShort(midi_out, TIME_PROC(TIME_INFO), Pm_Message(0xB0, id, 127));
+		if (id>4)   Pm_WriteShort(midi_out, TIME_PROC(TIME_INFO), Pm_Message(0xB0, id-4, 0));
+		usleep(25*1000);
+	}
+	
+// 	for(unsigned char id = 0; id<127; id++){
+// 		Pm_WriteShort(midi_out, TIME_PROC(TIME_INFO), Pm_Message(0xB0, id, 0));
+// 	}
+// 	
+	return true;
+}
+
+//! device opened sucessfully
+//! \return true if device is open and working
+bool KorgInput::device_available(){
+	return device_open;
+}
+
+
+//! fetch new data
+//! \return int count how many datasets were fetched
+int KorgInput::fetch_data(){
+	int new_data_fetched = 0;
+	//poll all data:
+	while(Pm_Poll(midi)){
+		//new data fetch sucessfull? build up dataset:
+		if (fetch_single()){
+			new_data_fetched++;
+		}
+	}
+
+	if (new_data_fetched > 0){
+		printf("> fetched %d datasets\n",new_data_fetched);
+	}
+	
+	return new_data_fetched;
+}
+
+bool KorgInput::fetch_single(){
+	PmEvent buffer[1];
+	
+	//poll data:
+	PmError status = Pm_Poll(midi);
+	
+	if (status) {
+		int length = Pm_Read(midi,buffer, 1);
+		if (length > 0) {
+			//store incoming data:
+			if (Pm_MessageStatus(buffer[0].message) == 0xb0){
+				unsigned int idx = Pm_MessageData1(buffer[0].message) & 0xFF;
+				data[idx] = Pm_MessageData2(buffer[0].message) & 0xFF;
+				data_new[idx] = true;
+			}
+			
+			if (DEBUG_INCOMING_DATA){
+				printf("> incoming message: time %ld, %2lx %2lx %2lx\n",
+					(long) buffer[0].timestamp,
+					(long) Pm_MessageStatus(buffer[0].message),
+					(long) Pm_MessageData1(buffer[0].message),
+					(long) Pm_MessageData2(buffer[0].message));
+			}
+			
+			return true;
+		} else {
+			printf("> error, message len <= 0?!\n");
+			return false;
+		}
+	}
+	
+}
+
+//! access buffered data
+//! \param index of data channel to fetch
+//! \return unsigned char value for data channel <index> (0..127)
+unsigned char KorgInput::get_buffered_raw_value(unsigned char index){
+	return data[index];
+}
+
+//! fetch a value + new flag & clear
+//! \param idx index
+//! \param uchar ptr to data
+//! \return true if new data available
+bool KorgInput::get_and_reset(unsigned char idx, unsigned char *val){
+	//fetch state & reset:
+	bool new_data = data_new[idx];
+	data_new[idx] = false;
+	
+	//override data only if new val arrived:
+	if (new_data){
+		*val = get_buffered_raw_value(idx);
+	}	
+	
+	return new_data;
+}
+
+//! access slider 
+//! \param slider index 0..8
+//! \param uchar ptr to value
+//! \return bool if new value arrived
+bool KorgInput::get_slider(unsigned char index, unsigned char *val){
+	assert(index <= 8);
+	if (device_type == NANO_KONTROL){
+		switch(index){
+			default: 
+				
+				return get_and_reset(0x02 + index, val);
+			case(5):
+			case(6):
+				return get_and_reset(0x02 + index + 1, val);
+			case(7):
+			case(8):
+				return get_and_reset(0x02 + index + 3, val);
+		}
+	}else{
+		//NANO_KONTROL2
+		return get_and_reset(index, val);
+	}
+}
+
+//! access button 
+//! \param button index 
+//! \param uchar ptr to value
+//! \return bool if new value arrived
+bool KorgInput::get_button(unsigned char index, unsigned char *val){
+	assert(index <= 127);
+	
+	if (device_type == NANO_KONTROL){
+		printf("> NANO_KONTROL NOT SUPPORTED, different layout of buttons! exiting\n");
+		exit(1);
+		/*if (row == 0){
+			return get_and_reset(0x17+index, val);
+		}else if (row == 1){
+			printf("> NANO_KONTROL does not have center row. ignored\n");
+			return false;
+		}else{
+			return get_and_reset(0x21+index, val);
+		}*/
+	}else{
+		//NANO_KONTROL2 - fetch button value
+		return get_and_reset(index, val);
+	}
+}
+
+//! access pot 
+//! \param pot index 0..8
+//! \param unsigned char ptr to value (0..127)
+//! \param bool true if new data available
+bool KorgInput::get_pot(unsigned char index, unsigned char *val){
+	assert(index <= 8);
+	if (device_type == NANO_KONTROL){
+		return get_and_reset(0x0E + index, val);
+	}else{
+		//NANO_KONTROL2
+		return get_and_reset(0x10 + index, val);
+	}
+}
+
+void  KorgInput::set_led(unsigned char index, bool status){
+	Pm_WriteShort(midi_out, TIME_PROC(TIME_INFO), Pm_Message(0xB0, index, status?127:0));
+}
\ No newline at end of file
diff --git a/examples/demo_korg_slider/korg_input.h b/examples/demo_korg_slider/korg_input.h
new file mode 100644
index 0000000..96f7064
--- /dev/null
+++ b/examples/demo_korg_slider/korg_input.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <portmidi.h>
+#include <porttime.h>
+#include <cstdio>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+class KorgInput{
+public:
+	KorgInput();
+	~KorgInput();
+	
+	bool  get_slider(unsigned char index, unsigned char *val);
+	bool  get_pot(unsigned char index, unsigned char *val);
+	bool  get_button(unsigned char index, unsigned char *val);
+	void  set_led(unsigned char index, bool status);
+	
+	bool get_and_reset(unsigned char idx, unsigned char *val);
+	void set_pot(unsigned char index, int value);
+	
+	int  fetch_data();
+	bool device_available();
+	
+	
+private:
+	int resolve_id();
+	
+	unsigned char data[256];
+	bool data_new[256];
+	int device_type;
+	enum DEVICE_TYPE {NANO_KONTROL, NANO_KONTROL2};
+	bool open_device();
+	bool fetch_single();
+	int fetch_device_id(const char*, bool);
+	unsigned char get_buffered_raw_value(unsigned char index);
+	
+	bool device_open;
+	PmStream * midi;
+	PmStream * midi_out;
+};
\ No newline at end of file
diff --git a/examples/demo_korg_slider/main.cpp b/examples/demo_korg_slider/main.cpp
new file mode 100644
index 0000000..b756860
--- /dev/null
+++ b/examples/demo_korg_slider/main.cpp
@@ -0,0 +1,229 @@
+#include <cstdio>
+#include <stdio.h>
+#include <string.h>
+
+
+#include "korg_input.h"
+#include "humotion/client/client.h"
+#include "boost/date_time/posix_time/posix_time.hpp"
+#include <boost/thread/thread_time.hpp>
+#include <boost/thread/thread.hpp>
+
+using namespace boost;
+humotion::GazeState last_awake_state;
+bool awake_state;
+
+enum JOINT_SLIDER_ID{
+	MOUTH_POSITION_LEFT=0,
+	MOUTH_POSITION_CENTER,
+	MOUTH_POSITION_RIGHT,
+	PAN,
+	TILT
+	//NECK_ROLL,
+};
+
+enum JOINT_POTI_ID{
+	MOUTH_OPENING_LEFT=0,
+	MOUTH_OPENING_CENTER,
+	MOUTH_OPENING_RIGHT,
+	EYE_VERGENCE,
+	EYELID_OPENING_UPPER,
+	EYELID_OPENING_LOWER,
+	EYEBROW_RIGHT,
+	EYEBROW_LEFT
+};
+
+
+
+float to_float(unsigned char val){
+	return ((float)(val))/127.0;
+}
+
+void slidervalues_to_mouthstate(KorgInput *input, humotion::MouthState *result){
+	unsigned char tmp;
+	
+	//do not handle updates when not awake
+	if (!awake_state){
+		return;
+	}
+	
+	if (input->get_slider(MOUTH_POSITION_LEFT,  &tmp))result->position_left   = 0.0 + 40.0 * (1.0-to_float(tmp));
+	if (input->get_slider(MOUTH_POSITION_CENTER,  &tmp))result->position_center = 0.0 + 40.0 * (1.0-to_float(tmp));
+	if (input->get_slider(MOUTH_POSITION_RIGHT,  &tmp))result->position_right  = 0.0 + 40.0 * (1.0-to_float(tmp));
+
+	if (input->get_pot(MOUTH_OPENING_LEFT,  &tmp)) result->opening_left   = 40.0 * to_float(tmp);
+	if (input->get_pot(MOUTH_OPENING_CENTER,  &tmp)) result->opening_center= 40.0 * to_float(tmp);
+	if (input->get_pot(MOUTH_OPENING_RIGHT,  &tmp)) result->opening_right  = 40.0 * to_float(tmp);
+
+	//printf("OOO %4.2f %4.2f\n",result.opening_center, result.position_center);
+}
+
+void wake_up(humotion::GazeState *result){
+	if (awake_state){
+		//already awake, return
+		return;
+	}
+	
+	//restore last awake state
+	*result = last_awake_state;
+	//force reopening
+	result->eyeblink_request_left  = 10;
+	result->eyeblink_request_right = 10;
+	
+	awake_state = true;
+}
+
+void sleep(humotion::GazeState *result){
+	if (!awake_state){
+		//already sleeping, return
+		return;
+	}
+	
+	//store last awake state
+	last_awake_state = *result;
+	awake_state = false;
+	
+	//override values:
+	result->pan  = 0.0;
+	result->tilt = -20.0;
+	result->roll = 0.0;
+	
+	//result->eyelid_opening = 0.0;
+	result->eyebrow_left  = 0.0;
+	result->eyebrow_right = 0.0;
+	
+	result->eyeblink_request_left = -1;
+	result->eyeblink_request_right = -1;
+	
+	
+}
+
+void slidervalues_to_gazestate(KorgInput *input, humotion::GazeState *result){
+	unsigned char tmp;
+	
+	//PLAY
+	if (input->get_button(0x29, &tmp)){
+		if (tmp != 0){
+			wake_up(result);
+		}
+	}
+	
+	//STOP
+	if (input->get_button(0x2A, &tmp)){
+		if (tmp != 0){
+			sleep(result);
+		}
+	}
+	
+	//do not handle updates when not awake
+	if (!awake_state){
+		return;
+	}
+	
+	if (input->get_slider(PAN,  &tmp)) result->pan  = -50.0 + 100.0 * to_float(tmp);
+	if (input->get_slider(TILT, &tmp)) result->tilt = -40.0 + 80.0 * to_float(tmp);
+	result->roll = 0.0;
+	
+	if (input->get_pot(EYELID_OPENING_UPPER, &tmp)) result->eyelid_opening_upper = 0.0 + 120.0*to_float(tmp);
+	if (input->get_pot(EYELID_OPENING_LOWER, &tmp)) result->eyelid_opening_lower = 0.0 + 120.0*to_float(tmp);
+
+	
+	if (input->get_pot(EYEBROW_LEFT,  &tmp)) result->eyebrow_left  = 60.0 - 120.0*to_float(tmp);
+	if (input->get_pot(EYEBROW_RIGHT,  &tmp)) result->eyebrow_right = 60.0 - 120.0*to_float(tmp);
+	
+	if (input->get_pot(EYE_VERGENCE,  &tmp)) result->vergence = -20.0 * to_float(tmp);
+	
+	
+	//check for blink requests: 
+	if (input->get_button(0x24, &tmp)){
+		if (tmp != 0){
+			//blink left
+			result->eyeblink_request_left = 300;
+		}
+	}
+	if (input->get_button(0x44, &tmp)){
+		if (tmp != 0){
+			result->eyeblink_request_right = 300;
+		}
+	}
+	if (input->get_button(0x34, &tmp)){
+		if (tmp != 0){
+			//blink both
+			result->eyeblink_request_right = 150;
+			result->eyeblink_request_left  = 150;
+		}
+	}
+	
+	//printf("PTR = %3.1f %3.1f %3.1f\n",result->pan,result->tilt,result->roll);
+}
+
+int main(int argc, char **argv){
+	humotion::MouthState mouth_state;
+	humotion::GazeState gaze_state;
+	awake_state = true;
+	
+	if (argc != 2){
+		printf("> ERROR: invalid parameter count.\n\nusage: %s <scope>\n       (e.g. %s /flobi)\n\n",argv[0],argv[0]);
+		exit(EXIT_FAILURE);
+	}
+
+	printf("> starting korg kontrol demo. please make sure the korg device is connected via usb\n");
+	
+	KorgInput *kinput = new KorgInput();
+	
+	if (!kinput->device_available()){
+		printf("> no korg kontrol device found on usb bus... exiting\n");
+		return 0;
+	}
+	
+	//set led status for blink requests:
+	kinput->set_led(0x24, true);
+	kinput->set_led(0x34, true);
+	kinput->set_led(0x44, true);
+	
+	//set led for stop/play
+	kinput->set_led(0x29, true);
+	kinput->set_led(0x2A, true);
+	
+	
+	//human motion connection:
+	humotion::client::Client *client = new humotion::client::Client(argv[1], "ROS");
+	
+	//send values to human motion server
+	float loop_delay = 1000.0 / 50.0; //run with 50Hz
+	
+	boost::system_time timeout = get_system_time() + posix_time::milliseconds(loop_delay);
+	
+	while(client->ok()){
+		//allow middleware to process data
+		client->tick();
+		
+		//clear previous eyeblinks
+		gaze_state.eyeblink_request_left  = 0;
+		gaze_state.eyeblink_request_right = 0;
+	
+		//handle slider data
+		if (kinput->fetch_data() > 0){
+			//debug
+			mouth_state.dump();
+			gaze_state.dump();
+			printf("\n");
+
+			//transfer values to robot:
+			slidervalues_to_mouthstate(kinput, &mouth_state);
+			slidervalues_to_gazestate(kinput, &gaze_state);
+		} 
+		
+		//printf("BLINK: %d %d\n",gaze_state.eyeblink_request_left, gaze_state.eyeblink_request_right);
+		
+		//periodically send mouth & gaze target
+		client->update_mouth_target(mouth_state);
+		client->update_gaze_target(gaze_state);
+		client->send_all();
+		
+		thread::sleep(timeout);
+		timeout = get_system_time() + posix_time::milliseconds(loop_delay);
+
+	}
+	
+}
diff --git a/src/server/motion_generator.cpp b/src/server/motion_generator.cpp
index 6f36b35..fe5f251 100644
--- a/src/server/motion_generator.cpp
+++ b/src/server/motion_generator.cpp
@@ -76,7 +76,7 @@ void MotionGenerator::set_mouth_target(MouthState s){
 //! was there incoming gaze data the last second?
 //! \return true if there was data incoming in the last second, false otherwise
 bool MotionGenerator::gaze_target_input_active(){
-    if (last_gaze_target_update + posix_time::milliseconds(1000) > get_system_time() ){
+    if (last_gaze_target_update  + posix_time::milliseconds(1000) > get_system_time() ){
         //incoming data -> if gaze is disabled, enable it!
         joint_interface->enable_gaze_joints();
         return true;
