Revision 87c0fad1 server/include/ROS/LookatCallbackWrapperROS.h

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