/ - Diff - humotion - Research for Cognitive Interaction

Revision a4795834

     //class MekaDataReceiver;
     #include "ros/ros.h"
     #include "sensor_msgs/JointState.h"
     #include "trajectory_msgs/JointTrajectory.h"
     class MekaJointInterface : public humotion::server::JointInterface{
     public:
-...
     private:
         void incoming_jointstates(const sensor_msgs::JointState & msg);
         void store_dummy_data(int id, double timestamp);
         void store_min_max(int id, float min, float max);
         ros::Subscriber joint_state_subscriber;
         ros::Publisher target_publisher;
         double get_timestamp_ms();

     */
         //create humotion interface
         string scope = "/";
         string scope = "/xsc3/";
         MekaJointInterface *jointinterface = new MekaJointInterface(scope);
         humotion::server::Server *humotion_server = new humotion::server::Server("/meka", "ROS", jointinterface);
         humotion::server::Server *humotion_server = new humotion::server::Server("/flobi", "ROS", jointinterface);
         //finallz run it
         //finally run it
         jointinterface->run();
         while(humotion_server->ok()){

     #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
-...
     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
-...
     //! 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){
-...
             */
+    }
     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:

Also available in: Unified diff