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

void ATTCallback (IvyClientPtr app, void* data , int argc, char **argv)
{
	double att_unit_coef= 0.0139882;
	double phi, theta, yaw;
		
	sscanf(argv[0],"%lf %lf %lf %*d %*d %*d",&phi,&theta,&yaw);
	phi*=att_unit_coef*DEG2RAD;
	theta*=-att_unit_coef*DEG2RAD;
	yaw*=-att_unit_coef*DEG2RAD;
	
	q.setRPY(phi,theta,yaw);
	
	//ROS_INFO("Phi %f; Theta %f; Yaw %f", phi,theta,yaw);
	//ROS_INFO("q1 %f; q2 %f; q3 %f; q4 %f", q.x(),q.y(),q.z(),q.w());

	imu_data.header.stamp = ros::Time::now();
	imu_data.orientation.x=q.x();
	imu_data.orientation.y=q.y();
	imu_data.orientation.z=q.z();
	imu_data.orientation.w=q.w();
	
	imu_message.publish(imu_data);
	
	//Only temporary until the rates are equal
	att_data.header.stamp = ros::Time::now();
	att_data.orientation.x=q.x();
	att_data.orientation.y=q.y();
	att_data.orientation.z=q.z();
	att_data.orientation.w=q.w();

	att_message.publish(att_data);
}

/**
  * Packs current state in a odom message. Needs a quaternion for conversion.
  */
void pack_pose(tf::Quaternion& q, nav_msgs::Odometry& odom)
{
    q.setRPY(0, 0, _theta);

    odom.header.stamp = ros::Time::now();

    odom.pose.pose.position.x = _x;
    odom.pose.pose.position.y = _y;

    odom.pose.pose.orientation.x = q.x();
    odom.pose.pose.orientation.y = q.y();
    odom.pose.pose.orientation.z = q.z();
    odom.pose.pose.orientation.w = q.w();
}

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    double r, p ,y;
    
    lastQuat = tf::Quaternion(msg->pose.pose.orientation.x,
                              msg->pose.pose.orientation.y,
                              msg->pose.pose.orientation.z,
                              msg->pose.pose.orientation.w);
    
    btMatrix3x3(lastQuat).getRPY(r, p, y);
    
    lastQuat.setRPY(r, p, y + M_PI / 2.0);
    
    lastOdom = *msg;
}

void imuMsgCallback(const sensor_msgs::Imu& imu_msg)
{
  tf::quaternionMsgToTF(imu_msg.orientation, tmp_);

  tfScalar yaw, pitch, roll;
  tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);

  tmp_.setRPY(roll, pitch, 0.0);

  transform_.setRotation(tmp_);

  transform_.stamp_ = imu_msg.header.stamp;

  tfB_->sendTransform(transform_);
}

  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());  
}