humotion

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)

#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));

}

#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;

};

#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);

	}

}

    if (last_gaze_target_update  + posix_time::milliseconds(1000) > get_system_time() ){