humotion

cmake_minimum_required(VERSION 2.8.3)

project(humotion)

set(ENV{ROS_LANG_DISABLE} "genjava")

set(ROS_BUILD_TYPE Debug)

################################################################

# check for ROS support:

find_package(catkin REQUIRED COMPONENTS

    roscpp

    std_msgs

    sensor_msgs

    message_generation

    genmsg

    dynamic_reconfigure

)

IF (NOT catkin_FOUND)

  MESSAGE(FATAL_ERROR "Error: could not find ROS middleware!")

ENDIF (NOT catkin_FOUND)

INCLUDE(FindPkgConfig)

##libreflexxes

#IF (libreflexxes_DIR)

#  MESSAGE("using libreflexxes_DIR as override ('${libreflexxes_DIR}')")

#  SET(REFLEXXES_PREFIX ${libreflexxes_DIR})

#  SET(REFLEXXES_LIBRARY "ReflexxesTypeII")

#ELSE ()

  PKG_CHECK_MODULES(REFLEXXES REQUIRED libReflexxesTypeII>=1.2.3)

  IF (NOT REFLEXXES_FOUND)

    message(FATAL_ERROR "Error: could not find lib libReflexxesTypeII")

  ENDIF ()

#ENDIF ()

SET(REFLEXXES_LIBRARY_DIRS "${REFLEXXES_PREFIX}/lib")

SET(REFLEXXES_INCLUDE_DIRS "${REFLEXXES_PREFIX}/include")

#resolve to absolute library filename

#or is there an easier way to get the absolute lib filename from pkg config files?!

find_library(REFLEXXES_LIBRARY NAMES ${REFLEXXES_LIBRARIES} HINTS ${REFLEXXES_LIBRARY_DIRS})

MESSAGE("-- using libReflexxesTypeII version ${REFLEXXES_VERSION} from ${REFLEXXES_INCLUDE_DIRS} and ${REFLEXXES_LIBRARY_DIRS}")

MESSAGE("-- will link against ${REFLEXXES_LIBRARY}")

set(CMAKE_CXX_FLAGS "-g -Wall")

add_definitions ("-Wall")

#######################################

## Declare ROS messages and services ##

#######################################

add_message_files(

        FILES

        gaze.msg

        position_lcr.msg

        mouth.msg

)

## Generate added messages and services with any dependencies listed here

generate_messages(

        DEPENDENCIES

        std_msgs

        humotion

)

# add dynamic reconfigure api

generate_dynamic_reconfigure_options(

  cfg/humotion.cfg

)

#

###################################

## catkin specific configuration ##

###################################

## The catkin_package macro generates cmake config files for your package

## Declare things to be passed to dependent projects

## INCLUDE_DIRS: uncomment this if you package contains header files

## LIBRARIES: libraries you create in this project that dependent projects also need

## CATKIN_DEPENDS: catkin_packages dependent projects also need

## DEPENDS: system dependencies of this project that dependent projects also need

catkin_package(

    INCLUDE_DIRS include

    LIBRARIES humotion

    #CATKIN_DEPENDS message_runtime

    #DEPENDS system_lib

)

###########

## Build ##

###########

## Specify additional locations of header files

## Your package locations should be listed before other locations

include_directories(BEFORE ${Boost_INCLUDE_DIRS} ${REFLEXXES_INCLUDE_DIRS})

include_directories(BEFORE include)

#make sure to use ros messages from current build

include_directories(BEFORE ${CATKIN_DEVEL_PREFIX}/include)

#this should be appended:

include_directories(${catkin_INCLUDE_DIRS})

#link_directories (${Boost_LIBRARY_DIRS} ${REFLEXXES_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS})

## Declare a cpp library

