Revision 60b91de0 server/include/ROS/LookatCallbackWrapperROS.h
| server/include/ROS/LookatCallbackWrapperROS.h | ||
|---|---|---|
| 62 | 62 |
feedback.result = 1; |
| 63 | 63 |
|
| 64 | 64 |
hlrc_server::lookattargetGoalConstPtr request = goal; |
| 65 |
const geometry_msgs::Point &p = request->point; |
|
| 65 |
const geometry_msgs::PointStamped &p = request->point;
|
|
| 66 | 66 |
|
| 67 |
Eigen::Vector3d target(p.x, p.y, p.z);
|
|
| 67 |
Eigen::Vector3d target(p.point.x, p.point.y, p.point.z);
|
|
| 68 | 68 |
try {
|
| 69 |
if (!request->header.frame_id.empty()) { // only consider tf, when frame_id is non-empty
|
|
| 69 |
if (!p.header.frame_id.empty()) { // only consider tf, when frame_id is non-empty
|
|
| 70 | 70 |
geometry_msgs::TransformStamped base_to_source = |
| 71 | 71 |
// lookupTransform can throw |
| 72 |
tfBuffer.lookupTransform(FLOBI_BASE_LINK, request->header.frame_id,
|
|
| 73 |
request->header.stamp, ros::Duration(0.1));
|
|
| 72 |
tfBuffer.lookupTransform(FLOBI_BASE_LINK, p.header.frame_id,
|
|
| 73 |
p.header.stamp, ros::Duration(0.1));
|
|
| 74 | 74 |
|
| 75 | 75 |
target = eig_eyes_to_base * (tf2::transformToEigen(base_to_source) * target); |
| 76 | 76 |
} else {
|
| ... | ... | |
| 103 | 103 |
gaze_state.vergence = angles::to_degrees(atan2(eye_distance, distance)); |
| 104 | 104 |
|
| 105 | 105 |
// use timestamp from request |
| 106 |
gaze_state.timestamp.set(request->header.stamp.sec, request->header.stamp.nsec);
|
|
| 106 |
gaze_state.timestamp.set(p.header.stamp.sec, p.header.stamp.nsec);
|
|
| 107 | 107 |
gaze_state.gaze_type = humotion::GazeState::GAZETYPE_ABSOLUTE; |
| 108 | 108 |
|
| 109 | 109 |
// processing |
Also available in: Unified diff