Revision dbf66483 src/server/controller.cpp

View differences:

src/server/controller.cpp
139 139
    // check if this timestamp allows a valid conversion:
140 140
    Timestamp history_begin =
141 141
            joint_interface_->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();
142
    // Timestamp history_end   =
143
    //       joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();
142
    Timestamp history_end   =
143
            joint_interface_->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();
144 144

  
145 145
    // printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(),
146 146
    // history_begin.to_seconds(), history_end.to_seconds());
......
152 152
        // therefore we will use the last known targets (see below)
153 153
        // in case we did not see this timestamp before, show a warning:
154 154
        if (last_known_absolute_timestamp_ != relative_target_timestamp) {
155
            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f "
156
                   "[this should only happen during startup]\n", relative_target_timestamp.to_seconds());
155
            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f (tsmap = [%f - %f])"
156
                   "[this should only happen during startup]\n", relative_target_timestamp.to_seconds(),
157
                   history_begin.to_seconds(),
158
                   history_end.to_seconds()
159
                   );
157 160
            last_known_absolute_target_pan_ = 0.0;
158 161
            last_known_absolute_target_tilt_ = 0.0;
159 162
            last_known_absolute_target_roll_ = 0.0;
......
199 202
    // printf("pan  now = %4.1f, rel=%4.1f ===> %4.2f\n", pan, relative.pan, absolute_gaze.pan);
200 203
    // printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n", tilt, relative.tilt, absolute_gaze.tilt);
201 204

  
205
    // update the timestamp. this gaze target is now absolute, set appropriate timestamp
206
    //printf("%f %f %f 333\n", absolute_gaze.pan, absolute_gaze.tilt, absolute_gaze.roll);
207
    //absolute_gaze.tilt = 0.0;
208
    //absolute_gaze.roll = 0.0;
209

  
210
    //absolute_gaze.roll = absolute_gaze.pan; //0.012;
211
    absolute_gaze.timestamp = Timestamp::now();
212

  
202 213
    // store debug data:
203 214
    // this is the position we had at the ts of the relative target
204
    store_debug_data("controller/pan_position_at_relative_ts", last_known_absolute_target_pan_);
215
    store_debug_data("controller/pan_neck_position_at_relative_ts", neck_pan);
216
    store_debug_data("controller/pan_overall_position_at_relative_ts", last_known_absolute_target_pan_);
205 217
    // this is the relative movement that was requested
206 218
    store_debug_data("controller/pan_target_relative", relative.pan);
207 219
    // this is the calculated overall target

Also available in: Unified diff