add_library(${PROJECT_NAME}

    src/client/client.cpp

    src/client/middleware.cpp

    src/client/middleware_ros.cpp

    src/server/server.cpp

    src/server/config.cpp

    src/server/middleware.cpp

    src/server/middleware_ros.cpp

    src/server/controller.cpp

    src/server/joint_interface.cpp

    src/server/motion_generator.cpp

    src/server/gaze_motion_generator.cpp

    src/server/reflexxes_motion_generator.cpp

    src/server/mouth_motion_generator.cpp

    src/server/eye_motion_generator.cpp

    src/server/eyelid_motion_generator.cpp

    src/server/eyebrow_motion_generator.cpp

    src/server/neck_motion_generator.cpp

    src/mouth_state.cpp

    src/gaze_state.cpp

    src/timestamp.cpp

    src/timestamped_list.cpp

)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")

## Add cmake target dependencies of the executable/library

## as an example, message headers may need to be generated before nodes

add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp ${PROJECT_NAME}_gencfg)

## Specify libraries to link a library or executable target against

target_link_libraries(${PROJECT_NAME}

    ${Boost_LIBRARIES}

    ${catkin_LIBRARIES}

    ${REFLEXXES_LIBRARY}

)

set_property(TARGET ${PROJECT_NAME} PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE)

#############

## Install ##

#############

# all install targets should use catkin DESTINATION variables

# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation

## in contrast to setup.py, you can choose the destination

# install(PROGRAMS

#   scripts/my_python_script

#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

# )

## Mark executables and/or libraries for installation

install(TARGETS ${PROJECT_NAME}

    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

)

## Mark cpp header files for installation

install(DIRECTORY include/humotion/

    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

)

#############

## Testing ##

#############

## Add gtest based cpp test target and link libraries

catkin_add_gtest(${PROJECT_NAME}-test-server test/server.cpp)

if(TARGET ${PROJECT_NAME}-test-server)

target_link_libraries(${PROJECT_NAME}-test-server ${PROJECT_NAME})

endif()

catkin_add_gtest(${PROJECT_NAME}-test-client test/client.cpp)

if(TARGET ${PROJECT_NAME}-test-client)

target_link_libraries(${PROJECT_NAME}-test-client ${PROJECT_NAME})

endif()

catkin_add_gtest(${PROJECT_NAME}-test-timestamp test/timestamp.cpp)

if(TARGET ${PROJECT_NAME}-test-timestamp)

target_link_libraries(${PROJECT_NAME}-test-timestamp ${PROJECT_NAME})

endif()

### Add folders to be run by python nosetests

# catkin_add_nosetests(test)

#finally build (some) examples

#add_subdirectory(./examples)

#################

## style guide ##

#################

set(ENABLE_CPPLINT 1)

include(${CMAKE_CURRENT_SOURCE_DIR}/stylecheck/CpplintWrapper.cmake)

CPPLINT_RECURSIVE(cpplint_include

  ${CMAKE_CURRENT_SOURCE_DIR}/include

  ${CMAKE_CURRENT_SOURCE_DIR}/include

  ${CMAKE_CURRENT_BINARY_DIR}/include)

CPPLINT_RECURSIVE(cpplint_src

  ${CMAKE_CURRENT_SOURCE_DIR}/src

  ${CMAKE_CURRENT_SOURCE_DIR}/src

  ${CMAKE_CURRENT_BINARY_DIR}/src)

add_dependencies(${PROJECT_NAME} cpplint_src cpplint_include)

#workaround for qtcreator ide integration. do not remove!

file(GLOB_RECURSE NODE_DUMMY_TARGETS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.h *.cfg *.yaml *.xml *.launch)

add_custom_target(_dummy_target SOURCES ${NODE_DUMMY_TARGETS})

                   GNU LESSER GENERAL PUBLIC LICENSE

                       Version 3, 29 June 2007

 Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>

 Everyone is permitted to copy and distribute verbatim copies

 of this license document, but changing it is not allowed.

  This version of the GNU Lesser General Public License incorporates

the terms and conditions of version 3 of the GNU General Public

License, supplemented by the additional permissions listed below.

  0. Additional Definitions.

  As used herein, "this License" refers to version 3 of the GNU Lesser

General Public License, and the "GNU GPL" refers to version 3 of the GNU

General Public License.

  "The Library" refers to a covered work governed by this License,

other than an Application or a Combined Work as defined below.

  An "Application" is any work that makes use of an interface provided

by the Library, but which is not otherwise based on the Library.

