姿态角度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.0double getYaw(const geometry_msgs::Quaternion& q) {double yaw = tf::getYaw(q);return std::isnan(yaw) ? 0.0 : yaw;}