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

std::vector<desperate_housewife::fittedGeometriesSingle> HandPoseGenerator::SortbyHand(std::vector<desperate_housewife::fittedGeometriesSingle> objects_vec, tf::StampedTransform hand_position)
{

	geometry_msgs::Pose hand_position_local;
	hand_position_local.position.x = hand_position.getOrigin().x();
	hand_position_local.position.y = hand_position.getOrigin().y();
	hand_position_local.position.z = hand_position.getOrigin().z();
	hand_position_local.orientation.x = hand_position.getRotation().x();
	hand_position_local.orientation.y = hand_position.getRotation().y();
	hand_position_local.orientation.z = hand_position.getRotation().z();
	hand_position_local.orientation.w = hand_position.getRotation().w();

	Eigen::Matrix4d T_h_w = FromMsgtoEigen(hand_position_local);

	std::vector< desperate_housewife::fittedGeometriesSingle > objects_vec_roted_by_hand = objects_vec;

	for (unsigned int i = 0; i < objects_vec.size(); i++)
	{
		Eigen::Matrix4d T_o_w = FromMsgtoEigen(objects_vec_roted_by_hand[i].pose);
		geometry_msgs::Pose pose_temp;
		Eigen::Matrix4d T_o_h = T_h_w.inverse() * T_o_w;
		fromEigenToPose(T_o_h, pose_temp);
		// desperate_housewife::fittedGeometriesSingle object_temp = objects_vec[i];
		objects_vec_roted_by_hand[i].pose =  pose_temp;
		// objects_vec_roted_by_hand.push_back(object_temp);
	}

	std::sort(objects_vec_roted_by_hand.begin(), objects_vec_roted_by_hand.end(), [](desperate_housewife::fittedGeometriesSingle first, desperate_housewife::fittedGeometriesSingle second) {
		double distfirst = std::sqrt( first.pose.position.x * first.pose.position.x + first.pose.position.y * first.pose.position.y + first.pose.position.z * first.pose.position.z);
		double distsecond = std::sqrt( second.pose.position.x * second.pose.position.x + second.pose.position.y * second.pose.position.y + second.pose.position.z * second.pose.position.z);
		return (distfirst < distsecond);
	});

	return objects_vec_roted_by_hand;
}

    geometry_msgs::PoseStamped getCartBaseLinkPosition()
    {
        geometry_msgs::PoseStamped pose_base_link;
        pose_base_link.header.frame_id = base_link_;

        while (nh_.ok()) {
            try {
                tf_listener_.lookupTransform(base_link_, lwr_tip_link_, ros::Time(0), base_link_transform_);

                pose_base_link.pose.position.x = base_link_transform_.getOrigin().x();
                pose_base_link.pose.position.y = base_link_transform_.getOrigin().y();
                pose_base_link.pose.position.z = base_link_transform_.getOrigin().z();
                pose_base_link.pose.orientation.x = base_link_transform_.getRotation().x();
                pose_base_link.pose.orientation.y = base_link_transform_.getRotation().y();
                pose_base_link.pose.orientation.z = base_link_transform_.getRotation().z();
                pose_base_link.pose.orientation.w = base_link_transform_.getRotation().w();

                return pose_base_link;
            }
            catch (tf::TransformException ex){
                ROS_ERROR("%s",ex.what());
                ros::Duration(0.1).sleep();
            }
        }
    }

void InputOutputLinControl::markerFromPose(std::string name_space, double red, double green, double blue, tf::StampedTransform pose, visualization_msgs::Marker& marker)
{
	marker.header.frame_id = pose.frame_id_;
	marker.header.stamp = pose.stamp_;
	marker.ns = name_space.c_str();
	marker.id = 0;
	marker.type = visualization_msgs::Marker::ARROW;
	marker.action = visualization_msgs::Marker::ADD;

	marker.pose.position.x = pose.getOrigin().getX();
	marker.pose.position.y = pose.getOrigin().getY();
	marker.pose.position.z = pose.getOrigin().getZ();
	marker.pose.orientation.x = pose.getRotation().getX();
	marker.pose.orientation.y = pose.getRotation().getY();
	marker.pose.orientation.z = pose.getRotation().getZ();
	marker.pose.orientation.w = pose.getRotation().getW();

	marker.scale.x = 1;
	marker.scale.y = 1;
	marker.scale.z = 0.8;

	marker.color.a = 1;
	marker.color.g = green;
	marker.color.r = red;
	marker.color.b = blue;

	marker.lifetime = ros::Duration(0);
}

gazebo::math::Pose gazebo::IAI_BoxDocking::tfToGzPose(tf::StampedTransform transform)
{
  math::Quaternion rotation = math::Quaternion( transform.getRotation().w(), transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z());
  math::Vector3 origin = math::Vector3(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()); //T_q^w  
  math::Pose pose = math::Pose(origin,rotation);
  return pose;
}