Defining a subclass of a class defined by the Library is deemed a mode

of using an interface provided by the Library.

  A "Combined Work" is a work produced by combining or linking an

Application with the Library.  The particular version of the Library

with which the Combined Work was made is also called the "Linked

Version".

  The "Minimal Corresponding Source" for a Combined Work means the

Corresponding Source for the Combined Work, excluding any source code

for portions of the Combined Work that, considered in isolation, are

based on the Application, and not on the Linked Version.

  The "Corresponding Application Code" for a Combined Work means the

object code and/or source code for the Application, including any data

and utility programs needed for reproducing the Combined Work from the

Application, but excluding the System Libraries of the Combined Work.

  1. Exception to Section 3 of the GNU GPL.

  You may convey a covered work under sections 3 and 4 of this License

without being bound by section 3 of the GNU GPL.

  2. Conveying Modified Versions.

  If you modify a copy of the Library, and, in your modifications, a

facility refers to a function or data to be supplied by an Application

that uses the facility (other than as an argument passed when the

facility is invoked), then you may convey a copy of the modified

version:

   a) under this License, provided that you make a good faith effort to

   ensure that, in the event an Application does not supply the

   function or data, the facility still operates, and performs

   whatever part of its purpose remains meaningful, or

   b) under the GNU GPL, with none of the additional permissions of

   this License applicable to that copy.

  3. Object Code Incorporating Material from Library Header Files.

  The object code form of an Application may incorporate material from

a header file that is part of the Library.  You may convey such object

code under terms of your choice, provided that, if the incorporated

material is not limited to numerical parameters, data structure

layouts and accessors, or small macros, inline functions and templates

(ten or fewer lines in length), you do both of the following:

   a) Give prominent notice with each copy of the object code that the

   Library is used in it and that the Library and its use are

   covered by this License.

   b) Accompany the object code with a copy of the GNU GPL and this license

   document.

  4. Combined Works.

  You may convey a Combined Work under terms of your choice that,

taken together, effectively do not restrict modification of the

portions of the Library contained in the Combined Work and reverse

engineering for debugging such modifications, if you also do each of

the following:

   a) Give prominent notice with each copy of the Combined Work that

   the Library is used in it and that the Library and its use are

   covered by this License.

   b) Accompany the Combined Work with a copy of the GNU GPL and this license

   document.

   c) For a Combined Work that displays copyright notices during

   execution, include the copyright notice for the Library among

   these notices, as well as a reference directing the user to the

   copies of the GNU GPL and this license document.

   d) Do one of the following:

       0) Convey the Minimal Corresponding Source under the terms of this

       License, and the Corresponding Application Code in a form

       suitable for, and under terms that permit, the user to

       recombine or relink the Application with a modified version of

       the Linked Version to produce a modified Combined Work, in the

       manner specified by section 6 of the GNU GPL for conveying

       Corresponding Source.

       1) Use a suitable shared library mechanism for linking with the

       Library.  A suitable mechanism is one that (a) uses at run time

       a copy of the Library already present on the user's computer

       system, and (b) will operate properly with a modified version

       of the Library that is interface-compatible with the Linked

       Version.

   e) Provide Installation Information, but only if you would otherwise

   be required to provide such information under section 6 of the

   GNU GPL, and only to the extent that such information is

   necessary to install and execute a modified version of the

   Combined Work produced by recombining or relinking the

   Application with a modified version of the Linked Version. (If

   you use option 4d0, the Installation Information must accompany

   the Minimal Corresponding Source and Corresponding Application

   Code. If you use option 4d1, you must provide the Installation

   Information in the manner specified by section 6 of the GNU GPL

   for conveying Corresponding Source.)

  5. Combined Libraries.

  You may place library facilities that are a work based on the

Library side by side in a single library together with other library

facilities that are not Applications and are not covered by this

License, and convey such a combined library under terms of your

choice, if you do both of the following:

   a) Accompany the combined library with a copy of the same work based

   on the Library, uncombined with any other library facilities,

   conveyed under the terms of this License.

   b) Give prominent notice with the combined library that part of it

   is a work based on the Library, and explaining where to find the

   accompanying uncombined form of the same work.

  6. Revised Versions of the GNU Lesser General Public License.

  The Free Software Foundation may publish revised and/or new versions

