(mpc_lateral_controller) is_converged gets false after manual driving

[TomoyukiaW]

Checklist

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

Description

I'm working with Autoware v1.0. After we drive the vehicle manually, sometimes is_converged in mpc_lateral_controller.cpp gets false, trajectory_follower is staying in stopping state. I feel it's often happened after we drive backward with moving steering (meandering backward). I dumped cmd.steering_tire_angle and m_current_steering.steering_tire_angle with the following debug print.

mpc_lateral_controller.cpp: L308
bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) const
{
  // wait for a while to propagate the trajectory shape to the output command when the trajectory
  // shape is changed.
  if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) {
    RCLCPP_DEBUG(logger_, "trajectory shaped is changed");
    return false;
  }

  const bool is_converged =
    std::abs(cmd.steering_tire_angle - m_current_steering.steering_tire_angle) <
    static_cast<float>(m_converged_steer_rad);

  {
    RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "cmd_tire_angle=%lf cur_tire_andle=%lf diff=%lf",
      cmd.steering_tire_angle, m_current_steering.steering_tire_angle,
      std::abs(cmd.steering_tire_angle - m_current_steering.steering_tire_angle));
  }

  return is_converged;
}

In this log, diff is 0.239239 grater than 0.1.

Expected behavior

After the manual driving and when we start auto operation, is_converged should be true.

Actual behavior

Sometimes is_converged gets false.

Steps to reproduce

1. After the auto operation in some path, set autoware control disabled in AutowareStatePanel.
2. Drive backward with moving steering.
3. Set autoware control enabled, operation mode auto in AutowareStatePanel.
   ->autoware control gets enabled, operation mode gets Auto. But the vehicle doesn't move. The console output the log, "target speed > 0, but keep stop condition is met. Keep STOPPED."

Versions

• Ubuntu 22.04
• ROS2
• Autoware v1.0

Possible causes

Should we reset m_ctrl_cmd_prev as well as MPC state like the following? In my debugging, sometimes only is_mpc_solved couldn't be detected the above situation, so I added OperationModeState::LOCAL and OperationModeState::REMOTE states conditions to reset in this condition. (Semantically, using autoware control = disabled is better, but it is not passed to mpc_lateral_controller.cpp.)

mpc_lateral_controller.cpp: L254
  // reset previous MPC result
  // Note: When a large deviation from the trajectory occurs, the optimization stops and
  // the vehicle will return to the path by re-planning the trajectory or external operation.
  // After the recovery, the previous value of the optimization may deviate greatly from
  // the actual steer angle, and it may make the optimization result unstable.
  if (!is_mpc_solved || 
      input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL ||
      input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE) {
    m_mpc.resetPrevResult(m_current_steering);
    m_ctrl_cmd_prev = getInitialControlCommand();
  } else {
    setSteeringToHistory(ctrl_cmd);
  }

Additional context

No response

[mehmetdogru]

@TomoyukiaW Could you please confirm the issue persists on autoware's latest status.

[kyoichi-sugahara]

@TomoyukiaW
Thank you so much for the detailed report and proposal!
And I think I'm facing the exactly same problem with you.

I'm not so sure wether your solution solves the problem,
but if you want to utilize information of control status, how about defining is_under_control with following way?
https://github.com/autowarefoundation/autoware.universe/blob/b4cd3b0b74a1c5f53d168bb6601030a5e0d30839/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp/#L651-L652

Also, if you can make a PR to solve this issue we really appreciate and review the PR soon 😍

[VRichardJP]

sometimes is_converged in mpc_lateral_controller.cpp gets false, trajectory_follower is staying in stopping state

Sounds similar to #4915, no?

#5183 fixes it but is not included in 1.0 release

[TomoyukiaW]

@idorobotics I'm sorry for my late response. I was working with another task from last weekend. I checked this issue in the customer vehicle in some project. And currently we are in verification phase. So, there is no time to prepare the latest source with the customer configuration. But I think my mentioned source code part is not changed in the latest source. I'll check other person's opinions and consider the best way to solve/check this issue.

[TomoyukiaW]

@kyoichi-sugahara Thank you for the status suggestion. It's my expected status. I'll create a PR after checking how to create it...

[TomoyukiaW]

@VRichardJP If my understanding is correct, #4915 is the issue not in autonomous mode. So, it seems that after applying #5183, still occur this issue.

[kyoichi-sugahara]

@TomoyukiaW
I created draft PR and I confirmed my problem can be solved with this change.
(I will add description later sorry 🙇
I really appreciate your report and proposal. That was really helpful!!!!
Do you think reseting m_ctrl_cmd_prev is enough?
How to judge whether manual driving or not is being discussed internally.

[TomoyukiaW]

@kyoichi-sugahara Thank you for supporting this issue! In my environment, resetting m_ctrl_cmd_prev was enough to clear the issue. I think aligning m_ctrl_cmd_prev to current vehicle tire angle during manual driving (and not during stopping in autonomous driving mode) is reasonable.

I'm sorry for not having supported PR. It was happened in the customer environment, and I did the instant WA. But I didn't have the chance to capture the rosbag to verify the modification for PR. I'll ask someone's help next time.