姿态角度yaw 值限制在 0~2π 的范围内,yaw 数据限制在 -π ~ π 之间的范围内,yaw 数据是nan就f赋值为0.0

姿态角度yaw 值限制在 0~2π 的范围内,yaw 数据限制在 -π ~ π 之间的范围内,yaw 数据是nan就f赋值为0.0

1.yaw 值限制在 0~2π 的范围内

 //Yaw值限制在 0~2π 的范围内
    double normalizeYaw(double yaw) {
        while (yaw < 0.0) {
            yaw += 2 * M_PI;
        }
        while (yaw >= 2 * M_PI) {
            yaw -= 2 * M_PI;
        }
        return yaw;
    }

2.yaw 数据限制在 -π ~ π 之间的范围内

    // yaw 数据限制在 -π ~ π 之间的范围内
    double yawNormalize(double yaw) {
        while (yaw > M_PI) {
            yaw -= 2 * M_PI;
        }
        while (yaw < -M_PI) {
            yaw += 2 * M_PI;
        }
        return yaw;
    }

 3.yaw 数据是nan就f赋值为0.0

        //是nan就f赋值为0.0
        double getYaw(const geometry_msgs::Quaternion& q) {
        double yaw = tf::getYaw(q);
        return std::isnan(yaw) ? 0.0 : yaw;
    }

4.