of the GNU Lesser General Public License from time to time. Such new

versions will be similar in spirit to the present version, but may

differ in detail to address new problems or concerns.

  Each version is given a distinguishing version number. If the

Library as you received it specifies that a certain numbered version

of the GNU Lesser General Public License "or any later version"

applies to it, you have the option of following the terms and

conditions either of that published version or of any later version

published by the Free Software Foundation. If the Library as you

received it does not specify a version number of the GNU Lesser

General Public License, you may choose any version of the GNU Lesser

General Public License ever published by the Free Software Foundation.

  If the Library as you received it specifies that a proxy can decide

whether future versions of the GNU Lesser General Public License shall

apply, that proxy's public statement of acceptance of any version is

permanent authorization for you to choose that version for the

Library.

this is the low level robot control software to allow

human like motion generation on a humanoid robotic head

#!/usr/bin/env python

import re

PACKAGE = "humotion"

from dynamic_reconfigure.parameter_generator_catkin import *

def fetch_default(param_name):

    for line in open("../src/server/config.cpp"):

        if param_name in line:

            result = re.findall("=\s*([\d\w.]+);", line)

            print result

            if result:

                return result[0]

            else:

                print("ERROR: could not find parameter %s in config.cpp" % (param_name))

                sys.exit(1)

def add_entry(group, param_name, descr, min, max):

    default_str = fetch_default(param_name)

    default_val = float(default_str)

    if (default_val > max):

        print("ERROR: default value %f for %s exceeds max value (%f)" % (default_val, param_name, max))

        sys.exit(1)

    if (default_val < min):

        print("ERROR: default value %f for %s is under min value (%f)" % (default_val, param_name, min))

        sys.exit(1)

    group.add(param_name, double_t, 0, descr, default_val, min, max)

def add_entry_bool(group, param_name, descr):

    default = fetch_default(param_name)

    default_val = True

    if (default == "false"):

        default_val = False

    group.add(param_name, bool_t, 0, descr, default_val)

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)

add_entry(general_group, "threshold_angle_omr_limit", "threshold for a deflection that triggers a correction neck saccade (in percent of OMR)", 0.1, 1.0)

neck_group = gen.add_group("neck")

add_entry(neck_group, "scale_velocity_neck", "scaling factor for neck velocity (in percent, 1.0 = full human velocities)", 0.1, 1.0)

add_entry(neck_group, "scale_acceleration_neck", "scaling factor for neck acceleration (in percent, 1.0 = full human acceleration)", 0.1, 1.0)

add_entry(neck_group, "limit_velocity_neck", "limit for neck velocity (in deg/s)", 100.0, 800.0)

add_entry(neck_group, "limit_acceleration_neck", "limit for neck acceleration (in deg/s^2)", 100.0, 10000.0)

eye_group = gen.add_group("eye")

add_entry(eye_group, "scale_velocity_eye", "scaling factor for eye velocity (in percent, 1.0 = full human velocities)",  0.1, 1.0)

add_entry(eye_group, "scale_acceleration_eye", "scaling factor for eye acceleration (in percent, 1.0 = full human acceleration)", 0.1, 1.0)

add_entry(eye_group, "limit_velocity_eye", "limit for eye velocity (in deg/s)", 100.0, 1000.0)

add_entry(eye_group, "limit_acceleration_eye", "limit for eye acceleration (in deg/s^2)", 100.0, 80000.0)

add_entry_bool(eye_group, "use_neck_target_instead_of_position_eye", "use neck target in difference calc instead of real position")

eyeblink_group = gen.add_group("eyeblink")

add_entry(eyeblink_group, "eyeblink_duration", "duration for an eyeblink (in seconds)", 0.01, 1.0)

add_entry(eyeblink_group, "eyeblink_periodic_distribution_lower", "lower bound for probalistic eyeblink distribution (in seconds)", 0.1, 100.0)

add_entry(eyeblink_group, "eyeblink_periodic_distribution_upper", "upper bound for probalistic eyeblink distribution (in seconds)", 0.1, 100.0)

