C++ Rotation::GetQuaternion方法代码示例

geometry_msgs::Quaternion conversions::KDLRotToQuaternionMsg(const KDL::Rotation & in){
	geometry_msgs::Quaternion out;
	in.GetQuaternion(out.x, out.y, out.z, out.w);
	return out;
}

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
	PointCloud cloud;
	pcl::fromROSMsg(*msg, cloud);

	g_cloud_frame = cloud.header.frame_id;
	g_cloud_ready = true;

	if(!g_head_depth_ready) return;

	Mat img3D;
	img3D = Mat::zeros(cloud.height, cloud.width, CV_32FC3);
	//img3D.create(cloud.height, cloud.width, CV_32FC3);
	
	int yMin, xMin, yMax, xMax;
	yMin = 0; xMin = 0;
	yMax = img3D.rows; xMax = img3D.cols;

	//get 3D from depth
	for(int y = yMin ; y < img3D.rows; y++) {
		Vec3f* img3Di = img3D.ptr<Vec3f>(y);
	
		for(int x = xMin; x < img3D.cols; x++) {
			pcl::PointXYZ p = cloud.at(x,y);
			if((p.z>g_head_depth-0.2) && (p.z<g_head_depth+0.2)) {
				img3Di[x][0] = p.x*1000;
				img3Di[x][1] = p.y*1000;
				img3Di[x][2] = hypot(img3Di[x][0], p.z*1000); //they seem to have trained with incorrectly projected 3D points
				//img3Di[x][2] = p.z*1000;
			} else {
				img3Di[x] = 0;
			}
		}
	}
	
	g_means.clear();
	g_votes.clear();
	g_clusters.clear();
	
	//do the actual estimate
	estimator.estimate( 	img3D,
							g_means,
							g_clusters,
							g_votes,
							g_stride,
							g_maxv,
							g_prob_th,
							g_larger_radius_ratio,
							g_smaller_radius_ratio,
							false,
							g_th
						);
	
	//assuming there's only one head in the image!
	if(g_means.size() > 0) {	
		geometry_msgs::PoseStamped pose_msg;
		pose_msg.header.frame_id = msg->header.frame_id;
	
		cv::Vec<float,POSE_SIZE> pose(g_means[0]);
	
		KDL::Rotation r = KDL::Rotation::RPY(
											 from_degrees( pose[5]+180+g_roll_bias ), 
											 from_degrees(-pose[3]+180+g_pitch_bias),
											 from_degrees(-pose[4]+    g_yaw_bias  )
											);
		double qx, qy, qz, qw;
		r.GetQuaternion(qx, qy, qz, qw);
	
		geometry_msgs::PointStamped head_point;
		geometry_msgs::PointStamped head_point_transformed;
	
		head_point.header = pose_msg.header;

		head_point.point.x = pose[0]/1000;
		head_point.point.y = pose[1]/1000;
		head_point.point.z = pose[2]/1000;
	
		try {
			listener->waitForTransform(head_point.header.frame_id, g_head_target_frame, ros::Time::now(), ros::Duration(2.0));
			listener->transformPoint(g_head_target_frame, head_point, head_point_transformed);
		} catch(tf::TransformException ex) {
			ROS_WARN(
				"Transform exception, when transforming point from %s to %s\ndropping pose (don't worry, this is probably ok)",
				head_point.header.frame_id.c_str(), g_head_target_frame.c_str());
				ROS_WARN("Exception was %s", ex.what());
			return;
		}
		
		tf::Transform trans;
		
		pose_msg.pose.position = head_point_transformed.point;
		pose_msg.header.frame_id = head_point_transformed.header.frame_id;
	
		pose_msg.pose.orientation.x = qx;
		pose_msg.pose.orientation.y = qy;
		pose_msg.pose.orientation.z = qz;
		pose_msg.pose.orientation.w = qw;

		trans.setOrigin(tf::Vector3(
			pose_msg.pose.position.x, 
//.........这里部分代码省略.........

    bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface){
    	

    	m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);
    	
    	geometry_msgs::Pose pose;
    	cmdCartPose.read(pose);
#if 1
    	std::cout << "A new pose arrived" << std::endl;
    	std::cout << "position: " << pose.position.x <<  " " << pose.position.y << " " << pose.position.z  << std::endl;
#endif


		m_traject_end.p.x(pose.position.x);
		m_traject_end.p.y(pose.position.y);
		m_traject_end.p.z(pose.position.z);
		m_traject_end.M=Rotation::Quaternion(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);

		// get current position
		geometry_msgs::Pose pose_meas;

		m_position_meas_port.read(pose_meas);
		m_traject_begin.p.x(pose_meas.position.x);
		m_traject_begin.p.y(pose_meas.position.y);
		m_traject_begin.p.z(pose_meas.position.z);
		m_traject_begin.M=Rotation::Quaternion(pose_meas.orientation.x,pose_meas.orientation.y,pose_meas.orientation.z,pose_meas.orientation.w);

		KDL::Rotation errorRotation = (m_traject_end.M)*(m_traject_begin.M.Inverse());

//		KDL::Rotation tmp=errorRotation;
//		double q1, q2, q3, q4;
//		cout << "tmp" << endl;
//		cout << tmp(0,0) << ", " << tmp(0,1) << ", " << tmp(0,2) << endl;
//		cout << tmp(1,0) << ", " << tmp(1,1) << ", " << tmp(1,2) << endl;
//		cout << tmp(2,0) << ", " << tmp(2,1) << ", " << tmp(2,2) << endl;
//		cout << endl;
//		tmp.GetQuaternion(q1,q2,q3,q4);
//		cout << "tmp quaternion: " << q1 << q2 << q3 << q4 << endl;

		double x,y,z,w;
		errorRotation.GetQuaternion(x,y,z,w);

		Eigen::AngleAxis<double> aa;
		aa = Eigen::Quaterniond(w,x,y,z);
		currentRotationalAxis = aa.axis();
		deltaTheta = aa.angle();

//		std::cout << "----EIGEN---------" << std::endl << "currentRotationalAxis: "  << std::endl << currentRotationalAxis << std::endl;
//		std::cout << "deltaTheta" << deltaTheta << std::endl;

//		currentRotationalAxis[0]=x;
//		currentRotationalAxis[1]=y;
//		currentRotationalAxis[2]=z;
//		if(currentRotationalAxis.norm() > 0.001){
//			currentRotationalAxis.normalize();
//			deltaTheta = 2*acos(w);
//		}else{
//			currentRotationalAxis.setZero();
//			deltaTheta = 0.0;
//		}

//		std::cout << "----KDL-----------" << std::endl << "currentRotationalAxis: "  << std::endl << currentRotationalAxis << std::endl;
//		std::cout << "deltaTheta" << deltaTheta << std::endl;

		std::vector<double> cartPositionCmd = std::vector<double>(3,0.0);
		cartPositionCmd[0] = pose.position.x;
		cartPositionCmd[1] = pose.position.y;
		cartPositionCmd[2] = pose.position.z;

		std::vector<double> cartPositionMsr = std::vector<double>(3,0.0);
		//the following 3 assignments will get over written except for the first time
		cartPositionMsr[0] = pose_meas.position.x;
		cartPositionMsr[1] = pose_meas.position.y;
		cartPositionMsr[2] = pose_meas.position.z;

		std::vector<double> cartVelocity = std::vector<double>(3,0.0);

		if ((int)motionProfile.size() == 0){//Only for the first run
			for(int i = 0; i < 3; i++)
			{
				cartVelocity[i] = 0.0;
			}
		}else{
			for(int i = 0; i < 3; i++)
			{
				cartVelocity[i] = motionProfile[i].Vel(m_time_passed);
				cartPositionMsr[i] = motionProfile[i].Pos(m_time_passed);
			}
		}

		motionProfile.clear();

		// Set motion profiles
		for(int i = 0; i < 3; i++){
			motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[i], m_maximum_acceleration[i]));
			motionProfile[i].SetProfile(cartPositionMsr[i], cartPositionCmd[i], cartVelocity[i]);
		}
		//motionProfile for theta
		motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[3], m_maximum_acceleration[3]));
		motionProfile[3].SetProfile(0.0,deltaTheta,0.0);
