| 190 | } |
| 191 | |
| 192 | bool ImuAccVisual::checkQuaternionValidity( |
| 193 | const sensor_msgs::msg::Imu::ConstSharedPtr msg) |
| 194 | { |
| 195 | // REP-145: orientation_covariance[0] == -1 signals "not estimated". |
| 196 | if (msg->orientation_covariance[0] == -1.0) |
| 197 | { |
| 198 | return false; |
| 199 | } |
| 200 | double x = msg->orientation.x, y = msg->orientation.y, |
| 201 | z = msg->orientation.z, w = msg->orientation.w; |
| 202 | // Near-zero length indicates the default (0, 0, 0, 0) quaternion, |
| 203 | // which would silently zero out any vector multiplication. |
| 204 | if (std::sqrt(x * x + y * y + z * z + w * w) < 0.0001) |
| 205 | { |
| 206 | return false; |
| 207 | } |
| 208 | return true; |
| 209 | } |
| 210 | |
| 211 | } // namespace rviz_imu_plugin |
nothing calls this directly
no outgoing calls
no test coverage detected