add_entry(eyeblink_group, "eyeblink_probability_after_saccade", "probability for an eyeblink after a saccade (in percent)", 0.01, 1.0)

add_entry(eyeblink_group, "eyeblink_blocked_time", "blocking time for further eyeblinks (in seconds)", 0.1, 100.0)

eyelids_group = gen.add_group("eyelids")

add_entry_bool(eyelids_group, "eyelids_follow_eyemotion", "should the eyelids follow the eye tilt motion?")

breath_group = gen.add_group("breath")

add_entry(breath_group, "breath_period", "duration for a full breath periond: inhale, pause and exhale (in seconds)", 1.0, 100.0)

add_entry(breath_group, "breath_amplitude", "amplitude for breath animation (in deg)", 0.0, 10.0)

exit(gen.generate(PACKAGE, "humotion", "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);

	}

}

PROJECT(meka_humotion_server)

cmake_minimum_required(VERSION 2.8)

SET(MAIN ${PROJECT_NAME})

set(ENV{ROS_LANG_DISABLE} "genjava")

set(ROS_BUILD_TYPE Debug)

################################################################

# check for ROS support:

find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs message_generation genmsg)

IF (catkin_FOUND)

    set(ROS_FOUND 1)

    message(STATUS "ROS Support is ON")

    add_definitions(-DROS_SUPPORT=1)

ENDIF (catkin_FOUND)

IF (NOT catkin_FOUND)

  message(FATAL_ERROR "Error: could not find ROS!")

ENDIF ()

FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread)

FIND_PACKAGE(humotion REQUIRED)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")

#add_message_files(

#    FILES

#)

#generate_messages(

#    DEPENDENCIES

#    std_msgs

#)

#hack to allow sub dir calls work

catkin_package(

    INCLUDE_DIRS include

    LIBRARIES humotion

    #CATKIN_DEPENDS message_runtime

    #DEPENDS system_lib

)

include_directories( ${catkin_INCLUDE_DIRS})

# add include directories

INCLUDE_DIRECTORIES(${YARP_INCLUDE_DIRS} ${ICUB_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} include/)

link_directories(${Boost_LIBRARY_DIRS} ${humotion_LIBRARY_DIRS})

# add required linker flags

SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ICUB_LINK_FLAGS}")

