17.9.1

Description

Abstract

Fixes issue in EgoEntitySimulation::overwrite() where steering angle was always set to 0.0 instead of being calculated from angular velocity. This caused angular velocity and acceleration to be forced to 0.0 in subsequent steps.

Discovery: Found during FollowTrajectoryAction overshoot investigation, see 1921.

Issue

Old implementation unconditionally set steering to 0.0 for DELAY_STEER_VEL and DELAY_STEER_ACC_GEARED vehicle models:

// OLD: Always set steering to 0.0
case VehicleModelType::DELAY_STEER_VEL:
  state(4) = 0;  // WRONG: ignores angular velocity
  [[fallthrough]];

Impact: Vehicle model computes angular velocity/acceleration as 0.0 in subsequent steps → entity state diverges from FollowTrajectoryAction expectations.

Solution

Calculate steering angle from angular velocity using bicycle kinematic model: steering = atan((ω * L) / v)

case VehicleModelType::DELAY_STEER_VEL:
  if (std::abs(status.action_status.twist.linear.x) < min_linear_velocity_for_steer_calculation) {
    state(4) = 0.0;  // At standstill, steering undefined
  } else {
    state(4) = std::atan(
      (status.action_status.twist.angular.z * wheel_base_) /
      status.action_status.twist.linear.x);
  }
  [[fallthrough]];

Where:

• ω = twist.angular.z (yaw rate)
• L = wheel_base_ (front axle - rear axle distance)
• v = twist.linear.x (linear velocity)

Additional changes:

• Added wheel_base_ member variable initialized from parameters
• Moved getCurrentPose() default parameter from .cpp to .hpp

Testing

Compared tool (from 1921) vs full ss2 stack with identical input:

• Before fix: Angular velocity/acceleration diverge after overwrite() call
• After fix: Angular velocity/acceleration remain consistent

Related Issues