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

    first test version

diff --git a/examples/meka/src/mekajointinterface.cpp b/examples/meka/src/mekajointinterface.cpp
index f1884e2..635c633 100644
--- a/examples/meka/src/mekajointinterface.cpp
+++ b/examples/meka/src/mekajointinterface.cpp
@@ -5,110 +5,76 @@ using namespace std;
 #define POSITION_CONTROL 1
 
 void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){
-    //joint_pos_global = msg.position[3];
-    cout << msg;
+    //fetch current timestamp
+    double timestamp = get_timestamp_ms();
+
+    //iterate through incoming joints and filter out joints we need:
+    for(int i=0; i<msg.name.size(); i++){
+        string name = msg.name[i];
+        //printf("incoming data for joint '%s'\n", name.c_str());
+
+        int id = -1;
+        if (name == "NECK_PAN"){
+            id = ID_NECK_PAN;
+        }else if(name == "NECK_TILT"){
+            id = ID_NECK_TILT;
+        }
+
+        //store data:
+        if (id != -1){
+            //printf("> storing joint data for joint id %d\n", id);
+            JointInterface::store_incoming_position(id, msg.position[i], timestamp);
+            JointInterface::store_incoming_speed(   id, msg.velocity[i], timestamp);
+        }
+    }
+
+    //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);
+    store_dummy_data(ID_EYES_RIGHT_BROW, timestamp);
+
+}
+
+void MekaJointInterface::store_dummy_data(int id, double timestamp){
+    JointInterface::store_incoming_position(id, 0.0, timestamp);
+    JointInterface::store_incoming_speed(id, 0.0, timestamp);
 }
 
 //! constructor
 MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointInterface(){
     scope = _scope;
+    string js_topic = scope + "joint_states";
+    string target_topic = scope + "meka_roscontrol/head_position_trajectory_controller/command";
 
     //subscribe to meka joint states
     int argc = 0;
     ros::init(argc, (char**)NULL, "meka_humotion");
     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
-    //to access the joints. now we need to define a mapping to map those to our motor ids.
-    //this is what we use the enum bimap for (convertion fro/to motorid is handled
-    //by \sa convert_enum_to_motorid() and \sa convert_motorid_to_enum() lateron
-/*
-    //MOUTH
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER));
-
-    //NECK
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_PAN,    ID_NECK_PAN));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_TILT,   ID_NECK_TILT));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL));
-
-    //EYES
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LR,   ID_EYES_LEFT_LR));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LR,   ID_EYES_RIGHT_LR));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD));
-
-    //EYELIDS
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW));
-
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW));
-
-    Property options;
-    options.put("device", "remote_controlboard");
-    options.put("local", "/local/head");
-    options.put("remote", scope+"/head");
-    dd.open(options);
-
-    //fetch views:
-    dd.view(iencs);
-    dd.view(ipos);
-    dd.view(ivel);
-    dd.view(ilimits);
-
-    if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel)){
-        printf("> ERROR: failed to open icub views\n");
-        exit(EXIT_FAILURE);
-    }
+    printf("> listening on jointstates on '%s'\n",js_topic.c_str());
+    joint_state_subscriber = n.subscribe(js_topic, 150, &MekaJointInterface::incoming_jointstates , this);
+
+    printf("> sending targets on '%s'\n", target_topic.c_str());
+    target_publisher = n.advertise<trajectory_msgs::JointTrajectory>(target_topic, 100);
 
-    int joints;
 
     //tell humotion about min/max joint values:
     init_joints();
-
-    iencs->getAxes(&joints);
-    positions.resize(joints);
-    velocities.resize(joints);
-    commands.resize(joints);
-
-    //set position mode:
-    if (POSITION_CONTROL){
-        commands=200000.0;
-        ipos->setRefAccelerations(commands.data());
-        ipos->setPositionMode();
-    }else{
-        ivel->setVelocityMode();
-        commands=1000000;
-        ivel->setRefAccelerations(commands.data());
-    }
-
-    //attach to facial expressions:
-    string emotion_scope = scope + "/face/raw/in";
-    printf("> opening connection to %s\n", emotion_scope.c_str());
-
-    for(int i=0; i<4; i++){
-        //strange, if we use one output port only the first command is executed?! flushing issues?
-        string emotion_port_out = "/emotionwriter" + to_string(i);
-        if (!emotion_port[i].open(emotion_port_out.c_str())){
-            printf("> ERROR: failed to open to %s\n",emotion_port_out.c_str());
-            exit(EXIT_FAILURE);
-        }
-        if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())){
-            printf("> ERROR: failed to connect emotion ports\n");
-            exit(EXIT_FAILURE);
-        }
-    }
-    */
 }
 
 //! destructor
