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

void Robot::updateFrom(tf::TransformListener *tfListener) {
	using namespace Ogre;
	static tf::StampedTransform baseTF;
	static Vector3 translation = Vector3::ZERO;
	static Quaternion orientation = Quaternion::IDENTITY;
	static Quaternion qRot = Quaternion(-sqrt(0.5), 0.0f, sqrt(0.5), 0.0f)*Quaternion(-sqrt(0.5), sqrt(0.5), 0.0f, 0.0f);
	static Quaternion qYn90 = Quaternion(Degree(90), Vector3::NEGATIVE_UNIT_Y);
	static tfScalar yaw,pitch,roll;
	static Matrix3 mRot;
	
	try {
		tfListener->lookupTransform("map","base_footprint",ros::Time(0), baseTF);
		
		translation.x = baseTF.getOrigin().x();
		translation.y = baseTF.getOrigin().y();
		translation.z = baseTF.getOrigin().z();
		translation = qRot*translation + Vector3(0.0f, 1.0f, 0.0f);
		baseTF.getBasis().getEulerYPR(yaw,pitch,roll);
		mRot.FromEulerAnglesYXZ(Radian(yaw),Radian(0.0f),Radian(0.0f));
		orientation.FromRotationMatrix(mRot);
		orientation = qYn90*orientation;
		
		robot->setPosition(translation);
		robot->setOrientation(orientation);
		
	} catch (tf::TransformException ex){
		ROS_ERROR("%s",ex.what());
	}
}

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

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

/*! \brief set the class attribut */
void Segment::setSegmentAttribut(tf::StampedTransform distal_transform_,tf::StampedTransform proximal_transform_,string segment_id_)
{
	this->proximal_joint.setValue(proximal_transform_.getOrigin().x(),proximal_transform_.getOrigin().y(),proximal_transform_.getOrigin().z());
	this->distal_joint.setValue(distal_transform_.getOrigin().x(),distal_transform_.getOrigin().y(),distal_transform_.getOrigin().z()) ;
	this->vec_seg 	   = -(this->proximal_joint - this->distal_joint);
	this->vec_uni 	   = this->vec_seg.normalized() ;
	if(this->vec_uni.getX() != 0)   this->p_limite = this->vec_seg.getX()/this->vec_uni.getX();
	this->segment_id = segment_id_;	
	//-------------------------> affichage des variables du segment
/*
	std::cout <<"prox_x =" << proximal_transform_.getOrigin().x() << std::endl;	
	std::cout <<"prox_y =" << proximal_transform_.getOrigin().y() << std::endl;	
	std::cout <<"prox_z =" << proximal_transform_.getOrigin().z() << std::endl;	

	std::cout <<"dist_x =" << distal_transform_.getOrigin().x() << std::endl;	
	std::cout <<"dist_y =" << distal_transform_.getOrigin().y() << std::endl;	
	std::cout <<"dist_z =" << distal_transform_.getOrigin().z() << std::endl;

	std::cout <<"vec_seg = (" << vec_seg.x() << ";" << vec_seg.y() << ";" << vec_seg.z() << ")" << std::endl;	
	std::cout <<"vec_uni = (" << vec_uni.x() << ";" << vec_uni.y() << ";" << vec_uni.z() << ")" << std::endl;	
	std::cout <<"p_limite =" << this->vec_seg.getX()/this->vec_uni.getX() << std::endl;	
	std::cout <<"segment_id =" << segment_id << std::endl;
*/
	//------------------------------------------------------------------------------------------------------------
}

  void computeFrustum()
  {
    // compute frustum based on camera info and tf
    const double alphaY = cameraInfo.K[4];
    const double fovY = 2 * atan(cameraInfo.height / (2 * alphaY));
    tf::Vector3 up(0, 1, 0), focal(1, 0, 0);
    up = camToWorld * up;
    focal = camToWorld * focal;

    camera.fovy = fovY;
    camera.clip[0] = 0.001;
    camera.clip[1] = 12.0;
    camera.window_size[0] = cameraInfo.width;
    camera.window_size[1] = cameraInfo.height;
    camera.window_pos[0] = 0;
    camera.window_pos[1] = 0;
    camera.pos[0] = worldToCam.getOrigin().x();
    camera.pos[1] = worldToCam.getOrigin().y();
    camera.pos[2] = worldToCam.getOrigin().z();
    camera.view[0] = up.x();
    camera.view[1] = up.y();
    camera.view[2] = up.z();
    camera.focal[0] = focal.x();
    camera.focal[1] = focal.y();
    camera.focal[2] = focal.z();

    Eigen::Matrix4d viewMatrix;
    Eigen::Matrix4d projectionMatrix;
    Eigen::Matrix4d viewProjectionMatrix;
    camera.computeViewMatrix(viewMatrix);
    camera.computeProjectionMatrix(projectionMatrix);
    viewProjectionMatrix = projectionMatrix * viewMatrix;
    pcl::visualization::getViewFrustum(viewProjectionMatrix, frustum);
  }

