Revision 01ff8464
| server/include/ROS/LookatCallbackWrapperROS.h | ||
|---|---|---|
| 39 | 39 |
hlrc_server::lookattargetFeedback feedback; |
| 40 | 40 |
hlrc_server::lookattargetResult result; |
| 41 | 41 |
const tf2_ros::Buffer& tfBuffer; |
| 42 |
const std::string FLOBI_BASE_LINK="BASE_LINK"; |
|
| 42 |
const std::string FLOBI_BASE_LINK="floka_BASE_LINK";
|
|
| 43 | 43 |
// pan/tilt will be computed w.r.t. to virtual frame at height of eyes |
| 44 | 44 |
Eigen::Affine3d eig_eyes_to_base; |
| 45 | 45 |
double eye_distance; // distance from center of eyes to eye frame |
| ... | ... | |
| 52 | 52 |
{
|
| 53 | 53 |
// eyes frame has z-axis pointing along view dir (90° rotation w.r.t. base) |
| 54 | 54 |
// and z-position is shifted to height of eyes |
| 55 |
eig_eyes_to_base = (Eigen::Translation3d(0.070621, 0, 0.2006)
|
|
| 55 |
eig_eyes_to_base = (Eigen::Translation3d(0.078, 0, 0.188)
|
|
| 56 | 56 |
* Eigen::AngleAxisd(angles::from_degrees(90), Eigen::Vector3d::UnitY()) |
| 57 | 57 |
* Eigen::AngleAxisd(angles::from_degrees(180), Eigen::Vector3d::UnitZ())).inverse(); |
| 58 |
eye_distance = 0.143 / 2; |
|
| 58 |
//eig_eyes_to_base = Eigen::Translation3d(0.070621, 0, 0.2006).inverse(); |
|
| 59 |
|
|
| 60 |
//std::cout << "MATRIX" << eig_eyes_to_base << std::endl; |
|
| 61 |
//* Eigen::AngleAxisd(angles::from_degrees(90), Eigen::Vector3d::UnitY()) |
|
| 62 |
//* Eigen::AngleAxisd(angles::from_degrees(180), Eigen::Vector3d::UnitZ())).inverse(); |
|
| 63 |
eye_distance = 0.069; |
|
| 59 | 64 |
} |
| 60 | 65 |
|
| 61 | 66 |
void call(const GoalConstPtr &goal){
|
| ... | ... | |
| 72 | 77 |
tfBuffer.lookupTransform(FLOBI_BASE_LINK, p.header.frame_id, |
| 73 | 78 |
p.header.stamp, ros::Duration(0.1)); |
| 74 | 79 |
|
| 75 |
target = eig_eyes_to_base * (tf2::transformToEigen(base_to_source) * target); |
|
| 80 |
target = eig_eyes_to_base * (tf2::transformToEigen(base_to_source) * target); |
|
| 81 |
|
|
| 76 | 82 |
} else {
|
| 77 | 83 |
// otherwise we interpret target directly w.r.t. eye center |
| 84 |
ROS_WARN("NO Frame provided with target!");
|
|
| 78 | 85 |
} |
| 79 | 86 |
} catch (const std::exception &e) {
|
| 80 | 87 |
// Failed to resolve tf |
| ... | ... | |
| 93 | 100 |
// normalize vector from eyes-center to target |
| 94 | 101 |
target /= distance; |
| 95 | 102 |
// pan = rotation about x-axis: angle of projection of target onto yz-plane to z-axis |
| 96 |
gaze_state.pan = 90 - angles::to_degrees(atan2(target.z(), target.y()));
|
|
| 103 |
gaze_state.pan = (90 - angles::to_degrees(atan2(target.z(), target.y())));
|
|
| 97 | 104 |
// tilt = rotation about y-axis: angle between target and yz-plane |
| 98 | 105 |
Eigen::Vector2d projection(target.y(), target.z()); |
| 99 | 106 |
gaze_state.tilt = angles::to_degrees(atan2(target.x(), projection.norm())); |
| 100 | 107 |
} |
| 101 | 108 |
// fill in rest of gaze_state |
| 102 | 109 |
gaze_state.roll = request->roll; |
| 103 |
gaze_state.vergence = angles::to_degrees(atan2(eye_distance, distance)); |
|
| 110 |
gaze_state.vergence = -angles::to_degrees(atan2(eye_distance, distance));
|
|
| 104 | 111 |
|
| 105 | 112 |
// use timestamp from request |
| 106 | 113 |
gaze_state.timestamp.set(p.header.stamp.sec, p.header.stamp.nsec); |
Also available in: Unified diff