C++ Quaternion::getAxis方法代码示例

bool Hand_filter::jumped(const tf::Quaternion& q_current,const tf::Quaternion& q_previous,const float threashold) const {
    tf::Vector3 axis = q_current.getAxis();

   // std::cout<< "axis: " << axis.x() << " " << axis.y() << " " << axis.z() << std::endl;
    return false;
}

  void LeapMotionListener::leapmotionCallback(const leap_motion::leapros2::ConstPtr& dataHand)
{
      dataHand_=(*dataHand);
      ROS_INFO("ORIENTATION OF THE HAND SET TO \n X: %f\n  Y: %f\n Z: %f\n ",dataHand_.ypr.x,dataHand_.ypr.y,dataHand_.ypr.z);
      ROS_INFO("DIRECTION OF THE HAND SET TO \n X: %f\n  Y: %f\n Z: %f\n ",dataHand_.direction.x,dataHand_.direction.y,dataHand_.direction.z);
      ROS_INFO("NORMAL OF THE HAND SET TO \n X: %f\n  Y: %f\n Z: %f\n ",dataHand_.normal.x,dataHand_.normal.y,dataHand_.normal.z);
      //ROS_INFO("INSIDE CALLBACK");
      //We create the values of reference for the first postion of our hand
      if (FIRST_VALUE)
        {
        dataLastHand_.palmpos.x=dataHand_.palmpos.x;
        dataLastHand_.palmpos.y=dataHand_.palmpos.y;
        dataLastHand_.palmpos.z=dataHand_.palmpos.z;
        FIRST_VALUE=0;
        Updifferencex=dataLastHand_.palmpos.x+10;
        Downdifferencex=dataLastHand_.palmpos.x-10;
        Updifferencez=dataLastHand_.palmpos.z+10;
        Downdifferencez=dataLastHand_.palmpos.z-20;
        Updifferencey=dataLastHand_.palmpos.y+20;
        Downdifferencey=dataLastHand_.palmpos.y-20;
        //ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n  Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
         
        //sleep(2);
        }
        else
        {
            //We get the distance between the finger and transform it into gripper distance
            rot7=DtA+DtAx*dataHand_.finger_distance;
            rot8=DtA+DtAx*dataHand_.finger_distance;
            joint_msg_leap=jointstate_;
            joint_msg_leap.position[7] = -rot8;
            joint_msg_leap.position[6] = rot7;
            
            if ((dataHand_.palmpos.x<Downdifferencex)||(dataHand_.palmpos.x>Updifferencex)||(dataHand_.palmpos.y<Downdifferencey)||(dataHand_.palmpos.y>Updifferencey)||(dataHand_.palmpos.z<Downdifferencez)||(dataHand_.palmpos.z>Updifferencez))
              {
                //q.setRPY(0,0,M_PI/2);//Fixed Position for testing..with this pose the robot is oriented to the ground
                q.setRPY(dataHand_.ypr.x,dataHand_.ypr.y,dataHand_.ypr.z);
                pose.orientation.x = q.getAxis().getX();//cambiado aposta getZ()
                pose.orientation.y = q.getAxis().getY();
                pose.orientation.z = q.getAxis().getZ();//cambiado aposta getX()
                pose.orientation.w = q.getW();
                //pose.orientation.w = ;
                //pose.orientation.z=1;
                //pose.orientation.y=0;
                //pose.orientation.x=0;
                //We need to send the correct axis to the robot. Currently they are rotated and x is z
                //ROS_INFO("VALUES OF THE QUATERNION SET TO \n X: %f\n  Y: %f\n Z: %f W: %f\n",pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
                pose.position.y +=(dataHand_.palmpos.x-dataLastHand_.palmpos.x) ;
                pose.position.z +=(dataHand_.palmpos.y-dataLastHand_.palmpos.y);
                if(pose.position.z>Uplimitez)
                pose.position.z=Uplimitez;
                pose.position.x +=(dataHand_.palmpos.z-dataLastHand_.palmpos.z);
                //Here we instantiate an autogenerated service class 
                srv.request.target = pose ;
                if (pclient->call(srv))
                {
                  //ROS_INFO("Ret: %d", (int)srv.response.ret);
                  dataLastHand_.palmpos.x=dataHand_.palmpos.x;
                  dataLastHand_.palmpos.y=dataHand_.palmpos.y;
                  dataLastHand_.palmpos.z=dataHand_.palmpos.z;
                  // Both limits for x,y,z to avoid small changes
                  Updifferencex=dataLastHand_.palmpos.x+10;//
                  Downdifferencex=dataLastHand_.palmpos.x-10;
                  Updifferencez=dataLastHand_.palmpos.z+10;
                  Downdifferencez=dataLastHand_.palmpos.z-20;
                  Updifferencey=dataLastHand_.palmpos.y+20;
                  Downdifferencey=dataLastHand_.palmpos.y-20;
                  old_pose=pose;
                  ROS_INFO("response %f\n",srv.response.ret);
                }
                else
                {
                  ROS_ERROR("Position out of range");
                  pose=old_pose;
                } 
              }
         //We get the aswer of the service and publish it into the joint_msg_leap message
         //joint_msg_leap.position[0] = srv.response.joint1;
         //joint_msg_leap.position[1] = srv.response.joint2;
         //joint_msg_leap.position[2] = srv.response.joint3;
         //joint_msg_leap.position[3] = srv.response.joint4;
         //joint_msg_leap.position[4] = srv.response.joint5;
         //joint_msg_leap.position[5] = srv.response.joint6;
         //robo_pub.publish(joint_msg_leap);
        }
 //ROS_INFO("END EFFECTOR POSITION \n X: %f\n  Y: %f\n Z: %f\n", pose.position.x,pose.position.y,pose.position.z);
 //ROS_INFO("ORIENTATION OF THE HAND SET TO \n X: %f\n  Y: %f\n Z: %f\n ",q.getAxis().getX(),q.getAxis().getY(),q.getAxis().getZ());  
}