C++ Quaternion::x方法代码示例

void ATTCallback (IvyClientPtr app, void* data , int argc, char **argv)
{
	double att_unit_coef= 0.0139882;
	double phi, theta, yaw;
		
	sscanf(argv[0],"%lf %lf %lf %*d %*d %*d",&phi,&theta,&yaw);
	phi*=att_unit_coef*DEG2RAD;
	theta*=-att_unit_coef*DEG2RAD;
	yaw*=-att_unit_coef*DEG2RAD;
	
	q.setRPY(phi,theta,yaw);
	
	//ROS_INFO("Phi %f; Theta %f; Yaw %f", phi,theta,yaw);
	//ROS_INFO("q1 %f; q2 %f; q3 %f; q4 %f", q.x(),q.y(),q.z(),q.w());

	imu_data.header.stamp = ros::Time::now();
	imu_data.orientation.x=q.x();
	imu_data.orientation.y=q.y();
	imu_data.orientation.z=q.z();
	imu_data.orientation.w=q.w();
	
	imu_message.publish(imu_data);
	
	//Only temporary until the rates are equal
	att_data.header.stamp = ros::Time::now();
	att_data.orientation.x=q.x();
	att_data.orientation.y=q.y();
	att_data.orientation.z=q.z();
	att_data.orientation.w=q.w();

	att_message.publish(att_data);
}

tf::Matrix3x3 JPCT_Math::quatToMatrix(tf::Quaternion q) {
    tf::Matrix3x3 matrix; 
    float norm = magnitudeSquared(q);
    float s = (double)norm > 0.0?2.0f / norm:0.0F;
    float xs = q.x() * s;
    float ys = q.y() * s;
    float zs = q.z() * s;
    float xx = q.x() * xs;
    float xy = q.x() * ys;
    float xz = q.x() * zs;
    float xw = q.w() * xs;
    float yy = q.y() * ys;
    float yz = q.y() * zs;
    float yw = q.w() * ys;
    float zz = q.z() * zs;
    float zw = q.w() * zs;
    matrix[0][0] = 1.0F - (yy + zz);
    matrix[1][0] = xy - zw;
    matrix[2][0] = xz + yw;
    matrix[0][1] = xy + zw;
    matrix[1][1] = 1.0F - (xx + zz);
    matrix[2][1] = yz - xw;
    matrix[0][2] = xz - yw;
    matrix[1][2] = yz + xw;
    matrix[2][2] = 1.0F - (xx + yy);

    return matrix;
}

void Hand_filter::update(tf::Vector3 p, tf::Quaternion& q){

    if(b_first){
        p_filter_buffer.push_back(p);
        q_filter_buffer.push_back(q);
        if(p_filter_buffer.size() == p_filter_buffer.capacity()){
            b_first = false;
            ROS_INFO("====== hand filter ======");
           // ROS_INFO("buffer full: %d",p_filter_buffer.size());
            ROS_INFO("p: %f %f %f",p.x(),p.y(),p.z());
            ROS_INFO("q: %f %f %f %f",q.x(),q.y(),q.z(),q.w());

            k_position(0) = p.x();k_position(1) = p.y(); k_position(2) = p.z();
            kalman_filter.Init(k_position);

            q_tmp = q;
            p_tmp = p;

        }
    }else{


        /// Orientation filter
       if(jumped(q,q_tmp,q_threashold)){
            ROS_INFO("q jumped !");
            q = q_tmp;
        }

       q_attractor(q,up);
       q = q_tmp.slerp(q,0.1);


       /// Position filter
        if(!jumped(p,p_tmp,p_threashold)){

            k_measurement(0) = p.x();
            k_measurement(1) = p.y();
            k_measurement(2) = p.z();

        }else{
            ROS_INFO("p jumped !");
            k_measurement(0) = p_tmp.x();
            k_measurement(1) = p_tmp.y();
            k_measurement(2) = p_tmp.z();
        }

        kalman_filter.Update(k_measurement,0.001);
        kalman_filter.GetPosition(k_position);
        p.setValue(k_position(0),k_position(1),k_position(2));



        q_tmp = q;
        p_tmp = p;

    }


}

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

  ss << "(" << p_quat.x() << ", " << p_quat.y() << ", " << p_quat.z() << ", " << p_quat.w() << ")";

  return ss.str();
}

/**
  * Packs current state in a odom message. Needs a quaternion for conversion.
  */
void pack_pose(tf::Quaternion& q, nav_msgs::Odometry& odom)
{
    q.setRPY(0, 0, _theta);

    odom.header.stamp = ros::Time::now();

    odom.pose.pose.position.x = _x;
    odom.pose.pose.position.y = _y;

    odom.pose.pose.orientation.x = q.x();
    odom.pose.pose.orientation.y = q.y();
    odom.pose.pose.orientation.z = q.z();
    odom.pose.pose.orientation.w = q.w();
}

