feat(pid_longitudinal_controller): don't switch to DRIVE if the state conditions are not met (#11369)

[kyoichi-sugahara]

In the analysis of the product using version 0.48, I observed the state oscillation reported in the following PR
autowarefoundation#11369
so cherry-pick it.

Thank you @MertClk

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[sonarqubecloud]

Quality Gate passed

Issues
1 New issue
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud

[Owen-Liuyuxuan]

@co-pilot-auto Summarize and review this PR. Visualize the updated state machine of the controller.

[tier4-autoware-private-bot]

@Owen-Liuyuxuan 🔄 Co-Pilot-Auto Client Triggered... Please wait for the results.

View Client trigger workflow progress: GitHub Actions Run

[tier4-autoware-private-bot]

@Owen-Liuyuxuan Here's the analysis:

Here's a summary and review of the PR, along with the updated state machine diagram.

Summary of the PR

This Pull Request (PR) addresses an issue in the pid_longitudinal_controller where the control state would oscillate between DRIVE and STOPPED when Autoware control was disabled. This oscillation could lead to unnecessary state changes and potentially confusing behavior.

The core change introduces a condition to keep the controller in the STOPPED state if Autoware control is not enabled (!is_under_control) and the stopped_condition is met. This prevents the controller from attempting to transition to DRIVE when it's not supposed to be actively controlling the vehicle.

Key Changes:

1. pid_longitudinal_controller.cpp:

   • A new conditional check if (!is_under_control && stopped_condition) return; is added at the beginning of the STOPPED state logic within updateControlState. This ensures that if Autoware control is disabled and the vehicle is in a stopped state, it remains in STOPPED.

2. test_controller_node.cpp:

   • The test case test_stopped_state_when_autoware_control_disabled is updated to explicitly set msg.is_autoware_control_enabled = true; when publishing OperationModeState. This change seems to be part of ensuring the test environment correctly reflects the conditions under which the controller should operate, although the PR description focuses on the !is_autoware_control_enabled case for the fix. It's important to ensure the test covers both scenarios (enabled and disabled control).

Review of the PR

The PR effectively addresses the reported oscillation issue. By explicitly checking is_under_control within the STOPPED state, it prevents unwanted transitions to DRIVE when Autoware is not in active control. This improves the stability and predictability of the longitudinal controller's state machine.

Positive Aspects:

• Directly addresses the problem: The fix targets the root cause of the oscillation.
• Clear intent: The added condition !is_under_control && stopped_condition clearly states the intent to remain in STOPPED when control is disabled.
• Improved stability: Prevents unnecessary state changes, leading to a more stable control system when Autoware is not actively engaged.

Potential Considerations/Suggestions:

• Test Coverage for !is_autoware_control_enabled: While the test_controller_node.cpp change sets is_autoware_control_enabled = true, it would be beneficial to explicitly add a test case that verifies the new behavior: that the controller remains in STOPPED when is_autoware_control_enabled is false. The current test modification seems to ensure the controller can transition when enabled, but not directly test the fix for the disabled case.
• Clarity of stopped_condition: The stopped_condition is a composite of several factors (e.g., is_stopped_pose, is_stopped_speed, is_emergency_stop_prevention_stopped). It's good that this is reused, but understanding its full implications for the new STOPPED state logic is crucial.
• Interaction with departure_condition_from_stopped: The existing departure_condition_from_stopped is still relevant for transitioning out of STOPPED when is_under_control is true. The new logic acts as an additional guard to prevent departure when control is disabled. This interaction seems correct.

Updated State Machine Diagram

Here's the Mermaid stateDiagram-v2 illustrating the updated control state transitions for the PidLongitudinalController, highlighting the change in the STOPPED state.

stateDiagram-v2
        direction LR
    [*] --> STOPPED

    state "STOPPED<br/>Entry: Brake hold etc." as STOPPED
    state "DRIVE<br/>Entry: PID control, delay comp." as DRIVE
    state "STOPPING<br/>Entry: Smooth stopping sequence" as STOPPING
    state "EMERGENCY<br/>Entry: Emergency stop" as EMERGENCY

    STOPPED --> DRIVE : is_under_control && departure_condition_from_stopped
    STOPPED --> STOPPING : is_under_control && has_nonzero_target_vel && !departure_condition_from_stopped
    STOPPED --> EMERGENCY : emergency_condition
    STOPPED --> STOPPED : !is_under_control && stopped_condition
    
    DRIVE --> STOPPING : stopping_condition
    DRIVE --> EMERGENCY : emergency_condition
    
    STOPPING --> STOPPED : stopped_condition
    STOPPING --> EMERGENCY : emergency_condition
    
    EMERGENCY --> STOPPED : emergency_recovery_condition

Loading

Explanation of Changes in Diagram:

• The most significant change is within the STOPPED state. A new self-loop transition STOPPED --> STOPPED : !is_under_control && stopped_condition has been added. This explicitly shows that if Autoware control is disabled (!is_under_control) and the vehicle meets the stopped_condition, it will remain in the STOPPED state, preventing the oscillation.
• The existing transitions from STOPPED to DRIVE and STOPPING now implicitly require is_under_control to be true, as the new self-loop takes precedence when is_under_control is false. I've made this explicit in the diagram for clarity.

This diagram clearly visualizes how the PR's change prevents the controller from leaving the STOPPED state when Autoware control is not enabled, thus resolving the oscillation.