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