/ - Diff - humotion - Research for Cognitive Interaction

     //! \param MouthState m to set
     //! \param send data to server (optional, use manual call to send_*() to trigger update on server)
     void Middleware::update_mouth_target(MouthState m, bool send){
     	mouth_state = m;
     	if (send){
     		send_mouth_target();
+    	}
         mouth_state = m;
         if (send){
             send_mouth_target();
+        }
+    }
     //! set gaze target
-...
     //! send all targets to server
     void Middleware::send_all(){
     	send_mouth_target();
         send_mouth_target();
         send_gaze_target();
+    }

             tick_necessary = false;
+        }
     	//create node handle
         //create node handle
         ros::NodeHandle n;
     	//set up publishers:
         //set up publishers:
         mouth_target_publisher = n.advertise<humotion::mouth>(base_scope + "/humotion/mouth/target", 100);
         gaze_target_publisher  = n.advertise<humotion::gaze>(base_scope + "/humotion/gaze/target", 100);

     //! publish targets to motor boards:
     void EyebrowMotionGenerator::publish_targets(){
        //publish values if there is an active gaze input within the last timerange
        if (gaze_target_input_active()){
         joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_BROW);
         joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_BROW);
+       }
         //publish values if there is an active gaze input within the last timerange
         if (gaze_target_input_active()){
             joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_BROW);
             joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_BROW);
+        }
+    }

         handle_eyeblink_timeout();
         //eyeblinks override position target (if necessary)
         override_lids_for_eyeblink();
         override_lids_for_eyeblink();
+    }
     //! process any external blink requests
     void EyelidMotionGenerator::start_external_eyeblinks(int duration_left, int duration_right){
         //manual eyeblinks will ALWAYs get executed as we use a negative block timeout (=timout has already passed)
         if ((duration_left != 0) || (duration_right != 0)){
     	eyeblink_blocked_timeout = get_system_time() - posix_time::seconds(100);
             eyeblink_blocked_timeout = get_system_time() - posix_time::seconds(100);
+        }
-...
         //set upper to minimal allowed value (=as closed as possible) + a small offset
         //set lower to the same value (instead of max) in order to avoid collisions
         switch(joint_id){
             default:
                 //no eyelid -> return
                 return;
             case(JointInterface::ID_EYES_LEFT_LID_UPPER):
             case(JointInterface::ID_EYES_LEFT_LID_LOWER):
                 //use the upper value + 10 deg as close state:
                 value = joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LID_UPPER) + 10.0;
                 //overwrite last target_
                 joint_interface->set_target_position(joint_id, value);
                 break;
             case(JointInterface::ID_EYES_RIGHT_LID_UPPER):
             case(JointInterface::ID_EYES_RIGHT_LID_LOWER):
                 //use the upper value + 10 deg as close state:
                 value = joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LID_UPPER) + 10.0;
                 //overwrite last target_
                 joint_interface->set_target_position(joint_id, value);
                 break;
         default:
             //no eyelid -> return
             return;
         case(JointInterface::ID_EYES_LEFT_LID_UPPER):
         case(JointInterface::ID_EYES_LEFT_LID_LOWER):
             //use the upper value + 10 deg as close state:
             value = joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LID_UPPER) + 10.0;
             //overwrite last target_
             joint_interface->set_target_position(joint_id, value);
             break;
         case(JointInterface::ID_EYES_RIGHT_LID_UPPER):
         case(JointInterface::ID_EYES_RIGHT_LID_LOWER):
             //use the upper value + 10 deg as close state:
             value = joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LID_UPPER) + 10.0;
             //overwrite last target_
             joint_interface->set_target_position(joint_id, value);
             break;
