Revision 60b91de0
| client/python/hlrc_client/MiddlewareROS.py | ||
|---|---|---|
| 31 | 31 |
import rospy |
| 32 | 32 |
import actionlib |
| 33 | 33 |
from hlrc_server.msg import * |
| 34 |
from geometry_msgs.msg import Point |
|
| 34 |
from geometry_msgs.msg import PointStamped
|
|
| 35 | 35 |
from actionlib_msgs.msg import GoalStatus |
| 36 | 36 |
|
| 37 | 37 |
class MiddlewareROS(Middleware): |
| ... | ... | |
| 210 | 210 |
|
| 211 | 211 |
# create a goal to send to the action server. |
| 212 | 212 |
goal = lookattargetGoal() |
| 213 |
goal.header.frame_id = frame_id |
|
| 214 |
goal.header.stamp = rospy.Time.now() |
|
| 215 |
|
|
| 216 |
p = Point() |
|
| 217 |
p.x = float(x) |
|
| 218 |
p.y = float(y) |
|
| 219 |
p.z = float(z) |
|
| 220 |
goal.point = p |
|
| 213 |
p = goal.point |
|
| 214 |
p.header.frame_id = frame_id |
|
| 215 |
p.header.stamp = rospy.Time.now() |
|
| 216 |
p.point.x = float(x) |
|
| 217 |
p.point.y = float(y) |
|
| 218 |
p.point.z = float(z) |
|
| 221 | 219 |
goal.roll = roll |
| 222 | 220 |
|
| 223 | 221 |
# send the goal to the server |
| server/action/lookattarget.action | ||
|---|---|---|
| 1 |
std_msgs/Header header |
|
| 2 |
geometry_msgs/Point point |
|
| 1 |
geometry_msgs/PointStamped point |
|
| 3 | 2 |
|
| 4 | 3 |
# roll angle |
| 5 | 4 |
float32 roll |
| 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