C++ Pose::setRotation方法代码示例

  tf::Pose MotionModel::updateAction(tf::Pose& p)
    {
      double delta_rot1;
      if (dxy < 0.01)
        delta_rot1 = 0.0;
      else
        delta_rot1 = angle_diff(atan2(delta_y, delta_x), delta_yaw);
      double delta_trans = dxy;
      double delta_rot2 = angle_diff(delta_yaw, delta_rot1);

      double delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1, M_PI)));
      double delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2, M_PI)));

      double delta_rot1_hat = angle_diff(delta_rot1, sampleGaussian(alpha1 * delta_rot1_noise*delta_rot1_noise + 
                                                                    alpha2 * delta_trans*delta_trans));
      double delta_trans_hat = delta_trans - sampleGaussian(alpha3 * delta_trans*delta_trans +
                                                            alpha4 * delta_rot1_noise*delta_rot1_noise +
                                                            alpha4 * delta_rot2_noise*delta_rot2_noise);
      double delta_rot2_hat = angle_diff(delta_rot2, sampleGaussian(alpha1 * delta_rot2_noise*delta_rot2_noise +
                                                                    alpha2 * delta_trans*delta_trans));

      delta_x = delta_trans_hat * cos(delta_rot1_hat);
      delta_y = delta_trans_hat * sin(delta_rot1_hat);
      delta_yaw = delta_rot1_hat + delta_rot2_hat;

   		tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
      tf::Transform base_to_global_ = tf::Transform(p.getRotation());
      noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
      p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
      p.setRotation(p.getRotation() * noisy_pose.getRotation());
    }

// Convert from CameraPose to tf::Pose (position and orientation)
inline void CameraPose2TFPose(const CameraPose& cameraPose, tf::Pose& pose)
{
  cv::Vec4d orientation = cameraPose.GetOrientationQuaternion();
  cv::Vec3d position = cameraPose.GetPosition();
  pose.setOrigin(tf::Vector3(position[0], position[1], position[2]));
  pose.setRotation(tf::Quaternion(orientation[1], orientation[2], orientation[3], orientation[0] )); // q = (x,y,z,w)
}

	void toPose(const MathLib::Matrix4& mat4, tf::Pose& pose) {
		MathLib::Matrix3 m1 = mat4.GetOrientation();
		MathLib::Vector3 v1 = m1.GetRotationAxis();
		tf::Vector3 ax(v1(0), v1(1), v1(2));
		pose.setRotation(tf::Quaternion(ax, m1.GetRotationAngle()));
		v1.Set(mat4.GetTranslation());
		pose.setOrigin(tf::Vector3(v1(0),v1(1),v1(2)));
	}

bool MarkerSearcherComputation::getNextSearchPose(tf::Pose& returnPose) {
    if(poseReturned) {
        return false;
    }
    returnPose.setOrigin(nextSearchPose.getOrigin());
    returnPose.setRotation(nextSearchPose.getRotation());
    poseReturned = true;
    return true;
}

    void toTfPose(const geometry_msgs::Pose& msg_pose, tf::Pose& tf_pose)
    {
        tf_pose.setOrigin(tf::Vector3(msg_pose.position.x,
                                      msg_pose.position.y,
                                      msg_pose.position.z));

        tf_pose.setRotation(tf::Quaternion(msg_pose.orientation.x,
                                           msg_pose.orientation.y,
                                           msg_pose.orientation.z,
                                           msg_pose.orientation.w));
    }

// Callback for the desired cartesian pose
void cartCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
	ROS_INFO_STREAM("Received Position Command");
	const geometry_msgs::PoseStamped* data = msg.get();
	des_ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
	des_ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));

	ROS_INFO_STREAM_THROTTLE(1, "Received Position: "<<des_ee_pose.getOrigin().x()<<","<<des_ee_pose.getOrigin().y()<<","<<des_ee_pose.getOrigin().z());

	if(bOrientCtrl) {
		tf::Quaternion q = des_ee_pose.getRotation();
		ROS_INFO_STREAM_THROTTLE(1, "Received Orientation: "<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w());
	}
}

