fix(vehicle_cmd_gate): fix the problems while transition from/to stopped state

[brkay54]

Description

This PR solves the following issues:

• If the user sends stop command to autoware while driving, autoware does not do steering control while stopping: Vehicle_cmd_gate uses current steering while stopping #5796
• Vehicle can not move because steering is not controlable when stopped #4915
• Vehicle get stuck because vehicle command gate does not want to send steering command while "paused" #6044

Related links

Tests performed

Tested on Psim, working as we want.

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[brkay54]

Hi @TakaHoribe, about the problem mentioned this issue, I moved the steering converge checking from controllers to vehicle_cmd_gate to solve this issue. If this method is okay, please let me know, I will create another PR to remove steering converge checking from controllers.

[codecov]

Codecov Report

Attention: 10 lines in your changes are missing coverage. Please review.

Comparison is base (fa8424d) 14.37% compared to head (19b03ae) 14.37%.
Report is 3 commits behind head on main.

Files	Patch %	Lines
control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp	0.00%	10 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5183      +/-   ##
==========================================
- Coverage   14.37%   14.37%   -0.01%     
==========================================
  Files        1907     1907              
  Lines      130072   130077       +5     
  Branches    37618    37617       -1     
==========================================
  Hits        18695    18695              
- Misses      90367    90372       +5     
  Partials    21010    21010              

Flag	Coverage Δ		*Carryforward flag
differential	41.02% <0.00%> (?)
total	14.37% <ø> (+<0.01%)	⬆️	Carriedforward from fa8424d

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[brkay54]

Hi @TakaHoribe @tkimura4, sorry to disturb you, but just I want to kindly ping you.

[tkimura4]

@kosuke55

Could you review this PR?

Hi @TakaHoribe, about the problem mentioned this #4915, I moved the steering converge checking from controllers to vehicle_cmd_gate to solve this issue. If this method is okay, please let me know, I will create another PR to remove steering converge checking from controllers.

What do you think about this opinion?

[TakaHoribe]

@brkay54 Thank you for your PR.

It is reasonable not to use the stop steering command when the vehicle is trying to stop. However, I can't see the purpose of the move of the steering convergence check. What is the reason why you think the check should be in the gate?

[brkay54]

@TakaHoribe, thank you for the reply.

However, I can't see the purpose of the move of the steering convergence check. What is the reason why you think the check should be in the gate?

The main cause is #4915
Checking steering convergence in the controller creates unnecessary complexity. For this case:

• pid_controller does not allow the vehicle to move because the steering is not converged.
• vehicle_cmd_gate does not allow to control steering because the vehicle stopped.
  These two nodes wait for themselves forever.

Because vehicle_cmd_gate checks conditions for start and stopping, I think it makes sense to move steering checking into vehicle_cmd_gate.

[TakaHoribe]

@brkay54 Thank you, I understood. It sounds reasonable. Would you reproduce the non-start issue in psim? Or does it happen only in the real vehicle testing?

[brkay54]

@TakaHoribe Yes, I can reproduce in psim.

2023-11-06.15-54-52.mp4

• Vehicle can start at the beginning in auto mode.
• I set steering about 20 deg with manual controller.
• After I set 20 deg, when I clicked auto, vehicle can not move because of steering converging issue.
• I set steering about 0 deg with manual controller.
• Vehicle can move in auto mode.

[brkay54]

@TakaHoribe -san, I rebased on the current master and solved the conflicts. And tested again, worked as we wanted.

cap-.2023-11-11-16-34-22.mp4

Merge order:
1 - #5183 and autowarefoundation/autoware_launch#607
2 - #5189 and autowarefoundation/autoware_launch#616
3 - #5203

Thank you for your effort, if you have any concerns please let me know.

[brkay54]

@TakaHoribe -san, sorry to bother you, it is a friendly ping.

[rej55]

LGTM

@TakaHoribe
Could you approve this PR as a code owner?

Sorry, I have a comment for this PR. Please check it.

[rej55]

@brkay54

 } else if (enable_keep_steering_until_convergence_) {
    if (fabs(current_kinematics_.twist.twist.linear.x) < stopped_vel_th) {
      if (!isSteeringConverged(filtered_commands.control.lateral)) {
        filtered_commands.control.longitudinal = createStopTransitionControlCmd();
      }
    }
 }

Do you have any reasons to implement this block into vehicle_cmd_gate?
I think it should be implemented at the end of trajectory_follower_node because vehicle_cmd_gate should just switch the control commands.

[brkay54]

Hi thank you for the review. Actually, I explained the reason above here.

[brkay54]