//.........这里部分代码省略.........

// construct rotation
urdf::Rotation toUrdf(const KDL::Rotation & r)
{
    double x,y,z,w;
    r.GetQuaternion(x,y,z,w);
    return urdf::Rotation(x,y,z,w);
}

int main(int argc, char **argv)
{

        float x=0.0f;
        float y=0.0f;
        float z=0.0f;
        float roll=0.0f;
        float pitch=0.0f;
        float yaw=0.0f;

        ros::init(argc, argv, "dan_tester");

        ros::NodeHandle n;

        ros::Subscriber sub = n.subscribe( "lar_comau/comau_joint_states",1,state_callback );
        ros::Publisher joint_state_pub = n.advertise<sensor_msgs::JointState>("lar_comau/comau_joint_state_publisher", 1);
        ros::Publisher cartesian_controller = n.advertise<lar_comau::ComauCommand>("lar_comau/comau_cartesian_controller", 1);

        ros::Rate loop_rate(10);

        //robot
        std::string robot_desc_string;
        n.param("robot_description", robot_desc_string, std::string());
        lar_comau::ComauSmartSix robot(robot_desc_string,"base_link", "link6");


        //ROS_INFO("\n\n%s\n\n",argv[1]);
        initPoses();
        int count = 0;
        while (ros::ok())
        {


                sensor_msgs::JointState msg;
                geometry_msgs::Pose pose;


                //std::stringstream ss;
                //ss << "hello world " << count;
                //msg.data = ss.str();
                std::string command;
                int command_index;


                cout << "Please enter pose value: ";
                cin >> command_index;
                if(command_index<my_poses.size() && command_index>=0) {
                        std::cout << "OK command "<<command_index<<std::endl;
                        MyPose mypose = my_poses[command_index];
                        std::cout << mypose.x << "," <<mypose.y<<std::endl;
                        pose.position.x = mypose.x;
                        pose.position.y = mypose.y;
                        pose.position.z = mypose.z;

                        KDL::Rotation rot = KDL::Rotation::RPY(
                                mypose.roll*M_PI/180.0f,
                                mypose.pitch*M_PI/180.0f,
                                mypose.yaw*M_PI/180.0f
                                );

                        double qx,qy,qz,qw;
                        rot.GetQuaternion(qx,qy,qz,qw);
                        pose.orientation.x = qx;
                        pose.orientation.y = qy;
                        pose.orientation.z = qz;
                        pose.orientation.w = qw;

                        lar_comau::ComauCommand comau_command;
                        comau_command.pose = pose;
                        comau_command.command = "joint";

                        cartesian_controller.publish(comau_command);
                        ros::spinOnce();
                        std::cout << "Published "<<comau_command<<std::endl;
                }





        }


        return 0;
}

