Revision 87c0fad1

View differences:

server/include/ROS/LookatCallbackWrapperROS.h
40 40
    hlrc_server::lookattargetResult result;
41 41
    const tf2_ros::Buffer& tfBuffer;
42 42
    const std::string FLOBI_BASE_LINK="BASE_LINK";
43
    Eigen::Affine3d eig_base_to_eyes; // base to center of eyes
43
    // pan/tilt will be computed w.r.t. to virtual frame at height of eyes
44
    Eigen::Affine3d eig_eyes_to_base;
44 45
    double eye_distance; // distance from center of eyes to eye frame
45 46

  
46 47
public:
......
49 50
       : CallbackWrapper<hlrc_server::lookattargetAction>(mw, scope, name, boost::bind(&LookatCallbackWrapper::call, this, _1))
50 51
       , tfBuffer(tfBuffer)
51 52
    {
52
        eye_distance = 0.15 / 2;
53
        // eyes frame has z-axis pointing along view dir (90° rotation w.r.t. base)
54
        // and z-position is shifted to height of eyes
55
        eig_eyes_to_base = (Eigen::Translation3d(0.070621, 0, 0.2006)
56
                * Eigen::AngleAxisd(angles::from_degrees(90), Eigen::Vector3d::UnitY())
57
                * Eigen::AngleAxisd(angles::from_degrees(180), Eigen::Vector3d::UnitZ())).inverse();
58
        eye_distance = 0.143 / 2;
53 59
    }
54 60

  
55 61
    void call(const GoalConstPtr &goal){
......
61 67
        Eigen::Vector3d target(p.x, p.y, p.z);
62 68
        try {
63 69
            if (!request->header.frame_id.empty()) { // only consider tf, when frame_id is non-empty
64
                geometry_msgs::TransformStamped source_to_base =
70
                geometry_msgs::TransformStamped base_to_source =
65 71
                    // lookupTransform can throw
66 72
                    tfBuffer.lookupTransform(FLOBI_BASE_LINK, request->header.frame_id,
67
                                             request->header.stamp, ros::Duration(0.01));
73
                                             request->header.stamp, ros::Duration(0.1));
68 74

  
69
                Eigen::Affine3d eig_source_to_eyes;
70
                eig_source_to_eyes = tf2::transformToEigen(source_to_base) * eig_base_to_eyes;
71
                target = eig_source_to_eyes * target;
75
                target = eig_eyes_to_base * (tf2::transformToEigen(base_to_source) * target);
72 76
            } else {
73 77
                // otherwise we interpret target directly w.r.t. eye center
74 78
            }
75 79
        } catch (const std::exception &e) {
76
            // Failed to resolve tf?
80
            // Failed to resolve tf
81
            ROS_WARN_STREAM_THROTTLE(2, e.what());
77 82
            result.result = 0;
78 83
            as_.setAborted(result, e.what());
79 84
            return;
80 85
        }
81 86

  
82 87
        // compute pan + tilt: let point z-axis of eyes frames towards target
88
        // alternatively, compute inverse kinematics of pan-tilt chain
89
        // as here: https://github.com/pal-robotics/head_action
83 90
        humotion::GazeState gaze_state;
84 91
        double distance = target.norm();
85 92
        if (distance > 1e-3) {
93
            // normalize vector from eyes-center to target
86 94
            target /= distance;
87

  
88 95
            // pan = rotation about x-axis: angle of projection of target onto yz-plane to z-axis
89
            gaze_state.pan = 90-angles::to_degrees(atan2(target.z(), target.y()));
96
            gaze_state.pan = 90 - angles::to_degrees(atan2(target.z(), target.y()));
90 97
            // tilt = rotation about y-axis: angle between target and yz-plane
91 98
            Eigen::Vector2d projection(target.y(), target.z());
92 99
            gaze_state.tilt = angles::to_degrees(atan2(target.x(), projection.norm()));

Also available in: Unified diff