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