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