C++ Transform::getRotation方法代码示例

void
save_localize_transform()
{
    std::ofstream filed_transform;
	std::stringstream file_name;

	std::string path = ros::package::getPath("prx_localizer");
	file_name << path << "/transform/localize_transform.bin";
    filed_transform.open(file_name.str().c_str(), std::ofstream::binary|std::ofstream::trunc);

    tfScalar x, y, z, w;
    x = g_localize_transform.getRotation().getX();
    y = g_localize_transform.getRotation().getY();
    z = g_localize_transform.getRotation().getZ();
    w = g_localize_transform.getRotation().getW();
    filed_transform.write((char *) &x, sizeof(tfScalar));
    filed_transform.write((char *) &y, sizeof(tfScalar));
    filed_transform.write((char *) &z, sizeof(tfScalar));
    filed_transform.write((char *) &w, sizeof(tfScalar));

    x = g_localize_transform.getOrigin().getX();
    y = g_localize_transform.getOrigin().getY();
    z = g_localize_transform.getOrigin().getZ();
    filed_transform.write((char *) &x, sizeof(tfScalar));
    filed_transform.write((char *) &y, sizeof(tfScalar));
    filed_transform.write((char *) &z, sizeof(tfScalar));

    filed_transform.close();
}

inline tf::Transform interpolateTF(tf::Transform start, tf::Transform end, double ratio)
{
	tf::Vector3 lerp_pos = start.getOrigin().lerp(end.getOrigin(), ratio);
	tf::Quaternion lerp_rot = start.getRotation().slerp(end.getRotation(), ratio);
	
	return tf::Transform(lerp_rot, lerp_pos);
}

void Match_Filter::scanCB(const sensor_msgs::LaserScan::ConstPtr& scan){		
	sensor_msgs::PointCloud2 cloud_in,cloud_out;
	projector_.transformLaserScanToPointCloud("/lidar_link",*scan,cloud_in,tfListener_);
	rot_tf.header.frame_id = "/ChestLidar";
//	lidar mx28 axis aligned mode
//	rot_tf.transform.rotation.x = enc_tf_.getRotation().x();
//	rot_tf.transform.rotation.y = enc_tf_.getRotation().y();
//	rot_tf.transform.rotation.z = enc_tf_.getRotation().z();
//	rot_tf.transform.rotation.w = enc_tf_.getRotation().w();

//	lidar mx28 axis perpendicular modeg
	tf::Quaternion q1;
	q1.setRPY(-M_PI/2,0,0);
	tf::Transform m1(q1);
	tf::Quaternion q2(enc_tf_.getRotation().x(),enc_tf_.getRotation().y(),enc_tf_.getRotation().z(),enc_tf_.getRotation().w());
	tf::Transform m2(q2);

	tf::Transform m4;
	m4 = m2*m1; // rotate lidar axis and revolute mx28 axis
	rot_tf.transform.rotation.x = m4.getRotation().x();
	rot_tf.transform.rotation.y = m4.getRotation().y();
	rot_tf.transform.rotation.z = m4.getRotation().z();
	rot_tf.transform.rotation.w = m4.getRotation().w();
	
	tf2::doTransform(cloud_in,cloud_out,rot_tf);
	point_cloud_pub_.publish(cloud_out);		
	moving_now.data = dxl_ismove_;
	dxl_ismove_pub_.publish(moving_now);
}