void InputOutputLinControl::updateMarkerFromPose(tf::StampedTransform pose, visualization_msgs::Marker& marker)
{
	marker.header.stamp = pose.stamp_;
	marker.header.frame_id = pose.frame_id_;
	marker.pose.position.x = pose.getOrigin().getX();
	marker.pose.position.y = pose.getOrigin().getY();
	marker.pose.position.z = pose.getOrigin().getZ();
	marker.pose.orientation.x = pose.getRotation().getX();
	marker.pose.orientation.y = pose.getRotation().getY();
	marker.pose.orientation.z = pose.getRotation().getZ();
	marker.pose.orientation.w = pose.getRotation().getW();
}

// Calculating and returning the center2center transform which belongs to stampedT_in
// [ A(front), B(right), C(back) or D(left) ]
tf::StampedTransform C2C_LEFT_Node::get_c2c(const tf::StampedTransform& stampedT_in)
{
	tf::Transform T_in = tf::Transform(stampedT_in.getRotation(), stampedT_in.getOrigin());

	if (stampedT_in.child_frame_id_[1] != '_')		// if stampedT_in was stand-alone (previously 1 transforms)
	{
		switch (stampedT_in.child_frame_id_[1] - '0')
		{
			case 0: case 1:	return tf::StampedTransform(apollon[1]*T_in*boreas[3].inverse(), ros::Time::now(), "apollon_center", "A_" + side + "_c2c");
			case 2: case 3:	return tf::StampedTransform(apollon[1]*T_in*boreas[4].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "B_" + side + "_c2c");
			case 4: case 5:	return tf::StampedTransform(apollon[1]*T_in*boreas[5].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "C_" + side + "_c2c");
			case 6: case 7:	return tf::StampedTransform(apollon[1]*T_in*boreas[6].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "D_" + side + "_c2c");
		}
	}

	else		// else stampedT_in was merged (previously 2 transforms)
	{
		if (stampedT_in.child_frame_id_[0] == 'A')
			return tf::StampedTransform(apollon[1]*T_in*boreas[3].inverse(), ros::Time::now(), "apollon_center", "A_" + side + "_c2c");
		else if (stampedT_in.child_frame_id_[0] == 'B')
			return tf::StampedTransform(apollon[1]*T_in*boreas[4].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "B_" + side + "_c2c");
		else if (stampedT_in.child_frame_id_[0] == 'C')
			return tf::StampedTransform(apollon[1]*T_in*boreas[5].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "C_" + side + "_c2c");
		else if (stampedT_in.child_frame_id_[0] == 'D')
			return tf::StampedTransform(apollon[1]*T_in*boreas[6].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "D_" + side + "_c2c");
	}
}

/**
 * Convert tf::StampedTransform to string
 */
template<> std::string toString<tf::StampedTransform>(const tf::StampedTransform& p_transform)
{
  std::stringstream ss;

  ss << "StampedTransform(Quaternion=" << toString(p_transform.getRotation()) << "; Vector3=" << toString(p_transform.getOrigin()) << ")";

  return ss.str();
}

bool
WmObjectCapture::updateObjectFrame(const ros::Time& stamp, tf::StampedTransform& m2c)
{
	std::vector<tf::StampedTransform> valids;

	getValidMarks(valids, stamp);
	if(valids.size()<4)
	{
		//ROS_INFO("No enough marks detected [%zu]", valids.size());
		return false;
	}

	tf::Transform media;
	double stdev;
	getBestTransform(valids, media, stdev);

	double m_tolerance2 = 0.006*0.006;
	if(stdev<m_tolerance2)
	{
		//ROS_INFO("TRANSFORM ACCEPTED ");
		m2c.child_frame_id_ = objectFrameId_;
		m2c.frame_id_ = cameraFrameId_;
		m2c.stamp_ = ros::Time::now();

		m2c.setOrigin(media.getOrigin());
		m2c.setRotation(media.getRotation());

		ROS_DEBUG("NEW FINAL (%lf, %lf, %lf) (%lf, %lf, %lf, %lf)",
				m2c.getOrigin().x(), m2c.getOrigin().y(), m2c.getOrigin().z(),
				m2c.getRotation().x(), m2c.getRotation().y(), m2c.getRotation().z(), m2c.getRotation().w());

		try
		{
			tfBroadcaster_.sendTransform(m2c);

		}catch(tf::TransformException & ex)
		{
			ROS_WARN("WmObjectCapture::updateObjectFrame %s",ex.what());
		}
		return true;
	}

	//ROS_INFO("TRANSFORM REJECTED ");
	return false;

}

geometry_msgs::Pose convertToPose(tf::StampedTransform transform)
{
	tf::Quaternion q = transform.getRotation();
	tf::Point p = transform.getOrigin();

	geometry_msgs::Pose pose;
	pose.position = createPoint(p.x(), p.y(), p.z());
	pose.orientation = createQuaternion(q.x(), q.y(), q.z(), q.w());
	return pose;
}