Sorry to bother you, but could you check the comment below? @TakaHoribe @rej55

[ahuazuipiaoliang]

@brkay54 @TakaHoribe @rej55 I checked the code, there are 3 relevant modules, which are trajectory_follower, vehicle_cmd_gate, default_ad_api;

1. If the required steering angle is too large, the steering angle cannot be converged, the the target velocity of longitudinal velocity should be set as 0;
2. When the target velocity is 0, the vehicle_cmd_gate will output a state of is_start_requested=false;
3. If is_start_requested==false, the default_ad_api will change the state to PAUSED and keep calling the service /control/vehicle_cmd_gate/set_pause to set PAUSED state of vehicle_cmd_gate;
4. When the state of vehicle_cmd_gate is PAUSED, the vehicle_cmd_gate will keep sending stop control cmd which the steering angle is the current_steering_angle of the vehicle.

So the vehicle enters a dead loop and keeps PAUSED state.
I suggest create paused_control_cmd for PAUSED state in vehicle_cmd_gate that the steering_cmd is the command of trajectory_follower. When the steering angle is converged, the longitudinal controller can output normal control command.

[brkay54]

Firstly, we need to avoid sending createStopControlCmd(), while the vehicle is stopping (not stopped yet in disengage state). Because it causes sending current_steering while the vehicle is decelerating. You can see the problem this issue.

Thank you for the suggestion @ahuazuipiaoliang! It makes sense to me, however, it will cause an undesired behavior. If we change both DISENGAGE and PAUSED states to send control_cmd's steering command, the steering will be controllable all the time. So, the vehicle won't be fully stopped, always it listens to the control_cmd's steering command.

I am not sure if it is a behavior we wanted or not. If it is okay, we can change it like @ahuazuipiaoliang said. cc @rej55 @TakaHoribe

[rej55]

@brkay54 cc @TakaHoribe
Sorry for late reply, I agree for the suggestion from @ahuazuipiaoliang.
IMO, the trajectory_follower should send the control_cmd not to move the steering if we want to do so.
(@TakaHoribe said the same thing when I talked with him)

[ahuazuipiaoliang]

@rej55 Thanks, Fujiyama. Actually, I have modified the code in my way and tested on real vehicle for several days. I have not found any issue yet.

[brkay54]

@rej55 Thanks for the reply, it makes sense to me. I am updating the PR.

[brkay54]

@rej55 @ahuazuipiaoliang Sorry to disturb you guys, thank you for your replies!

To be able to not listen to the trajectory_follower's output when the vehicle fully stopped, I found another way. I changed the structure like this:

  // Check engage
  if (!is_engaged_) {
    filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
  }

  // Check pause. Place this check after all other checks as it needs the final output.
  adapi_pause_->update(filtered_commands.control);
  if (adapi_pause_->is_paused()) {
    if (is_engaged_) {
      filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
    } else {
      filtered_commands.control = createStopControlCmd();
    }
  }

In this way, the vehicle will be able to control steering if the vehicle is in PAUSED state. Also, it won't be able to control the steering when it is fully stopped and disengaged. I made some tests to check:

cap-.2024-02-02-14-03-35.mp4

In the video above, I tested whether the vehicle can control the steering when we set a large steering value by using the manual controller or not. When I changed state to AUTO, the vehicle did not move until the steering converged as you can see.

In the second part, I set a large steering value again and I changed its state to STOP, as you can see, the vehicle did not change its steering. (It changed a little bit because the manual controller sets ENGAGE state to true while running and the steering was changing in the transition between ENGAGE to DISENGAGE)

Also, I checked the behavior of the vehicle while stopping. (It can control the steering or not while moving and disengaged) You can see the result in the video below:

cap-.2024-02-02-14-17-47.mp4

As you can see, It still can control steering after I click the STOP button.

Also, I tried this structure too:

  // Check engage
  if (!is_engaged_) {
    filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
  }

  // Check pause. Place this check after all other checks as it needs the final output.
  adapi_pause_->update(filtered_commands.control);
  if (adapi_pause_->is_paused()) {
    filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
  }

It also solves the problems above, but, when the vehicle fully stopped, trajectory_follower can control the steering as you can see below:

cap-.2024-02-02-14-31-00.mp4

Therefore I used the first structure. Please share your comments with me! Thank you for your efforts.

[rej55]

LGTM.
Sorry for late review.

[brkay54]

@tkimura4 @TakaHoribe We need code-owner approve. Could you check the PR?

[TakaHoribe]

LGTM, thank you.
Note: the keep-stop command can send the target steering in order to control steering before moving.