+        }
+    }

         float ud_max    = OMR_LIMIT_TRIGGERS_NECK_SACCADE * joint_interface->get_joint_max(JointInterface::ID_EYES_BOTH_UD);
         if (
             (eye_target_l < left_min) || (eye_target_l > left_max) ||
             (eye_target_r < right_min) || (eye_target_r > right_max) ||
             (eye_target_ud < ud_min) || (eye_target_ud > ud_max)){
                 (eye_target_l < left_min) || (eye_target_l > left_max) ||
                 (eye_target_r < right_min) || (eye_target_r > right_max) ||
                 (eye_target_ud < ud_min) || (eye_target_ud > ud_max)){
             //the eyeball gets close to OMR, activate a neck compensation motion:
             neck_saccade_omr = true;
         }else{

         gaze_state.eyeblink_request_right = msg->eyeblink_request_right();
         gaze_state.timestamp = msg->gaze_timestamp(); FIXME: convert RSB timestamp to our representation
         //event->getMetaData().getCreateTime() / 1000.0; //createTime() returns milliseconds -> convert to seconds
                                                              //event->getMetaData().getCreateTime() / 1000.0; //createTime() returns milliseconds -> convert to seconds
         controller->set_gaze_target(gaze_state);
                                                              controller->set_gaze_target(gaze_state);
+    }
     #endif

     //! \param int enum with joint id
     float MouthMotionGenerator::mouthstate_to_opening(MouthState m, int e){
         switch (e){
             default: printf("> get_opening(0x%02X): invalid joint enum!\n",e); exit(0);
             case(JointInterface::ID_LIP_LEFT_UPPER):
             case(JointInterface::ID_LIP_LEFT_LOWER):
                 return m.opening_left;
             case(JointInterface::ID_LIP_CENTER_UPPER):
             case(JointInterface::ID_LIP_CENTER_LOWER):
                 return m.opening_center;
             case(JointInterface::ID_LIP_RIGHT_UPPER):
             case(JointInterface::ID_LIP_RIGHT_LOWER):
                 return m.opening_right;
         default: printf("> get_opening(0x%02X): invalid joint enum!\n",e); exit(0);
         case(JointInterface::ID_LIP_LEFT_UPPER):
         case(JointInterface::ID_LIP_LEFT_LOWER):
             return m.opening_left;
         case(JointInterface::ID_LIP_CENTER_UPPER):
         case(JointInterface::ID_LIP_CENTER_LOWER):
             return m.opening_center;
         case(JointInterface::ID_LIP_RIGHT_UPPER):
         case(JointInterface::ID_LIP_RIGHT_LOWER):
             return m.opening_right;
+        }
+    }
-...
     //! \param int enum with joint id
     float MouthMotionGenerator::mouthstate_to_position(MouthState m, int e){
         switch (e){
             default: printf("> get_position(0x%02X): invalid joint enum!\n",e); exit(0);
             case(JointInterface::ID_LIP_LEFT_UPPER):
             case(JointInterface::ID_LIP_LEFT_LOWER):
                 return m.position_left;
             case(JointInterface::ID_LIP_CENTER_UPPER):
             case(JointInterface::ID_LIP_CENTER_LOWER):
                 return m.position_center;
             case(JointInterface::ID_LIP_RIGHT_UPPER):
             case(JointInterface::ID_LIP_RIGHT_LOWER):
                 return m.position_right;
         default: printf("> get_position(0x%02X): invalid joint enum!\n",e); exit(0);
         case(JointInterface::ID_LIP_LEFT_UPPER):
         case(JointInterface::ID_LIP_LEFT_LOWER):
             return m.position_left;
         case(JointInterface::ID_LIP_CENTER_UPPER):
         case(JointInterface::ID_LIP_CENTER_LOWER):
             return m.position_center;
         case(JointInterface::ID_LIP_RIGHT_UPPER):
         case(JointInterface::ID_LIP_RIGHT_LOWER):
             return m.position_right;
+        }
+    }

             unsigned int incoming_data_count = joint_interface->get_and_clear_incoming_position_count();
             if (incoming_data_count == 0){
                     incoming_data_count_invalid++;
                     if (incoming_data_count_invalid >= MOTION_UPDATERATE){
                         printf("> waiting for valid incoming joint data (position+velocity)\n");
                         incoming_data_count_invalid = 0;
+                    }
                 incoming_data_count_invalid++;
                 if (incoming_data_count_invalid >= MOTION_UPDATERATE){
                     printf("> waiting for valid incoming joint data (position+velocity)\n");
                     incoming_data_count_invalid = 0;
+                }
             }else{
                 //fine, joint data is arriving, exit waiting loop
                 break;
-...
             //check if we received joint information in the last step:
             unsigned int incoming_data_count = joint_interface->get_and_clear_incoming_position_count();
             if (incoming_data_count == 0){
                     //printf("> WARNING: no incoming joint data during the last iteration...\n");
                     incoming_data_count_invalid++;
                     if (incoming_data_count_invalid >= MOTION_UPDATERATE){
                         printf("> ERROR: no incoming joint data for >1 second -> will exit now\n");
                         exit(EXIT_FAILURE);
+                    }
                 //printf("> WARNING: no incoming joint data during the last iteration...\n");
                 incoming_data_count_invalid++;
                 if (incoming_data_count_invalid >= MOTION_UPDATERATE){
                     printf("> ERROR: no incoming joint data for >1 second -> will exit now\n");
                     exit(EXIT_FAILURE);
+                }
             }else{
                     incoming_data_count_invalid = 0;
                 incoming_data_count_invalid = 0;
+            }
             thread::sleep(timeout);

     using namespace humotion;
     TimestampedList::TimestampedList(unsigned int s){
     	//initialize the list to its desired size:
         //initialize the list to its desired size:
         TimestampedFloat now;
         tsf_list.resize(s, now);
+    }
-...
+    }
     void TimestampedList::insert(Timestamp timestamp, float val){
     	//erase first element:
         //erase first element:
         tsf_list.pop_front();
         tsf_list.push_back(TimestampedFloat(timestamp, val));
         //printf("insert [%5.3f] = %5.1f\n",timestamp,val);
-...
+        }
         timestamped_float_list_t::iterator it = tsf_list.end();
     	it--;
     	return it->value;
         it--;
         return it->value;