file(GLOB DUMMY_HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} include/*.h)

#add_dependencies(${MAIN} ${catkin_EXPORTED_TARGETS})

ADD_EXECUTABLE(${MAIN} src/main.cpp src/mekajointinterface.cpp ${DUMMY_HEADER_LIST})

TARGET_LINK_LIBRARIES(${MAIN} ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${humotion_LIBRARIES})

set_property(TARGET ${MAIN} PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE)

#INSTALL(TARGETS ${MAIN} DESTINATION bin)

install(TARGETS ${MAIN}

    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    RUNTIME DESTINATION bin 

)

#pragma once

#include <humotion/server/joint_interface.h>

#include <humotion/server/server.h>

#include <boost/bimap.hpp>

#include "ros/ros.h"

#include "sensor_msgs/JointState.h"

#include "trajectory_msgs/JointTrajectory.h"

#include <m3meka_msgs/M3ControlStates.h>

class MekaJointInterface : public humotion::server::JointInterface{

public:

    MekaJointInterface(std::string _input_scope, std::string control_scope, std::string _output_scope);

    ~MekaJointInterface();

    //void fetch_position(Device *dev, double timestamp);

    //void fetch_speed(Device *dev, double timestamp);

    //void fetch_position(int id, double value, double timestamp);

    //void fetch_speed(int id, double value, double timestamp);

    void run();

    static const int MAIN_LOOP_FREQUENCY = 50;

protected:

    void disable_joint(int e);

    void publish_target(int e, float position, float velocity);

    void enable_joint(int e);

    void execute_motion();

private:

    void incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state);

    void incoming_jointstates(const sensor_msgs::JointState & msg);

    void store_dummy_data(int id, humotion::Timestamp timestamp);

    void store_min_max(int id, float min, float max);

    ros::Subscriber joint_state_subscriber;

    ros::Subscriber control_state_subscriber;

    ros::Publisher target_publisher;

    void set_eyelid_angle(double angle);

    void set_eyebrow_angle(int id);

    void set_mouth();

    //iCubDataReceiver *icub_data_receiver;

    void init_joints();

    double lid_angle;

    int lid_opening_previous;

    int previous_mouth_state;

    std::string input_scope;

    std::string control_scope;

    std::string output_scope;

    float last_pos_eye_vergence;

    float last_pos_eye_pan;

    float last_vel_eye_vergence;

    float last_vel_eye_pan;

    bool controller_enabled;

    void store_joint(int id, float value);

    void set_target_in_positionmode(int id, double value);

    void set_target_in_velocitymode(int id, double value);

    int convert_enum_to_motorid(int e);

    int convert_motorid_to_enum(int id);

    typedef boost::bimap<int, int > enum_id_bimap_t;

    typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;

    enum_id_bimap_t enum_id_bimap;

};

<?xml version="1.0"?>

<package>

  <name>meka_humotion_server</name>

  <version>0.0.1</version>

  <description>humotion server for the mekabot</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag --> 

  <maintainer email="sschulz@todo.todo">Simon Schulz</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->

  <!-- Commonly used license strings: -->

  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->

  <license>TODO</license>

  <!-- Url tags are optional, but mutiple are allowed, one per tag -->

  <!-- Optional attribute type can be: website, bugtracker, or repository -->

  <!-- Example: -->

  <url type="website">https://projects.cit-ec.uni-bielefeld.de/git/flobidev.core.git</url> -->

  <!-- Author tags are optional, mutiple are allowed, one per tag -->

  <!-- Authors do not have to be maintianers, but could be -->

  <!-- Example: -->

  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->

  <!-- The *_depend tags are used to specify dependencies -->

  <!-- Dependencies can be catkin packages or system dependencies -->

  <!-- Examples: -->

  <!-- Use build_depend for packages you need at compile time: -->

  <!-- Use buildtool_depend for build tool packages: -->

  <!--   <buildtool_depend>catkin</buildtool_depend> -->

  <!-- Use run_depend for packages you need at runtime: -->

  <!-- <run_depend>message_runtime</run_depend> -->

  <!-- Use test_depend for packages you need only for testing: -->

  <!--   <test_depend>gtest</test_depend> -->

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>

  <build_depend>std_msgs</build_depend>

  <!-- <run_depend>roscpp</run_depend> -->

  <!-- <run_depend>std_msgs</run_depend> -->

  <!--<build_depend>actionlib</build_depend>

  <build_depend>actionlib_msgs</build_depend>

  <run_depend>actionlib</run_depend> -->

  <!-- <run_depend>actionlib_msgs</run_depend> -->

  <!-- The export tag contains other, unspecified, tags -->

  <export>

    <!-- You can specify that this package is a metapackage here: -->

    <!-- <metapackage/> -->

    <!-- Other tools can request additional information be placed here -->

  </export>

</package>

#include <stdio.h>

#include <humotion/server/server.h>

#include <string>

#include <iostream>

//#include "meka_data_receiver.h"

#include "mekajointinterface.h"

using namespace std;

int main(int argc, char *argv[]){

    /*Property params;

    params.fromCommand(argc, argv);

    if (!params.check("robot")){

        fprintf(stderr, "Please specify the name of the robot\n");

        fprintf(stderr, "--robot name (e.g. icub or icubSim)\n");

        return -1;

    }

    string robotName=params.find("robot").asString().c_str();

    string scope="/"+robotName;

*/

    if (argc != 5){

        printf("> ERROR: invalid number of parameters passed to server!\n\n");

        printf("usage   : %s <humotion base topic> <jointstates topic> <controlstates topic> <control output topic>\n\n",argv[0]);

        printf("example : %s /meka /joint_states  /meka_roscontrol_state_manager/state /meka_roscontrol/head_position_trajectory_controller/command)\n\n",argv[0]);

        exit(EXIT_FAILURE);

    }

    //create humotion interface

    string humotion_scope      = argv[1];

    string jointstates_scope   = argv[2];

    string controlstates_scope = argv[3];

    string control_scope       = argv[4];

    MekaJointInterface *jointinterface = new MekaJointInterface(jointstates_scope, controlstates_scope, control_scope);

    humotion::server::Server *humotion_server = new humotion::server::Server(humotion_scope, "ROS", jointinterface);

    //finally run it

    jointinterface->run();

    while(humotion_server->ok()){

        usleep(100000);

    }

    printf("> ros connection died, will exit now\n");

    return 0;

}

