Vehicle get stuck because vehicle command gate does not want to send steering command while "paused"

[VRichardJP]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I'm convinced that this is not my fault but a bug.

Description

I have this problem with freespace planning where Autoware often refuse to restart after it has been stopped halfway in a curve:

I observe the trajectory follower node complains the steering is not converged so it requires the vehicle to remain stopped. Yet, it outputs the correct steering target in the control command message:

Current steering when stuck (/vehicle/status/steering_status):

stamp:
  sec: 1704883251
  nanosec: 691513130
steering_tire_angle: -0.07187381386756897

Trajectory follower node command (/control/trajectory_follower/control_cmd):

stamp:
  sec: 1704882913
  nanosec: 889614390
lateral:
  stamp:
    sec: 1704882913
    nanosec: 889451380
  steering_tire_angle: 0.393218457698822
  steering_tire_rotation_rate: 0.5679231286048889
longitudinal:
  stamp:
    sec: 1704882913
    nanosec: 889483518
  speed: 0.0
  acceleration: -3.409665822982788
  jerk: 0.0

Right after the trajectory follower node, there is the vehicle command gate. And I see the control command keeps the current steering instead of forwarding the trajectory controller node command:

/control/command/control_cmd:

stamp:
  sec: 1704882999
  nanosec: 749559566
lateral:
  stamp:
    sec: 1704882999
    nanosec: 749559566
  steering_tire_angle: -0.07187381386756897
  steering_tire_rotation_rate: 0.0
longitudinal:
  stamp:
    sec: 1704882999
    nanosec: 749559566
  speed: 0.0
  acceleration: -1.5
  jerk: 0.0

By adding a couple printf, I found that the vehicle command is changed to a stop because the AdapiPauseInterface "is paused". Here:

https://github.com/autowarefoundation/autoware.universe/blob/142bc49b57f6ca002c99efa6406b22ce9012448d/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp#L436-L440

I am not sure what it means, or how to leave this state. Is it paused because the vehicle is stopped? But then, the vehicle is stopped because the steering is not converged, and the steering is not moved because the vehicle is stopped?

Any idea ?

Expected behavior

Autoware doesn't get stuck

Actual behavior

Gets stuck on steering not converged

Steps to reproduce

Difficult. The problem occurs only with my custom freespace planner algorithm

Versions

No response

Possible causes

No response

Additional context

No response

[brkay54]

Thank you for the report, it might be same issue as in https://github.com/autowarefoundation/autoware.universe/issues/4915 I created PRs for this issue but it still waits for review https://github.com/autowarefoundation/autoware.universe/pull/5183

[VRichardJP]

@brkay54 Yes, this is the same issue. I have tested https://github.com/autowarefoundation/autoware.universe/pull/5183 and it works perfectly.

[brkay54]

@VRichardJP Thank you for the confirmation. I also linked this issue to the PR.