void
fromTF(const tf::Transform &source, Eigen::Matrix4f &dest, geometry_msgs::Pose &pose_dest)
{
    Eigen::Quaternionf q(source.getRotation().getW(),source.getRotation().getX(), source.getRotation().getY(),source.getRotation().getZ());
    q.normalize();
    Eigen::Vector3f t(source.getOrigin().x(), source.getOrigin().y(), source.getOrigin().z());
    Eigen::Matrix3f R(q.toRotationMatrix());
    dest(0,0) = R(0,0);
    dest(0,1) = R(0,1);
    dest(0,2) = R(0,2);
    dest(1,0) = R(1,0);
    dest(1,1) = R(1,1);
    dest(1,2) = R(1,2);
    dest(2,0) = R(2,0);
    dest(2,1) = R(2,1);
    dest(2,2) = R(2,2);
    dest(3,0) = dest(3,1)= dest(3,2) = 0;
    dest(3,3) = 1;
    dest(0,3) = t(0);
    dest(1,3) = t(1);
    dest(2,3) = t(2);
    pose_dest.orientation.x = q.x();
    pose_dest.orientation.y = q.y();
    pose_dest.orientation.z = q.z();
    pose_dest.orientation.w = q.w();
    pose_dest.position.x = t(0);
    pose_dest.position.y = t(1);
    pose_dest.position.z = t(2);
}

std::vector<double> planning_models::KinematicModel::PlanarJointModel::computeJointStateValues(const tf::Transform& transform) const 
{
  std::vector<double> ret;
  ret.push_back(transform.getOrigin().x());
  ret.push_back(transform.getOrigin().y());
  ret.push_back(transform.getRotation().getAngle()*transform.getRotation().getAxis().z());
  return ret;
}

void leatherman::btTransformToPoseMsg(const tf::Transform &bt, geometry_msgs::Pose &pose)
{
  pose.position.x = bt.getOrigin().getX();
  pose.position.y = bt.getOrigin().getY();
  pose.position.z = bt.getOrigin().getZ();
  pose.orientation.x = bt.getRotation().x();
  pose.orientation.y = bt.getRotation().y();
  pose.orientation.z = bt.getRotation().z();
  pose.orientation.w = bt.getRotation().w();
}

void printTransform(tf::Transform& t){
	for(int i=0; i<3; i++){
		std::cout << t.getOrigin()[i] << "\t";
	}
	std::cout << t.getRotation()[3] << "\t";
	for(int i=0; i<3; i++){
		std::cout << t.getRotation()[i] << "\t";
	}
	std::cout << std::endl;
}

void
fromTF(const tf::Transform &source, geometry_msgs::Pose &dest)
{
    dest.orientation.x = source.getRotation().getX();
    dest.orientation.y = source.getRotation().getY();
    dest.orientation.z = source.getRotation().getZ();
    dest.orientation.w = source.getRotation().getW();
    dest.position.x = source.getOrigin().x();
    dest.position.y = source.getOrigin().y();
    dest.position.z = source.getOrigin().z();
}

    //FIXME
    //calculate the absolute orientation of the object
    //
    float getOrient(tf::Transform & pose1, tf::Transform & pose2)
    {
      float orient;

      /* The orientation is the angle of (x-axis) rotation about the z-axis,
       *  under the assumption that z-axis does not rotate on x or y */
      orient = acos(
          sqrt(pow(tf::Matrix3x3(pose1.getRotation()).getColumn(0).getX(),2) + pow(tf::Matrix3x3(pose1.getRotation()).getColumn(0).getY(),2)) /
          sqrt(pow(tf::Matrix3x3(pose2.getRotation()).getColumn(0).getX(),2) + pow(tf::Matrix3x3(pose2.getRotation()).getColumn(0).getY(),2))  );

      return orient;
    }

geometry_msgs::PoseWithCovarianceStamped
tfToPoseCovarianceStamped (const tf::Transform &pose)
{
	geometry_msgs::PoseWithCovarianceStamped pocv;
	pocv.pose.pose.position.x = pose.getOrigin().x();
	pocv.pose.pose.position.y = pose.getOrigin().y();
	pocv.pose.pose.position.z = pose.getOrigin().z();
	pocv.pose.pose.orientation.x = pose.getRotation().x();
	pocv.pose.pose.orientation.y = pose.getRotation().y();
	pocv.pose.pose.orientation.z = pose.getRotation().z();
	pocv.pose.pose.orientation.w = pose.getRotation().w();
	return pocv;
}

 void tfToPose(tf::Transform &trans, geometry_msgs::PoseStamped &msg)
 {
   tf::quaternionTFToMsg(trans.getRotation(), msg.pose.orientation);
   msg.pose.position.x = trans.getOrigin().x();
   msg.pose.position.y = trans.getOrigin().y();
   msg.pose.position.z = trans.getOrigin().z();
 }

