Revision 01ff8464

View differences:

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