commit 473a6a6cb26be5f06d683a6815e7a23fbba5daed
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Fri Nov 27 14:42:06 2015 +0100

    added package and msgs

diff --git a/examples/meka/src/mekajointinterface.cpp b/examples/meka/src/mekajointinterface.cpp
index 36a79b0..1ec959f 100644
--- a/examples/meka/src/mekajointinterface.cpp
+++ b/examples/meka/src/mekajointinterface.cpp
@@ -4,10 +4,20 @@ using namespace std;
 //WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
 #define POSITION_CONTROL 1
 
+void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){
+    //joint_pos_global = msg.position[3];
+    cout << msg;
+}
+
 //! constructor
 MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointInterface(){
     scope = _scope;
 
+    //subscribe to meka joint states
+    ros::NodeHandle n;
+    joint_state_subscriber = n.subscribe(scope + "joint_states", 150, &MekaJointInterface::incoming_jointstates , this);
+
+
     //add mapping from ids to enums:
     //this might look strange at the first sight but we need to have a generic
     //way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids
@@ -103,7 +113,144 @@ MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointI
 MekaJointInterface::~MekaJointInterface(){
 }
 
-/*
+
+
+void MekaJointInterface::run(){
+   //iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
+   //data_receiver->start();
+}
+
+
+//! set the target position of a joint
+//! \param enum id of joint
+//! \param float value
+void MekaJointInterface::publish_target_position(int e){
+    #if 0
+    //first: convert humotion enum to our enum:
+    int id = convert_enum_to_motorid(e);
+    if (id == -1){
+        return; //we are not interested in that data, so we just return here
+    }
+
+    if (id == ICUB_ID_NECK_PAN){
+        //PAN seems to be swapped
+        store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
+    }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
+        //icub handles eyes differently, we have to set pan angle + vergence
+        float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
+        float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);
+        //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
+
+        store_joint(ICUB_ID_EYES_PAN, pan);
+        store_joint(ICUB_ID_EYES_VERGENCE, vergence);
+    }else{
+        store_joint(id, joint_target[e]);
+    }
+    #endif
+}
+
+
+//! actually execute the scheduled motion commands
+void MekaJointInterface::execute_motion(){
+    #if 0
+    // set up neck and eye motion commands:
+    if (POSITION_CONTROL){
+        //position control
+        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
+            set_target_in_positionmode(i, target_angle[i]);
+        }
+    }else{
+        //velocity control
+        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
+            set_target_in_velocitymode(i, target_angle[i]);
+        }
+    }
+    //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
+
+
+    //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
+    set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
+
+    //eyebrows are set using a special command as well:
+    set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);
+    set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);
+
+    //mouth
+    set_mouth();
+
+    #endif
+}
+
+
+//! prepare and enable a joint
+//! NOTE: this should also prefill the min/max positions for this joint
+//! \param the enum id of a joint
+void MekaJointInterface::enable_joint(int e){
+#if 0
+    //FIXME ADD THIS:
+    // enable the amplifier and the pid controller on each joint
+    /*for (i = 0; i < nj; i++) {
+       amp->enableAmp(i);
+       pid->enablePid(i);
+    }*/
+
+
+    //set up smooth motion controller
+    //step1: set up framerate
+    //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
+
+    //step2: set controllertype:
+    //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
+    //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
+
+    //step3: set pid controller:
+    /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
+        printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
+        dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
+    }*/
+
+    //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
+
+    //check if setting pid controllertype was successfull:
+    /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
+        printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
+        exit(1);
+    }*/
+
+    //fetch min/max:
+   // init_joint(e);
+
+    //ok fine, now enable motor:
+    //printf("> enabling motor %s\n", joint_name.c_str());
+    //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
+#endif
+}
+
+//! shutdown and disable a joint
+//! \param the enum id of a joint
+void MekaJointInterface::disable_joint(int e){
+    /*
+        //first: convert humotion enum to our enum:
+        int motor_id = convert_enum_to_motorid(e);
+        if (motor_id == -1){
+            return; //we are not interested in that data, so we just return here
+        }
+
+        //fetch device:
+        Device *dev = get_device(motor_id);
+        printf("> FIXME: ADD DISABLE CODE\n");
+        printf("> FIXME: ADD DISABLE CODE\n");
+        printf("> FIXME: ADD DISABLE CODE\n");
+        printf("> FIXME: ADD DISABLE CODE\n");
+        printf("> FIXME: ADD DISABLE CODE\n");
+        printf("> FIXME: ADD DISABLE CODE\n");
+        printf("> FIXME: ADD DISABLE CODE\n");
+        printf("> FIXME: ADD DISABLE CODE\n");
+        printf("> FIXME: ADD DISABLE CODE\n");
+        */
+}
+
+#if 0
 
 //! conversion table for humotion motor ids to our ids:
 //! \param enum from JointInterface::JOINT_ID_ENUM
@@ -704,3 +851,4 @@ void iCubJointInterface::disable_joint(int e){
         printf("> FIXME: ADD DISABLE CODE\n");
         */
 }
+#endif