+    }
     float TimestampedList::get_interpolated_value(Timestamp target_ts){
-...
         mutex::scoped_lock scoped_lock(access_mutex);
         ///target_ts -= 0.001 * 4;
         TimestampedFloat previous;
         //printf("> latency %3.2fms\n", (Timestamped().to_seconds() - target_ts.to_seconds())*1000.0);
         for ( timestamped_float_list_t::iterator it = tsf_list.begin(); it != tsf_list.end(); ++it ){
     		if (it->timestamp >= target_ts){
     			//ok found close target
             if (it->timestamp >= target_ts){
                 //ok found close target
                 if (it == tsf_list.begin()){
     				//no preceding element
                     //no preceding element
                     printf("> warning, timestamp %6.3f smaller than first element %6.3f in timestamped list. this should not happen (increase ts buffer?)\n",target_ts.to_seconds(),tsf_list.begin()->timestamp.to_seconds());
     				return it->value;
     			}else{
     				//do interpolation
     				return interpolate(*it, previous, target_ts);
+    			}
+    		}
     		previous = *it;
+    	}
     	//we reached the end, return the last value
                     return it->value;
                 }else{
                     //do interpolation
                     return interpolate(*it, previous, target_ts);
+                }
+            }
             previous = *it;
+        }
         //we reached the end, return the last value
         printf("> warning: found no timestamp bigger than %f in timestamped list...\n", target_ts.to_seconds());
     	printf("           this should not happen as images will always be behind\n");
         printf("           this should not happen as images will always be behind\n");
         printf("           the motor data. returning most recent value (ts=%f)\n", previous.timestamp.to_seconds());
     	return previous.value;
         return previous.value;
+    }
     float TimestampedList::interpolate(TimestampedFloat a, TimestampedFloat b, Timestamp timestamp){
     	//a->timestamp < timestamp <= b->timestamp
         //a->timestamp < timestamp <= b->timestamp
         double dist_a = timestamp.to_seconds() - a.timestamp.to_seconds();
         double dist_b = b.timestamp.to_seconds() - timestamp.to_seconds();
     	double dist   = dist_a + dist_b;
         double dist   = dist_a + dist_b;
         float interpolation = a.value + (dist_a / dist) * (b.value - a.value);
     	return interpolation;
         return interpolation;
+    }
     ///tests
     void TimestampedList::run_tests(){
     	int size = 10;
     	TimestampedList list(size);
     	for(int i=0; i<size; i++){
         int size = 10;
         TimestampedList list(size);
         for(int i=0; i<size; i++){
             list.insert(Timestamp(i * 100.0,0), i*10.0);
+    	}
     	//test algorithm:
     	//test exact match:
     	for(int i=0; i<size; i++){
+        }
         //test algorithm:
         //test exact match:
         for(int i=0; i<size; i++){
             Timestamp ts(i*100.0,0);
             printf("> testing get_interpolated_value(%f) == %f (value read back = %f)\n",ts.to_seconds(), i*10.0, list.get_interpolated_value(ts));
             assert( list.get_interpolated_value(ts) == i*10.0);
+    	}
     	printf("passed test 1\n");
+        }
         printf("passed test 1\n");
         assert(list.get_interpolated_value(Timestamp(0.0,0)) == 0.0);
         assert(list.get_interpolated_value(Timestamp(110.0,0)) == 11.0);
         assert(list.get_interpolated_value(Timestamp(150.0,0)) == 15.0);
         assert(list.get_interpolated_value(Timestamp(999990.0,0)) == 90.0);
     	printf("passed test 2\n");
         printf("passed test 2\n");
         list.insert(Timestamp(1000.0 ,0), 200.0);
         list.insert(Timestamp(1300.0, 0), -100.0);
         assert(list.get_interpolated_value(Timestamp(1100, 0)) == 100.0);
         assert(list.get_interpolated_value(Timestamp(1200, 0)) == 0.0);
         assert(list.get_interpolated_value(Timestamp(1300, 0)) == -100.0);
         assert(list.get_interpolated_value(Timestamp(1250, 0)) == -50.0);
     	printf("passed test 3\n");
     	exit(0);
         printf("passed test 3\n");
         exit(0);
+    }

humotion

Revision 7ed40bef