#include "mekajointinterface.h"

using std::cout;

using std::cerr;

using humotion::Timestamp;

//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED

#define POSITION_CONTROL 1

void MekaJointInterface::incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state){

    //incoming controller status

    for (unsigned int i = 0; i<control_state.group_name.size(); i++){

        if (control_state.group_name[i] == "head"){

            //enable/disable based on controller status

            if (control_state.state[i] == m3meka_msgs::M3ControlStates::START){

                //controller up and running

                if (!controller_enabled){

                    printf("> incoming control state (%d), enabling jointstate output\n",control_state.state[i]);

                    controller_enabled = true;

                }

            }else{

                //controller in estop/stopped etc

                if (controller_enabled){

                    printf("> incoming control state (%d), DISABLING jointstate output\n",control_state.state[i]);

                    controller_enabled = false;

                }

            }

        }

    }

}

void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){

    //fetch current timestamp

    Timestamp timestamp = Timestamp(msg.header.stamp.toSec());

    //iterate through incoming joints and filter out joints we need:

    for(int i=0; i<msg.name.size(); i++){

        std::string name = msg.name[i];

        //printf("incoming data for joint '%s'\n", name.c_str());

        int id = -1;

        if (name == "head_j1"){

            id = ID_NECK_PAN;

        }else if(name == "head_j0"){

            id = ID_NECK_TILT;

        }

        //store data:

        if (id != -1){

            //printf("> storing joint data for joint id %d\n", id);

            if (i >= msg.position.size()){

                printf("> joint state msg is missing position data for joint '%s'...\n", name.c_str());

                return;

            }

            if (i >= msg.velocity.size()){

                //printf("> joint state msg is missing velocity data for joint '%s'...\n", name.c_str());

                //exit(EXIT_FAILURE);

                return;

            }

            //ok, safe to access data

            if (id == ID_NECK_PAN){

                //joint is inverted

                JointInterface::store_incoming_position(id, -180.0 / M_PI * msg.position[i], timestamp);

                JointInterface::store_incoming_velocity(id, -180.0 / M_PI * msg.velocity[i], timestamp);

            }else if (id == ID_NECK_TILT){

                JointInterface::store_incoming_position(id, 180.0 / M_PI * msg.position[i], timestamp);

                JointInterface::store_incoming_velocity(id, 180.0 / M_PI * msg.velocity[i], timestamp);

            }

        }

    }

    //dummy data uses current time

    timestamp = Timestamp::now();

    //store dummy positions for joints we do not know about:

    store_dummy_data(ID_LIP_LEFT_UPPER, timestamp);

    store_dummy_data(ID_LIP_LEFT_LOWER, timestamp);

    store_dummy_data(ID_LIP_CENTER_UPPER, timestamp);

    store_dummy_data(ID_LIP_CENTER_LOWER, timestamp);

    store_dummy_data(ID_LIP_RIGHT_UPPER, timestamp);

    store_dummy_data(ID_LIP_RIGHT_LOWER, timestamp);

    store_dummy_data(ID_NECK_ROLL, timestamp);

    store_dummy_data(ID_EYES_BOTH_UD, timestamp);

    store_dummy_data(ID_EYES_LEFT_LR, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LR, timestamp);

    store_dummy_data(ID_EYES_LEFT_LID_LOWER, timestamp);

    store_dummy_data(ID_EYES_LEFT_LID_UPPER, timestamp);

    store_dummy_data(ID_EYES_LEFT_BROW, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LID_LOWER, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LID_UPPER, timestamp);