bool publishMarkerArray(geometry_msgs::Pose axis, std::vector<geometry_msgs::Pose> trajectoyPoints, std::string refFrameName){
	visualization_msgs::MarkerArray tmpMarkerArray;

	KDL::Rotation axisR = KDL::Rotation::Quaternion(axis.orientation.x, axis.orientation.y,axis.orientation.z,axis.orientation.w);
	KDL::Rotation deltaR = KDL::Rotation::Quaternion(0.0,std::sin(-PI/4),0.0,std::cos(-PI/4));
	KDL::Rotation markerR = axisR*deltaR;

	visualization_msgs::Marker axisMarker;
	axisMarker.header.frame_id = refFrameName;
	axisMarker.header.stamp = ros::Time();
	axisMarker.ns = "articulationNS";
	axisMarker.id = 0;
	axisMarker.type = visualization_msgs::Marker::ARROW;
	axisMarker.action = visualization_msgs::Marker::ADD;
	axisMarker.scale.x = 2;
	axisMarker.scale.y = 2;
	axisMarker.scale.z = 2;
	axisMarker.color.a = 1.0;
	axisMarker.color.r = 0.0;
	axisMarker.color.g = 1.0;
	axisMarker.color.b = 0.0;

	//std::cout << "Before: "<<axisMarker.pose.orientation.x << " " << axisMarker.pose.orientation.y << " " << axisMarker.pose.orientation.z << " " << axisMarker.pose.orientation.w << std::endl;
	markerR.GetQuaternion(axisMarker.pose.orientation.x,axisMarker.pose.orientation.y,axisMarker.pose.orientation.z,axisMarker.pose.orientation.w);
	//std::cout << "After: "<<axisMarker.pose.orientation.x << " " << axisMarker.pose.orientation.y << " " << axisMarker.pose.orientation.z << " " << axisMarker.pose.orientation.w << std::endl;


	visualization_msgs::Marker axisText;
	axisText.header.frame_id = refFrameName;
	axisText.header.stamp = ros::Time();
	axisText.ns = "articulationNS";
	axisText.id = 1;
	axisText.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
	axisText.action = visualization_msgs::Marker::ADD;
	axisText.scale.x = 0.5;
	axisText.scale.y = 0.5;
	axisText.scale.z = 0.5;
	axisText.color.a = 1.0;
	axisText.color.r = 1.0;
	axisText.color.g = 1.0;
	axisText.color.b = 1.0;

	visualization_msgs::Marker trajPoint;
	trajPoint.header.frame_id = refFrameName;
	trajPoint.header.stamp = ros::Time();
	trajPoint.ns = "articulationNS";
	trajPoint.type = visualization_msgs::Marker::SPHERE;
	trajPoint.action = visualization_msgs::Marker::ADD;
	trajPoint.scale.x = .3;
	trajPoint.scale.y = .3;
	trajPoint.scale.z = .3;
	trajPoint.color.a = 1.0;
	trajPoint.color.r = 0.0;
	trajPoint.color.g = 1.0;
	trajPoint.color.b = 0.0;

	axisText.pose = axis;
	//std::stringstream s;
	//s << "(" << axisText.pose.position.x << ", " << axisText.pose.position.y << ")" ;
	axisText.text = "Axis of Rotation";

	tmpMarkerArray.markers.push_back(axisMarker);
	tmpMarkerArray.markers.push_back(axisText);

	for(int i=0; i<(int)trajectoyPoints.size(); i++){
		trajPoint.pose = trajectoyPoints[i];
		trajPoint.id = 2 + i;
		tmpMarkerArray.markers.push_back(trajPoint);
	}

	ROS_INFO("Publishing MarkerArray: Size=%d",(int)tmpMarkerArray.markers.size());
	vis_pub.publish( tmpMarkerArray );

	return true;
}

bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface)
{

	m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);

	geometry_msgs::Pose pose;
	cmdCartPose.read(pose);
	desired_pose = pose;

#if 1
	std::cout << "A new pose arrived" << std::endl;
	std::cout << "position: " << pose.position.x << " " << pose.position.y << " " << pose.position.z << std::endl;
#endif

	m_traject_end.p.x(pose.position.x);
	m_traject_end.p.y(pose.position.y);
	m_traject_end.p.z(pose.position.z);
	m_traject_end.M = Rotation::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z,
																					pose.orientation.w);

	// get current position
	geometry_msgs::Pose pose_meas;

	m_position_meas_port.read(pose_meas);
	m_traject_begin.p.x(pose_meas.position.x);
	m_traject_begin.p.y(pose_meas.position.y);
	m_traject_begin.p.z(pose_meas.position.z);
	m_traject_begin.M = Rotation::Quaternion(pose_meas.orientation.x, pose_meas.orientation.y, pose_meas.orientation.z,
																						pose_meas.orientation.w);

	xi = pose_meas.position.x;
	yi = pose_meas.position.y;
	zi = pose_meas.position.z;
	xf = pose.position.x;
	yf = pose.position.y;
	zf = pose.position.z;

	TrajVectorMagnitude = sqrt((xf - xi) * (xf - xi) + (yf - yi) * (yf - yi) + (zf - zi) * (zf - zi));
	TrajVectorDirection.x = (xf - xi) / TrajVectorMagnitude;
	TrajVectorDirection.y = (yf - yi) / TrajVectorMagnitude;
	TrajVectorDirection.z = (zf - zi) / TrajVectorMagnitude;

	double tx, ty, tz;
	tx = abs(xf - xi) / m_maximum_velocity[0];
	ty = abs(yf - yi) / m_maximum_velocity[0];
	tz = abs(zf - zi) / m_maximum_velocity[0];

	t_sync = TrajVectorMagnitude / m_maximum_velocity[0];

	KDL::Rotation errorRotation = (m_traject_end.M) * (m_traject_begin.M.Inverse());

	double x, y, z, w;
	errorRotation.GetQuaternion(x, y, z, w);

	Eigen::AngleAxis<double> aa;
	aa = Eigen::Quaterniond(w, x, y, z);
	currentRotationalAxis = aa.axis();
	deltaTheta = aa.angle();
	theta_vel = deltaTheta / t_sync;

	cout << "[CG::GenerateProfiles]:" << endl;

	m_time_begin = os::TimeService::Instance()->getTicks();
	m_time_passed = 0;

	return true;
}

 void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t)
 {
   double x, y, z, w;
   k.GetQuaternion(x, y, z, w);
   t = tf::Quaternion(x, y, z, w);
 }