void RTKRobotArm::getEEPose(tf::Pose& pose) {
	if(mRobot) {
		//Collect ee pose from RTK
		MathLib::Vector eeq(4);
		Matrix3 tmp = mRobot->GetReferenceFrame(nEndEffectorId).GetOrient();
		tmp.GetQuaternionRepresentation(eeq);
		MathLib::Vector3 eep = mRobot->GetReferenceFrame(nEndEffectorId).GetOrigin();

		// Convert to tf
		pose.setRotation(tf::Quaternion(eeq[1], eeq[2], eeq[3], eeq[0]));
		pose.setOrigin(tf::Vector3(eep[0], eep[1], eep[2]));
	} else{
		ROS_WARN("RTK Robot not initialized");
	}
}

	tf::Pose MotionModel::drawFromMotion(tf::Pose& p) 
		{
			double sxy=0.3*srr;
			delta_x+=sampleGaussian(srr*fabs(delta_x)+str*fabs(delta_yaw)+sxy*fabs(delta_y));
			delta_y+=sampleGaussian(srr*fabs(delta_y)+str*fabs(delta_yaw)+sxy*fabs(delta_x));
			delta_yaw+=sampleGaussian(stt*fabs(delta_yaw)+srt*dxy);
			delta_yaw=fmod(delta_yaw, 2*M_PI);
			if (delta_yaw>M_PI)
				delta_yaw-=2*M_PI;

   		tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
      tf::Transform base_to_global_ = tf::Transform(p.getRotation());
      noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
      p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
      p.setRotation(p.getRotation() * noisy_pose.getRotation());

			return p;
		}

//.........这里部分代码省略.........


        /******************************************************************
        *                            Odometry
        *****************************************************************/
        if (!m_paused) {

        // apply offset transformation:
        tf::Pose transformedPose;


        if (odomData.size()!=6)
        {
            ROS_ERROR( "Error getting odom data. length is %zu",odomData.size() );
            continue;
        }

        double dt = (stamp.toSec() - m_lastOdomTime);

        odomX = odomData[0];
        odomY = odomData[1];
        odomZ = odomData[2];
        odomWX = odomData[3];
        odomWY = odomData[4];
        odomWZ = odomData[5];

        tf::Quaternion q;
        // roll and pitch from IMU, yaw from odometry:
        if (m_useIMUAngles)
            q.setRPY(angleX, angleY, odomWZ);
        else
            q.setRPY(odomWX, odomWY, odomWZ);

        m_odomPose.setOrigin(tf::Vector3(odomX, odomY, odomZ));
        m_odomPose.setRotation(q);

        if(m_mustUpdateOffset) {
            if(!m_isInitialized) {
                if(m_initializeFromIMU) {
                    // Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
                    m_targetPose.setOrigin(m_odomPose.getOrigin());
                    m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, odomWZ));
                } else if(m_initializeFromOdometry) {
                    m_targetPose.setOrigin(m_odomPose.getOrigin());
                    m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, odomWZ));
                }
                m_isInitialized = true;
            } else {
                // Overwrite target pitch and roll angles with IMU data
                const double target_yaw = tf::getYaw(m_targetPose.getRotation());
                if(m_initializeFromIMU) {
                    m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, target_yaw));
                } else if(m_initializeFromOdometry){
                    m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, target_yaw));
                }
            }
            m_odomOffset = m_targetPose * m_odomPose.inverse();
            transformedPose = m_targetPose;
            m_mustUpdateOffset = false;
        } else {
            transformedPose = m_odomOffset * m_odomPose;
        }

        //
        // publish the transform over tf first
        //
        m_odomTransformMsg.header.stamp = stamp;
        tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
        m_transformBroadcaster.sendTransform(m_odomTransformMsg);


        //
        // Fill the Odometry msg
        //
        m_odom.header.stamp = stamp;
        //set the velocity first (old values still valid)
        m_odom.twist.twist.linear.x = (odomX - m_odom.pose.pose.position.x) / dt;
        m_odom.twist.twist.linear.y = (odomY - m_odom.pose.pose.position.y) / dt;
        m_odom.twist.twist.linear.z = (odomZ - m_odom.pose.pose.position.z) / dt;

        // TODO: calc angular velocity!
        //	m_odom.twist.twist.angular.z = vth; ??

        //set the position from the above calculated transform
        m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
        m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
        m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
        m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;


        m_odomPub.publish(m_odom);


        m_lastOdomTime = stamp.toSec();

        }
    }
    ROS_INFO("naoqi_sensors stopped.");

}

void eeStateCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
	const geometry_msgs::PoseStamped* data = msg.get();
	ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
	ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));
	isOkay = true;
}

void OdometryRemap::torsoOdomCallback(const nao_msgs::TorsoOdometryConstPtr& msgOdom, const nao_msgs::TorsoIMUConstPtr& msgIMU){
	if (m_paused){
		ROS_DEBUG("Skipping odometry callback, paused");
		return;
	}

	double odomTime = (msgOdom->header.stamp).toSec();
	ROS_DEBUG("Received [%f %f %f %f (%f/%f) (%f/%f) %f]", odomTime, msgOdom->x, msgOdom->y, msgOdom->z, msgOdom->wx, msgIMU->angleX, msgOdom->wy, msgIMU->angleY, msgOdom->wz);

	double dt = (odomTime - m_lastOdomTime);


	tf::Quaternion q;
	// roll and pitch from IMU, yaw from odometry:
	if (m_useIMUAngles)
		q.setRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz);
	else
		q.setRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz);


	m_odomPose.setOrigin(tf::Vector3(msgOdom->x, msgOdom->y, msgOdom->z));
	m_odomPose.setRotation(q);

	// apply offset transformation:
	tf::Pose transformedPose;

	if(m_mustUpdateOffset) {
        if(!m_isInitialized) {
            if(m_initializeFromIMU) {
                // Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
                m_targetPose.setOrigin(m_odomPose.getOrigin());
                m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz));
            } else if(m_initializeFromOdometry) {
                m_targetPose.setOrigin(m_odomPose.getOrigin());
                m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz));
            }
            m_isInitialized = true;
        } else {
            // Overwrite target pitch and roll angles with IMU data
            const double target_yaw = tf::getYaw(m_targetPose.getRotation());
            if(m_initializeFromIMU) {
                m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, target_yaw));
            } else if(m_initializeFromOdometry){
                m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, target_yaw));
            }
        }
 		m_odomOffset = m_targetPose * m_odomPose.inverse();
		transformedPose = m_targetPose;
		m_mustUpdateOffset = false;
	} else {
	    transformedPose = m_odomOffset * m_odomPose;
	}

	// publish the transform over tf first:
	m_odomTransformMsg.header.stamp = msgOdom->header.stamp;
	tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
	m_transformBroadcaster.sendTransform(m_odomTransformMsg);

	//next, publish the actual odometry message:
	m_odom.header.stamp = msgOdom->header.stamp;
	m_odom.header.frame_id = m_odomFrameId;


	//set the velocity first (old values still valid)
	m_odom.twist.twist.linear.x = (msgOdom->x - m_odom.pose.pose.position.x) / dt;
	m_odom.twist.twist.linear.y = (msgOdom->y - m_odom.pose.pose.position.y) / dt;
	m_odom.twist.twist.linear.z = (msgOdom->z - m_odom.pose.pose.position.z) / dt;
	// TODO: calc angular velocity!
//	m_odom.twist.twist.angular.z = vth; ??

	//set the position from the above calculated transform
	m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
	m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
	m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
	m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;

	//publish the message
	m_odomPub.publish(m_odom);

	m_lastOdomTime = odomTime;

	// TODO: not used
//	m_lastWx = msgOdom->wx;
//	m_lastWy = msgOdom->wy;
//	m_lastWz = msgOdom->wz;
}