The State Timer May Publish Uninitialized Odometry Data

[peter-ao209]

1. Problem Description

RACER updates the UAV's position, velocity, and yaw through an odometry callback, while an independent timer periodically publishes the UAV state.

In the current configuration:

• Odometry messages are received approximately every 10 ms.
• State messages are published approximately every 40 ms.
• The odometry callback and state timer callback are executed asynchronously.

Although odometry messages are published more frequently, there is no guarantee that the first odometry callback will be executed before the state timer callback during node startup.

If the state timer is triggered first, the state publication function may read variables such as odom_yaw_ before they have been assigned by the odometry callback. Because these variables are not explicitly initialized, they may contain arbitrary values, NaN, or Inf, which may then be published directly through the UAV state topic.

After the first odometry message is received, these variables are overwritten with valid data, and subsequent state messages usually become normal. Therefore, the issue is intermittent and is most likely to occur during node startup or after a temporary odometry interruption.

A typical execution sequence is shown below:

FSMData is created with uninitialized odometry variables
        ↓
The state timer and odometry subscriber are started
        ↓
The state timer callback is triggered first
        ↓
The callback reads the uninitialized odom_yaw_
        ↓
An arbitrary value, NaN, or Inf is published
        ↓
The first odometry message arrives
        ↓
The odometry callback writes valid data
        ↓
Subsequent state messages become normal

2. Code Analysis

2.1 Odometry Variables Are Not Explicitly Initialized

The relevant variables are declared as follows:

Eigen::Vector3d odom_pos_, odom_vel_;
Eigen::Quaterniond odom_orient_;
double odom_yaw_;

These variables are not assigned initial values when they are declared. In particular:

double odom_yaw_;

If this variable is read before its first assignment, its value is indeterminate.

Eigen vectors and quaternions should also not be assumed to be automatically initialized to zero or to an identity quaternion.

2.2 Odometry Data Is Updated Only by the Callback

The internal odometry state is written only after an odometry message is received:

void FastExplorationFSM::odometryCallback(
    const nav_msgs::OdometryConstPtr& msg) {
  fd_->odom_pos_(0) = msg->pose.pose.position.x;
  fd_->odom_pos_(1) = msg->pose.pose.position.y;
  fd_->odom_pos_(2) = msg->pose.pose.position.z;

  // Update velocity, orientation, and odom_yaw_
}

However, there is no flag indicating whether the first valid odometry message has already been received.

2.3 The State Publication Function Does Not Validate Odometry Data

The state timer callback directly reads the odometry variables:

void FastExplorationFSM::droneStateTimerCallback(...) {
  ...

  if (fd_->static_state_) {
    state.pos_ = fd_->odom_pos_;
    state.vel_ = fd_->odom_vel_;
    state.yaw_ = fd_->odom_yaw_;
  }

  msg.yaw = state.yaw_;
  drone_state_pub_.publish(msg);
}

Before publishing, the callback does not verify:

• Whether any odometry message has been received.
• Whether the position and velocity values are valid.
• Whether the yaw value is NaN or Inf.

As a result, uninitialized or invalid values may be broadcast directly.

3. Log Analysis

During a communication fuzzing experiment, the following exception occurred in the message callback:

bad callback: CommunicationFuzzer.message_callback
...
msg.yaw = clamp_angle(msg.yaw + self.gaussian())
...
math.sin(angle)
ValueError: math domain error

Further inspection showed that the input message contained:

msg.yaw = inf

The corresponding data propagation path was:

RACER publishes yaw = Inf
        ↓
The fuzzing callback receives the message
        ↓
Gaussian noise is added to Inf, and the result remains Inf
        ↓
math.sin(Inf) is called during angle normalization
        ↓
ValueError: math domain error is raised

The log shows that the downstream mathematical operation received a non-finite yaw value. Combined with the code behavior, this suggests that the state timer may have read and published the uninitialized odom_yaw_ before the first odometry message arrived.

4. Proposed Solution

The following changes are recommended.

4.1 Explicitly Initialize the Odometry Variables

Eigen::Vector3d odom_pos_{Eigen::Vector3d::Zero()};
Eigen::Vector3d odom_vel_{Eigen::Vector3d::Zero()};
Eigen::Quaterniond odom_orient_{Eigen::Quaterniond::Identity()};
double odom_yaw_{0.0};
bool have_odom_{false};

4.2 Set a Flag After Receiving Valid Odometry

void FastExplorationFSM::odometryCallback(
    const nav_msgs::OdometryConstPtr& msg) {
  // Update position, velocity, orientation, and yaw

  if (fd_->odom_pos_.allFinite() &&
      fd_->odom_vel_.allFinite() &&
      std::isfinite(fd_->odom_yaw_)) {
    fd_->have_odom_ = true;
  }
}

4.3 Validate Odometry Before Publishing the State

void FastExplorationFSM::droneStateTimerCallback(...) {
  if (!fd_->have_odom_) {
    ROS_WARN_THROTTLE(
        1.0, "Waiting for valid odometry.");
    return;
  }

  if (!fd_->odom_pos_.allFinite() ||
      !fd_->odom_vel_.allFinite() ||
      !std::isfinite(fd_->odom_yaw_)) {
    ROS_ERROR_THROTTLE(
        1.0, "Invalid odometry; skipping state publication.");
    return;
  }

  // Construct and publish the drone state
}

The key fix is to prevent state publication before the first valid odometry message is received and reject NaN or Inf values before publishing.