humotion

#pragma once

#include <humotion/server/joint_interface.h>

#include <humotion/server/server.h>

#include <boost/bimap.hpp>

//#include "meka_data_receiver.h"

//class MekaDataReceiver;

#include "ros/ros.h"

#include "sensor_msgs/JointState.h"

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

public:

    MekaJointInterface(std::string 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();

    enum JOINT_ID_ENUM{

        ICUB_ID_NECK_TILT = 0,

        ICUB_ID_NECK_ROLL = 1,

        ICUB_ID_NECK_PAN  = 2,

        ICUB_ID_EYES_BOTH_UD = 3,

        ICUB_ID_EYES_PAN = 4,

        ICUB_ID_EYES_VERGENCE = 5,

        //the following ids are used for remapping:

        ICUB_ID_EYES_LEFT_LR,

        ICUB_ID_EYES_RIGHT_LR,

        ICUB_ID_EYES_LEFT_LID_LOWER,

        ICUB_ID_EYES_LEFT_LID_UPPER,

        ICUB_ID_EYES_RIGHT_LID_LOWER,

        ICUB_ID_EYES_RIGHT_LID_UPPER,

        ICUB_ID_EYES_LEFT_BROW,

        ICUB_ID_EYES_RIGHT_BROW,

        ICUB_ID_LIP_LEFT_UPPER,

        ICUB_ID_LIP_LEFT_LOWER,

        ICUB_ID_LIP_CENTER_UPPER,

        ICUB_ID_LIP_CENTER_LOWER,

        ICUB_ID_LIP_RIGHT_UPPER,

        ICUB_ID_LIP_RIGHT_LOWER,

        ICUB_JOINT_ID_ENUM_SIZE

    };

    static const int MAIN_LOOP_FREQUENCY = 50;

protected:

    void disable_joint(int e);

    void publish_target_position(int e);

    void enable_joint(int e);

    void execute_motion();

private:

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

    ros::Subscriber joint_state_subscriber;

    double get_timestamp_ms();

    double target_angle[ICUB_JOINT_ID_ENUM_SIZE];

    double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];

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

    float last_pos_eye_vergence;

    float last_pos_eye_pan;

    float last_vel_eye_vergence;

    float last_vel_eye_pan;

    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;

};