void InputOutputLinControl::getRelativeTransformation2D(tf::StampedTransform v1, tf::StampedTransform v2, tf::Transform& relative_transform)
{
	double roll, pitch, yaw1, yaw2;
	v1.getBasis().getRPY(roll,pitch,yaw1);
	v2.getBasis().getRPY(roll,pitch,yaw2);

	double a11 = cos(yaw1 - yaw2);
	double a12 = sin(yaw1 - yaw2);
	double a13 = 0;

	double a21 = - a12;
	double a22 = a11;
	double a23 = 0;

	double a31 = 0;
	double a32 = 0;
	double a33 = 1;

	double t1 = v2.getOrigin().getX() - v1.getOrigin().getX() * a11 - v1.getOrigin().getY()*a12;
	double t2 = v2.getOrigin().getY() - v1.getOrigin().getY() * a11 + v1.getOrigin().getX()*a12;
	double t3 = 0;

	relative_transform = tf::Transform(btMatrix3x3(a11,a12,a13,a21,a22,a23,a31,a32,a33),btVector3(t1,t2,t3));

}

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

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

/* ! \brief with transforms without segment_id */
Segment::Segment(tf::StampedTransform proximal_transform_,tf::StampedTransform distal_transform_)
{
	this->proximal_joint.setValue(proximal_transform_.getOrigin().x(),proximal_transform_.getOrigin().y(),proximal_transform_.getOrigin().z());
	this->distal_joint.setValue(distal_transform_.getOrigin().x(),distal_transform_.getOrigin().y(),distal_transform_.getOrigin().z()) ;
	this->vec_seg 	   = -(proximal_joint-distal_joint);
	this->vec_uni 	   = this->vec_seg.normalized() ;
	if(this->vec_uni.getX() != 0)   this->p_limite = this->vec_seg.getX()/this->vec_uni.getX();


}

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

}

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

// compute nav fn (using wavefront assumption)
void getNavFn(double* cost_map, tf::StampedTransform &robot_to_map_transform, double* nav_fn) {
  Point robot_pt(robot_to_map_transform.getOrigin().getX() / MAP_RESOLUTION,robot_to_map_transform.getOrigin().getY() / MAP_RESOLUTION);
  Point goal_pt(goal_pose.getOrigin().getX() / MAP_RESOLUTION, goal_pose.getOrigin().getY() / MAP_RESOLUTION);
  ROS_INFO("[getNavFn]\trobot_pt: (%d,%d), goal_pt(%d,%d)",robot_pt.x,robot_pt.y,goal_pt.x,goal_pt.y);

  // setup init map
  bool init_set_map[MAP_CELLS];
  for (int i=0; i<MAP_CELLS;i++) {
    init_set_map[i] = false;
  }
  setVal(init_set_map,goal_pt.x,goal_pt.y,true);

  //updateMapDijkstra(nav_fn, init_set_map, goal_pt, cost_map, INT_MAX);
  LPN2(nav_fn, cost_map, goal_pt, robot_pt);
}

	bool poseTracker::newPose(tf::StampedTransform transform){
		
		Eigen::Vector3f curr(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
		pose.push(curr);
		if(pose.size() < 50){
			return true;
		}

		float squaredNorm = (pose.front() - curr).squaredNorm();
		pose.pop();
		bool tmp = squaredNorm > 0.2;
		std::cout << tmp << "  Max: " << squaredNorm << std::endl;
		return squaredNorm > 0.2;

	}

// 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");
	}
}