void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){

    if(bFirst){

        origin_buffer.push_back(origin);
        orientation_buffer.push_back(orientation);

        if(origin_buffer.size() == origin_buffer.capacity()){
            bFirst = false;
            ROS_INFO("====== jump filter full ======");
        }

    }else{

        origin_tmp      = origin_buffer[origin_buffer.size()-1];
        orientation_tmp = orientation_buffer[orientation_buffer.size()-1];

        if(bDebug){
            std::cout<< "=== jum debug === " << std::endl;
            std::cout<< "p    : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl;
            std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl;
            std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl;

            std::cout<< "q    : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() <<  "\t" << orientation.w() << std::endl;
            std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl;
            std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl;
        }

        /// Position jump
        if(jumped(origin,origin_tmp,origin_threashold)){
            ROS_INFO("position jumped !");
            origin = origin_tmp;
           // exit(0);
        }else{
            origin_buffer.push_back(origin);
        }

        /// Orientation jump
        if(jumped(orientation,orientation_tmp,orientation_threashold)){
            ROS_INFO("orientation jumped !");
            orientation = orientation_tmp;
            //exit(0);
        }else{
            orientation_buffer.push_back(orientation);
        }
    }
}

//.........这里部分代码省略.........
            uint16_t rcv_pkg_id = *(uint16_t *)(rsp_frame +2);
            if(rcv_pkg_id == pkg_id) {
                //read the commands to display
                cmd_field[0] = rsp_frame[0];
                cmd_field[1] = rsp_frame[1];
                cmd_field[2] = 0;

                //Time sent from RTOS to check if the moving base has been reset
                uint8_t reset_robot = *(uint8_t *)(rsp_frame + 16);
                //Calculate the x,y,z,v,w odometry data
                double left_speed = *(int16_t *)(rsp_frame + 4)*M_PI/98500;
                double right_speed = *(int16_t *)(rsp_frame + 6)*M_PI/98500;

                //Get the newly traveled distance on each wheel
                if (rcv_pkg_id == 0 || reset_robot == 1 ) {
                    left_path_offset = *(int32_t *)(rsp_frame +8)*M_PI/98500;
                    right_path_offset = *(int32_t *)(rsp_frame + 12)*M_PI/98500;
                    left_path = 0;
                    right_path = 0;
                }
                else {
                    left_path = *(int32_t *)(rsp_frame +8)*M_PI/98500 - left_path_offset;
                    right_path = *(int32_t *)(rsp_frame + 12)*M_PI/98500 - right_path_offset;
                }

                //Calculate and publish the odometry data
                double left_delta = left_path - left_path1;
                double right_delta = right_path - right_path1;

                //only if the robot has traveled a significant distance will be calculate the odometry
                if(!((left_delta < 0.0075 && left_delta > -0.0075) && (right_delta < 0.0075 && right_delta > -0.0075))) {
                    //Calculate the pose in reference to world base
                    theta += (right_delta - left_delta)/0.3559;

                    //translate theta to [-pi; pi]
                    if(theta > (2*M_PI)) theta -= 2*M_PI;
                    if (theta < 0) theta += 2*M_PI;

                    //Calculate displacement
                    double disp = (right_delta + left_delta)/2;
                    x += disp*cos(theta);
                    y += disp*sin(theta);

                    //Calculate the twist in reference to the robot base
                    linear_x = (right_speed + left_speed)/2;
                    angular_z = (right_speed - left_speed)/0.362;

                    left_path1 = left_path;
                    right_path1 = right_path;

                }

                // Infer the rotation angle and publish transform
                move_base_quat = tf::Quaternion(tf::Vector3(0, 0, 1), theta);
                transform_broadcaster.sendTransform(tf::StampedTransform(
                                                        tf::Transform(move_base_quat, tf::Vector3(x,y,0)),
                                                        current_time, "odom", "base_robot"));

                //Publish the odometry message over ROS
                move_base_odom.header.stamp = current_time;
                //move_base_odom.header.stamp = ros::Time::now();
                //set the position
                move_base_odom.pose.pose.position.x = x;
                move_base_odom.pose.pose.position.y = y;
                move_base_odom.pose.pose.position.z = 0;
                move_base_odom.pose.pose.orientation.x = move_base_quat.x();
                move_base_odom.pose.pose.orientation.y = move_base_quat.y();
                move_base_odom.pose.pose.orientation.z = move_base_quat.z();
                move_base_odom.pose.pose.orientation.w = move_base_quat.w();

                //Set the velocity
                move_base_odom.child_frame_id = "base_robot";
                move_base_odom.twist.twist.linear.x = linear_x;
                move_base_odom.twist.twist.linear.y = 0;
                move_base_odom.twist.twist.angular.z = angular_z;

                //Push back the pose to path message
                pathMsg.header = move_base_odom.header;
                geometry_msgs::PoseStamped pose_stamped;
                pose_stamped.header = move_base_odom.header;
                pose_stamped.pose = move_base_odom.pose.pose;
                pathMsg.poses.push_back(pose_stamped);
                //publish the odom and path message
                odom_pub.publish(move_base_odom);
                path_pub.publish(pathMsg);

                //ROS_INFO("MCU responded: %s\t%d\t%f\t%f\t%f\t%f\t%d", cmd_field,\
                rcv_pkg_id,\
                left_speed,\
                right_speed,\
                left_path,\
                right_path,\
                reset_robot);

            }
            else {
                ROS_INFO("frame %d corrupted !", pkg_id);
                while(read(fd, rsp_frame, 1) > 0);
            }
        }