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

vpHomogeneousMatrix tfToVisp(tf::StampedTransform matrix_tf){
	vpHomogeneousMatrix matrix_visp;
	matrix_visp[0][0]=matrix_tf.getBasis()[0][0]; matrix_visp[0][1]=matrix_tf.getBasis()[0][1]; matrix_visp[0][2]=matrix_tf.getBasis()[0][2]; matrix_visp[0][3]=matrix_tf.getOrigin().x();
	matrix_visp[1][0]=matrix_tf.getBasis()[1][0]; matrix_visp[1][1]=matrix_tf.getBasis()[1][1]; matrix_visp[1][2]=matrix_tf.getBasis()[1][2]; matrix_visp[1][3]=matrix_tf.getOrigin().y();
	matrix_visp[2][0]=matrix_tf.getBasis()[2][0]; matrix_visp[2][1]=matrix_tf.getBasis()[2][1]; matrix_visp[2][2]=matrix_tf.getBasis()[2][2]; matrix_visp[2][3]=matrix_tf.getOrigin().z();
	return matrix_visp;
}

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

void InputOutputLinControl::getTrueRobotPose(double displacement, tf::StampedTransform robot_poseB, tf::StampedTransform& true_robot_pose)
{
	true_robot_pose.frame_id_ = robot_poseB.frame_id_;
	true_robot_pose.stamp_ = robot_poseB.stamp_;

	double roll, pitch, yaw;
	robot_poseB.getBasis().getRPY(roll,pitch,yaw);

	tf::Vector3 v;
	v.setX(robot_poseB.getOrigin().getX() - displacement*cos(yaw));
	v.setY(robot_poseB.getOrigin().getY() - displacement*sin(yaw));
	v.setZ(robot_poseB.getOrigin().getZ());

	true_robot_pose.setOrigin(v);

	true_robot_pose.setRotation(robot_poseB.getRotation());
}

void InputOutputLinControl::realRobotPoseB(double displacement, tf::StampedTransform real_robot_pose, tf::StampedTransform& real_robot_poseB)
{
	real_robot_poseB.frame_id_ = real_robot_pose.frame_id_;
	real_robot_poseB.stamp_ = real_robot_pose.stamp_;
	real_robot_poseB.child_frame_id_ = real_robot_pose.child_frame_id_;

	tf::Vector3 v;
	double roll, pitch, yaw;
	real_robot_pose.getBasis().getRPY(roll,pitch,yaw);

	v.setX(real_robot_pose.getOrigin().getX() + displacement*cos(yaw));
	v.setY(real_robot_pose.getOrigin().getY() + displacement*sin(yaw));
	v.setZ(real_robot_pose.getOrigin().getZ());
	real_robot_poseB.setOrigin(v);

	real_robot_poseB.setRotation(real_robot_pose.getRotation());
}

  void foutAppendTF(tf::StampedTransform &transform, std::ofstream &fout)
  {
    tf::Matrix3x3 rotMatrix = transform.getBasis();
    tf::Vector3 originVector = transform.getOrigin();

    
    tfScalar roll, pitch, yaw;
    
    
    
    
    rotMatrix.getRPY(roll, pitch, yaw);
    
    fout<< originVector.x() << " "
        << originVector.y() << " "
        << originVector.z() << " "
        << pcl::rad2deg(roll) << " "
        << pcl::rad2deg(pitch) << " "
        << pcl::rad2deg(yaw) << std::endl;
    
    
  }

bool InputOutputLinControl::updateRobotPoseByModel(double timestep, double linear_vel, double angular_vel, tf::StampedTransform& pose)
{

	pose.stamp_ = ros::Time::now();
	double roll, pitch, yaw;
	pose.getBasis().getRPY(roll,pitch,yaw);

	tf::Vector3 v;

	double theta = yaw + angular_vel * timestep;

	//Eulerian integration
	v.setX(pose.getOrigin().getX() + linear_vel * timestep * cos(theta));
	v.setY(pose.getOrigin().getY() + linear_vel * timestep * sin(theta));
	v.setZ(pose.getOrigin().getZ());

	pose.setOrigin(v);
	pose.setRotation(tf::createQuaternionFromYaw(theta));
	ROS_INFO("New orientation [%f]",theta);

	return true;
}

  void printTF(tf::StampedTransform &transform, std::string str)
  {
    tf::Matrix3x3 rotMatrix = transform.getBasis();
    tf::Vector3 originVector = transform.getOrigin();

    
    tfScalar roll, pitch, yaw;
    
    
    
    
    rotMatrix.getRPY(roll, pitch, yaw);
    
    
    ROS_INFO("TF %s (x y z r p y): %f %f %f %f %f %f", 
             str.c_str(),
             originVector.x(),
             originVector.y(), 
             originVector.z(),
             pcl::rad2deg(roll),
             pcl::rad2deg(pitch),
             pcl::rad2deg(yaw));
  }

bool InputOutputLinControl::getRobotCommands(double displacement, tf::StampedTransform robot_pose, double k1, double k2, geometry_msgs::Pose poseB, geometry_msgs::Twist velB, double& linear_vel, double& angular_vel)
{
	double roll, pitch, yaw;
	robot_pose.getBasis().getRPY(roll,pitch,yaw);

	double a = cos(yaw);
	double b = sin(yaw);
	double c = -sin(yaw)/displacement;
	double d = cos(yaw)/displacement;

	double u1 = velB.linear.x + k1 * (poseB.position.x - robot_pose.getOrigin().getX());
	double u2 = velB.linear.y + k2 * (poseB.position.y - robot_pose.getOrigin().getY());

	//double u1 = k1 * (poseB.position.x - robot_pose.getOrigin().getX());
	//double u2 = k2 * (poseB.position.y - robot_pose.getOrigin().getY());

	linear_vel = a * u1 + b * u2;
	angular_vel = c * u1 + d * u2;

	geometry_msgs::PoseStamped poseR;
	poseR.header.frame_id = robot_pose.frame_id_;
	poseR.header.stamp = robot_pose.stamp_;
	poseR.pose.position.x = robot_pose.getOrigin().getX();
	poseR.pose.position.y = robot_pose.getOrigin().getY();
	poseR.pose.position.z = robot_pose.getOrigin().getZ();
	poseR.pose.orientation.x = robot_pose.getRotation().getX();
	poseR.pose.orientation.y = robot_pose.getRotation().getY();
	poseR.pose.orientation.z = robot_pose.getRotation().getZ();
	poseR.pose.orientation.w = robot_pose.getRotation().getW();
	ROS_INFO("traiettoria del robot x [%f]",robot_pose.getOrigin().getX());
	ROS_INFO("traiettoria del robot y [%f]",robot_pose.getOrigin().getY());
	local_path.poses.push_back(poseR);
	local_path_.publish(local_path);

	return true;

}

tf::Transform OdomTf::get_tf_from_stamped_tf(tf::StampedTransform sTf) {
    tf::Transform tf(sTf.getBasis(), sTf.getOrigin()); //construct a transform using elements of sTf
    return tf;
}