| 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 |
155 |
printf("> WARNING: restored/guessed absolute target for unknown timestamp %f "
|
| 156 |
|
"[this should not happen]\n", relative_target_timestamp.to_seconds());
|
|
156 |
"[this should only happen during startup]\n", relative_target_timestamp.to_seconds());
|
| 157 |
157 |
last_known_absolute_target_pan_ = 0.0;
|
| 158 |
158 |
last_known_absolute_target_tilt_ = 0.0;
|
| 159 |
159 |
last_known_absolute_target_roll_ = 0.0;
|
| ... | ... | |
| 196 |
196 |
absolute_gaze.pan = pan + relative.pan;
|
| 197 |
197 |
absolute_gaze.tilt = tilt + relative.tilt;
|
| 198 |
198 |
absolute_gaze.roll = roll + relative.roll;
|
| 199 |
|
//printf("pan now = %4.1f, rel=%4.1f ===> %4.2f\n", pan, relative.pan, absolute_gaze.pan);
|
| 200 |
|
//printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n", tilt, relative.tilt, absolute_gaze.tilt);
|
|
199 |
// printf("pan now = %4.1f, rel=%4.1f ===> %4.2f\n", pan, relative.pan, absolute_gaze.pan);
|
|
200 |
// printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n", tilt, relative.tilt, absolute_gaze.tilt);
|
| 201 |
201 |
|
| 202 |
202 |
// store debug data:
|
| 203 |
203 |
store_debug_data("controller/last_known_absolute_target_pan", last_known_absolute_target_pan_);
|
| 204 |
204 |
store_debug_data("controller/pan", pan);
|
| 205 |
205 |
store_debug_data("controller/neck_pan", neck_pan);
|
| 206 |
206 |
store_debug_data("controller/absolute_gaze_pan", absolute_gaze.pan);
|
| 207 |
|
// store_debug_data("controller/relative_timestamp", relative_target_timestamp.to_seconds());
|
| 208 |
|
// printf("BEFORE DBG %f\n", relative.timestamp.to_seconds());
|
| 209 |
|
std::srand(std::time(0)); // use current time as seed for random generator
|
| 210 |
|
int random_variable = std::rand();
|
| 211 |
|
store_debug_data("controller/relative_timestamp2", random_variable);
|
| 212 |
|
printf("DBG %i\n", random_variable);
|
|
207 |
|
| 213 |
208 |
// FIXME: use ros TF for that calculation...
|
| 214 |
209 |
// see http://wiki.ros.org/tf/Tutorials/Time%20travel%20with%20tf%20%28C%2B%2B%29
|
| 215 |
210 |
// ros::Time past = now - ros::Duration(5.0);
|