void RockinPNPActionServer::tf2Affine(tf::StampedTransform& tf, Eigen::Affine3d& T)
{
  tf::Vector3 o=tf.getOrigin();
  tf::Quaternion q_tf=tf.getRotation();
  Eigen::Quaterniond q(q_tf[3],q_tf[0],q_tf[1],q_tf[2]);
  Eigen::Matrix3d R(q);
  Eigen::Vector3d t(o[0],o[1],o[2]);
  T.linear()=R; T.translation()=t;
  
}

void printTF(tf::StampedTransform t, WhichArm a)
{
	std::string p;
	if(a==RIGHT)
	{
		std::cout<< "#Right gripper:\n";
		p="r";
	}
	else
	{
		std::cout<< "#Left gripper:\n";
		p="l";
	}

	std::cout<< "p.position.x     = " << t.getOrigin().getX()    << ";\n"
			 << "p.position.y     = " << t.getOrigin().getY()    << ";\n"
			 << "p.position.z     = " << t.getOrigin().getZ()    << ";\n"
			 << "p.orientation.x  = " << t.getRotation().getX()  << ";\n"
			 << "p.orientation.y  = " << t.getRotation().getY()  << ";\n"
			 << "p.orientation.z  = " << t.getRotation().getZ()  << ";\n"
			 << "p.orientation.w  = " << t.getRotation().getW()  << ";\n";

	geometry_msgs::PoseStamped tmp;
	tmp.pose.position.x 	= t.getOrigin().getX()  ;
	tmp.pose.position.y     = t.getOrigin().getY()  ;
	tmp.pose.position.z     = t.getOrigin().getZ()  ;
	tmp.pose.orientation.x 	= t.getRotation().getX();
	tmp.pose.orientation.y  = t.getRotation().getY();
	tmp.pose.orientation.z  = t.getRotation().getZ();
	tmp.pose.orientation.w  = t.getRotation().getW();
	std::cout<< "    pose:\n"
			 << "      position:\n"
			 << "        x: "			<< tmp.pose.position.x << "\n"
			 << "        y: "			<< tmp.pose.position.y << "\n"
			 << "        z: "			<< tmp.pose.position.z << "\n"
			 << "      orientation:\n"
			 << "        x: "			<< tmp.pose.orientation.x << "\n"
			 << "        y: "			<< tmp.pose.orientation.y << "\n"
			 << "        z: "			<< tmp.pose.orientation.z << "\n"
			 << "        w: "			<< tmp.pose.orientation.w << "\n\n";
//	std::cout<<tmp<<"\n";
}

void transform_callback(tf::StampedTransform transform){
	double roll,pitch,yaw;

	Pos_Controller_U.position[0]=transform.getOrigin().x();
	Pos_Controller_U.position[1]=transform.getOrigin().y();
	Pos_Controller_U.position[2]=transform.getOrigin().z();
	tf::Matrix3x3(transform.getRotation()).getRPY(roll,pitch,yaw);
	Pos_Controller_U.yaw=yaw;
	ROS_INFO("new position : %f %f %f %f",Pos_Controller_U.position[0],Pos_Controller_U.position[1],Pos_Controller_U.position[2], yaw);

}

cv::Mat transform2mat (tf::StampedTransform transform) {
    double x = transform.getOrigin().x();
    double y = transform.getOrigin().y();
    double z = transform.getOrigin().z();
    tf::Matrix3x3 R(transform.getRotation());
    Mat P = (Mat_<float>(4,4) << R[0][0], R[0][1], R[0][2], x,
                                 R[1][0], R[1][1], R[1][2], y, 
                                 R[2][0], R[2][1], R[2][2], z,
                                 0, 0, 0, 1);
    return P;    
}

  void tfToPose(tf::StampedTransform &stampedTF, geometry_msgs::PoseStamped &msg)
  {
      tf::Vector3 translation = stampedTF.getOrigin();

      msg.pose.position.x = translation.x();
      msg.pose.position.y = translation.y();
      msg.pose.position.z = translation.z();

      tf::quaternionTFToMsg(stampedTF.getRotation(), msg.pose.orientation);
      msg.header.stamp = stampedTF.stamp_;
      msg.header.frame_id = stampedTF.frame_id_;
  }

///////////////////////////////////////////////////////
/////-------Cordinate Convert function---------////////
///////////////////////////////////////////////////////
void pathLocal2Global(nav_msgs::Path& path, const tf::StampedTransform transform)
{
	int length=path.poses.size();
	nav_msgs::Odometry zero;
	float angle = tf::getYaw(transform.getRotation()) - 0;
	for(int i=0;i<length;i++){
		float tmp_x = path.poses[i].pose.position.x - zero.pose.pose.position.x;
		float tmp_y = path.poses[i].pose.position.y - zero.pose.pose.position.y;
		float conv_x = cos(angle)*tmp_x - sin(angle)*tmp_y;
		float conv_y = sin(angle)*tmp_x + cos(angle)*tmp_y;
		path.poses[i].pose.position.x = conv_x + transform.getOrigin().x();
		path.poses[i].pose.position.y = conv_y + transform.getOrigin().y();
	}
}