bool computeMatrix(PointCloud::Ptr target,
                   PointCloud::Ptr world,
                   std::string target_name,
                   std::string world_name,
                   const bool broadcast)
{
    if ((!world_name.empty()) && (!target_name.empty()) &&
            (target->points.size() > 2) && (world->points.size() == target->points.size()))
    {
        Eigen::Matrix4f trMatrix;
        pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> svd;

        svd.estimateRigidTransformation(*target, *world, trMatrix);

        ROS_INFO("Registration completed and Registration Matrix is being broadcasted");

         transform=tf::Transform(tf::Matrix3x3(trMatrix(0, 0), trMatrix(0, 1), trMatrix(0, 2),
                                              trMatrix(1, 0), trMatrix(1, 1), trMatrix(1, 2),
                                              trMatrix(2, 0), trMatrix(2, 1), trMatrix(2, 2)),
                                tf::Vector3(trMatrix(0, 3), trMatrix(1, 3), trMatrix(2, 3)));

        Eigen::Vector3d origin(transform.getOrigin());
        double roll, pitch, yaw;
        tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
	std::cout << std::endl << "#################################################" << std::endl; 
	std::cout << std::endl << "########### TRANSFORMATION PARAMETERS ###########" << std::endl; 
	std::cout << std::endl << "#################################################" << std::endl; 
        std::cout << "origin: "<<origin.transpose() << std::endl;
        std::cout << "rpy: " << roll << " " << pitch << " " << yaw << std::endl;


    }

    return true;
}

	/**
	 * Send transform to FCU position controller
	 *
	 * Note: send only XYZ, Yaw
	 */
	void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
		// ENU frame
		tf::Vector3 origin = transform.getOrigin();
		tf::Quaternion q = transform.getRotation();

		/* Documentation start from bit 1 instead 0,
		 * Ignore velocity and accel vectors, yaw rate
		 */
		uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);

		if (uas->is_px4()) {
			/**
			 * Current PX4 has bug: it cuts throttle if there no velocity sp
			 * Issue #273.
			 *
			 * @todo Revesit this quirk later. Should be fixed in firmware.
			 */
			ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
		}

		// ENU->NED. Issue #49.
		set_position_target_local_ned(stamp.toNSec() / 1000000,
				MAV_FRAME_LOCAL_NED,
				ignore_all_except_xyz_y,
				origin.y(), origin.x(), -origin.z(),
				0.0, 0.0, 0.0,
				0.0, 0.0, 0.0,
				tf::getYaw(q), 0.0);
	}

Affine3d eigen_from_tf(tf::Transform transform) {
    Affine3d x;
    x = Translation3d(vec2vec(transform.getOrigin()));
    tf::Quaternion q = transform.getRotation();
    x *= Quaterniond(q.w(), q.x(), q.y(), q.z());
    return x;
}

// Move the end effector (tf jaco_tool_position) to a certain position
void JacoCustom::moveToPoint(tf::Transform tf_){

    geometry_msgs::Twist arm;
    arm.linear.x = tf_.getOrigin().getX();
    arm.linear.y = tf_.getOrigin().getY();
    arm.linear.z = tf_.getOrigin().getZ();

//    double roll, pitch, yaw;
//    tf_.getBasis().getRPY(roll,pitch, yaw);
//    arm.angular.x = roll;
//    arm.angular.y = pitch;
//    arm.angular.z = yaw;



    wpi_jaco_msgs::QuaternionToEuler conv;
    geometry_msgs::Quaternion quat;
    tf::quaternionTFToMsg(tf_.getRotation(), quat);
    conv.request.orientation = quat;
    if(quaternion_to_euler_service_client.call(conv)){
        arm.angular.x = conv.response.roll;
        arm.angular.y = conv.response.pitch;
        arm.angular.z = conv.response.yaw;
    }

    moveToPoint(arm);
}