Revision dbf66483 src/server/controller.cpp
| 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