Revision caf7373f src/server/controller.cpp

View differences:

src/server/controller.cpp
88 88
}
89 89

  
90 90
GazeState Controller::relative_gaze_to_absolute_gaze(GazeState relative){
91
    double pan, tilt, roll;
91 92
    GazeState absolute_gaze = relative;
92 93

  
93 94
    //incoming gaze state wants to set a relative gaze angle
......
95 96
    //in time and find out where the head was pointing at that specific time:
96 97
    Timestamp relative_target_timestamp = relative.timestamp;
97 98

  
98
    //fetch head / camera pose during that timestamp:
99
    double neck_pan  = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);
100
    double eye_l_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);
101
    double eye_r_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);
102
    double pan       = neck_pan + (eye_l_pan + eye_r_pan)/2.0;
103
    //
104
    double neck_tilt = joint_interface->get_ts_position(JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);
105
    double eye_tilt  = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);
106
    double tilt      = neck_tilt + eye_tilt;
107
    //
108
    double roll      = joint_interface->get_ts_position(JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);
99
    //check if this timestamp allows a valid conversion:
100
    Timestamp history_begin = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();
101
    Timestamp history_end   = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();
102

  
103
    //printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(), history_begin.to_seconds(), history_end.to_seconds());
104

  
105
    //our history keeps the last n=100 elements in a timestamped list
106
    if ((relative_target_timestamp < history_begin) || (history_begin.is_null())){
107
        //when the incoming data is older than that it makes no sense
108
        //to do any guesswork and try to calculate a valid absolute target
109
        //therefore we will use the last known targets (see below)
110
        //in case we did not see this timestamp before, show a warning:
111
        if (last_known_absolute_timestamp != relative_target_timestamp){
112
            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f [this should not happen]\n", relative_target_timestamp.to_seconds());
113
        }
114
    }else{
115
        //all fine, we can reconstruct the absolute target:
116
        //fetch head / camera pose during that timestamp:
117
        double neck_pan  = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);
118
        double eye_l_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);
119
        double eye_r_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);
120
        last_known_absolute_target_pan       = neck_pan + (eye_l_pan + eye_r_pan)/2.0;
121
        //
122
        double neck_tilt = joint_interface->get_ts_position(JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);
123
        double eye_tilt  = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);
124
        last_known_absolute_target_tilt      = neck_tilt + eye_tilt;
125
        //
126
        last_known_absolute_target_roll      = joint_interface->get_ts_position(JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);
127
        //safe this timestamp as known:
128
        last_known_absolute_timestamp = relative_target_timestamp;
129
    }
130

  
131
    pan  = last_known_absolute_target_pan;
132
    tilt = last_known_absolute_target_tilt;
133
    roll = last_known_absolute_target_roll;
109 134

  
110 135
    //substract offsets:
111
    pan -= relative.pan_offset;
136
    pan  -= relative.pan_offset;
112 137
    tilt -= relative.tilt_offset;
113 138
    roll -= relative.roll_offset;
114 139

  
......
126 151
    //ros::Time past = now - ros::Duration(5.0);
127 152
    //listener.waitForTransform("/turtle2", now,J "/turtle1", past, "/world", ros::Duration(1.0));
128 153
    //listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);
129
    absolute_gaze.dump();
154
    //absolute_gaze.dump();
155

  
130 156
    return absolute_gaze;
131 157
}
132 158

  

Also available in: Unified diff