@@ -120,9 +86,16 @@ MekaJointInterface::~MekaJointInterface(){
 void MekaJointInterface::run(){
    //iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
    //data_receiver->start();
+   ros::spin();
 }
 
 
+double MekaJointInterface::get_timestamp_ms(){
+    struct timespec spec;
+    clock_gettime(CLOCK_REALTIME, &spec);
+    return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;
+}
+
 //! set the target position of a joint
 //! \param enum id of joint
 //! \param float value
@@ -154,6 +127,44 @@ void MekaJointInterface::publish_target_position(int e){
 
 //! actually execute the scheduled motion commands
 void MekaJointInterface::execute_motion(){
+    //build msg
+    trajectory_msgs::JointTrajectory msg;
+    msg.joint_names.push_back("head_j0");
+    msg.joint_names.push_back("head_j1");
+
+    trajectory_msgs::JointTrajectoryPoint p[2];
+    p[0].positions.push_back(joint_target[ID_NECK_PAN] * M_PI / 180.0);
+    p[1].positions.push_back(joint_target[ID_NECK_TILT]* M_PI / 180.0);
+
+    p[0].time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);
+    p[1].time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);
+
+    msg.points.push_back(p[0]);
+    msg.points.push_back(p[1]);
+
+    target_publisher.publish(msg);
+
+    /*
+     void MekaJointInterface::store_min_max(int id, float min, float max){
+header:
+  seq: 636
+  stamp:
+    secs: 0
+    nsecs: 0
+  frame_id: ''
+joint_names: ['head_j0', 'head_j1']
+points:
+  -
+    positions: [-0.31, 0.01954768762234005]
+    velocities: []
+    accelerations: []
+    effort: []
+    time_from_start:
+      secs: 1
+      nsecs: 0
+
+     */
+
     #if 0
     // set up neck and eye motion commands:
     if (POSITION_CONTROL){
@@ -252,6 +263,33 @@ void MekaJointInterface::disable_joint(int e){
         */
 }
 
+void MekaJointInterface::store_min_max(int id, float min, float max){
+    joint_min[id] = min;
+    joint_max[id] = max;
+}
+
+void MekaJointInterface::init_joints(){
+    store_min_max(ID_NECK_TILT, -10, 10);
+    store_min_max(ID_NECK_PAN, -10, 10);
+
+    store_min_max(ID_NECK_ROLL, -1, 1);
+    store_min_max(ID_EYES_BOTH_UD, -1, 1);
+    store_min_max(ID_EYES_LEFT_LR, -1, 1);
+    store_min_max(ID_EYES_RIGHT_LR, -1, 1);
+    store_min_max(ID_EYES_LEFT_LID_UPPER, -1, 1);
+    store_min_max(ID_EYES_LEFT_LID_LOWER, -1, 1);
+    store_min_max(ID_EYES_RIGHT_LID_UPPER, -1, 1);
+    store_min_max(ID_EYES_RIGHT_LID_LOWER, -1, 1);
+    store_min_max(ID_EYES_LEFT_BROW, -1, 1);
+    store_min_max(ID_EYES_RIGHT_BROW, -1, 1);
+    store_min_max(ID_LIP_CENTER_UPPER, -1, 1);
+    store_min_max(ID_LIP_CENTER_LOWER, -1, 1);
+    store_min_max(ID_LIP_LEFT_UPPER, -1, 1);
+    store_min_max(ID_LIP_LEFT_LOWER, -1, 1);
+    store_min_max(ID_LIP_RIGHT_UPPER, -1, 1);
+    store_min_max(ID_LIP_RIGHT_LOWER, -1, 1);
+}
+
 #if 0
 
 //! conversion table for humotion motor ids to our ids:
