diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 7579e3da265c4..7f71374da4d0c 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -75,7 +75,8 @@ Return LateralOutput which contains the following to the controller node - `autoware_auto_control_msgs/AckermannLateralCommand` - LateralSyncData - - steer angle convergence + - is mpc ready to move? + - if not, the vehicle should stop until the stable mpc output is obtained ### MPC class @@ -104,8 +105,6 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 | | stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 | | stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.0 | -| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | -| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true | | new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | | new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | | mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.3 | diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 441101abfc243..190ef7a646aba 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -73,9 +73,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // Target vehicle speed threshold to enter the stop state. double m_stop_state_entry_target_speed; - // Convergence threshold for steering control. - double m_converged_steer_rad; - // max mpc output change threshold for 1 sec double m_mpc_converged_threshold_rps; @@ -85,9 +82,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // Distance threshold to check if the trajectory shape has changed. double m_new_traj_end_dist; - // Flag indicating whether to keep the steering control until it converges. - bool m_keep_steer_control_until_converged; - // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; @@ -245,13 +239,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase */ bool isTrajectoryShapeChanged() const; - /** - * @brief Check if the steering control is converged and stable now. - * @param cmd Steering control command to be checked. - * @return True if the steering control is converged and stable, false otherwise. - */ - bool isSteerConverged(const AckermannLateralCommand & cmd) const; - rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; /** diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 59d3f759c9c2f..414b3f9a9f056 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -61,8 +61,6 @@ # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 - converged_steer_rad: 0.1 - keep_steer_control_until_converged: true new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 3d19104112a71..6f9bcba522fda 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -60,8 +60,6 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) /* stop state parameters */ m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed"); m_stop_state_entry_target_speed = dp_double("stop_state_entry_target_speed"); - m_converged_steer_rad = dp_double("converged_steer_rad"); - m_keep_steer_control_until_converged = dp_bool("keep_steer_control_until_converged"); m_new_traj_duration_time = dp_double("new_traj_duration_time"); // [s] m_new_traj_end_dist = dp_double("new_traj_end_dist"); // [m] m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps"); // [rad/s] @@ -271,13 +269,11 @@ trajectory_follower::LateralOutput MpcLateralController::run( const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) { trajectory_follower::LateralOutput output; output.control_cmd = createCtrlCmdMsg(cmd); - // To be sure current steering of the vehicle is desired steering angle, we need to check + // To be sure lateral controller is ready to move, we need to check // following conditions. // 1. At the last loop, mpc should be solved because command should be optimized output. - // 2. The mpc should be converged. - // 3. The steer angle should be converged. - output.sync_data.is_steer_converged = - is_mpc_solved && isMpcConverged() && isSteerConverged(cmd); + // 2. The steering output of the mpc should be converged. + output.sync_data.is_controller_ready_to_move = is_mpc_solved && isMpcConverged(); return output; }; @@ -301,22 +297,6 @@ trajectory_follower::LateralOutput MpcLateralController::run( return createLateralOutput(ctrl_cmd, is_mpc_solved); } -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(m_converged_steer_rad); - - return is_converged; -} - bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data) { setTrajectory(input_data.current_trajectory, input_data.current_odometry); @@ -407,11 +387,6 @@ bool MpcLateralController::isStoppedState() const const double current_vel = m_current_kinematic_state.twist.twist.linear.x; const double target_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; - const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command - if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { - return false; // not stopState: keep control - } - if ( std::fabs(current_vel) < m_stop_state_entry_ego_speed && std::fabs(target_vel) < m_stop_state_entry_target_speed) { diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 921cd3995f6a9..8f23caabc3d8f 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -121,7 +121,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro bool m_enable_overshoot_emergency; bool m_enable_slope_compensation; bool m_enable_large_tracking_error_emergency; - bool m_enable_keep_stopped_until_steer_convergence; // smooth stop transition struct StateTransitionParams diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml index eb2ef443c4576..ad87526fd3057 100644 --- a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -6,7 +6,6 @@ enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: false - enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 644effce50824..157e3f5ab6295 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -49,8 +49,6 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) m_enable_large_tracking_error_emergency = node.declare_parameter("enable_large_tracking_error_emergency"); m_enable_slope_compensation = node.declare_parameter("enable_slope_compensation"); - m_enable_keep_stopped_until_steer_convergence = - node.declare_parameter("enable_keep_stopped_until_steer_convergence"); // parameters for state transition { @@ -495,8 +493,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; - const bool keep_stopped_condition = - m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; + const bool keep_stopped_condition = !lateral_sync_data_.is_controller_ready_to_move; const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; if ( diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md index 48e2a13ef664d..e1d5e7fd817a5 100644 --- a/control/pure_pursuit/README.md +++ b/control/pure_pursuit/README.md @@ -15,5 +15,6 @@ Return LateralOutput which contains the following to the controller node - `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle - LateralSyncData - - steer angle convergence + - can controller create predicted path for ego vehicle? + - if not, the vehicle should stop until the controller can create the predicted path - `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle diff --git a/control/pure_pursuit/config/pure_pursuit.param.yaml b/control/pure_pursuit/config/pure_pursuit.param.yaml index 0b8b464e9f969..3b075cce326ec 100644 --- a/control/pure_pursuit/config/pure_pursuit.param.yaml +++ b/control/pure_pursuit/config/pure_pursuit.param.yaml @@ -6,7 +6,6 @@ long_ld_lateral_error_threshold: 0.5 min_lookahead_distance: 4.35 max_lookahead_distance: 15.0 - converged_steer_rad: 0.1 reverse_min_lookahead_distance: 7.0 prediction_ds: 0.3 prediction_distance_length: 21.0 diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index daaebaacde8de..42bdadc33de8a 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -82,7 +82,6 @@ struct Param double min_lookahead_distance; double max_lookahead_distance; double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear - double converged_steer_rad_; double prediction_ds; double prediction_distance_length; // Total distance of prediction trajectory double resampling_ds; @@ -162,8 +161,6 @@ class PurePursuitLateralController : public LateralControllerBase AckermannLateralCommand generateOutputControlCmd(); - bool calcIsSteerConverged(const AckermannLateralCommand & cmd); - double calcLookaheadDistance( const double lateral_error, const double curvature, const double velocity, const double min_ld, const bool is_control_cmd); diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index f0c49b07e675a..5135d1c87f9cf 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -79,7 +79,6 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) param_.max_lookahead_distance = node.declare_parameter("max_lookahead_distance"); param_.reverse_min_lookahead_distance = node.declare_parameter("reverse_min_lookahead_distance"); - param_.converged_steer_rad_ = node.declare_parameter("converged_steer_rad"); param_.prediction_ds = node.declare_parameter("prediction_ds"); param_.prediction_distance_length = node.declare_parameter("prediction_distance_length"); param_.resampling_ds = node.declare_parameter("resampling_ds"); @@ -348,11 +347,6 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data) if (param_.enable_path_smoothing) { averageFilterTrajectory(*trajectory_resampled_); } - const auto cmd_msg = generateOutputControlCmd(); - - LateralOutput output; - output.control_cmd = cmd_msg; - output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg); // calculate predicted trajectory with iterative calculation const auto predicted_trajectory = generatePredictedTrajectory(); @@ -362,13 +356,14 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data) pub_predicted_trajectory_->publish(*predicted_trajectory); } - return output; -} + const auto cmd_msg = generateOutputControlCmd(); -bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd) -{ - return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < - static_cast(param_.converged_steer_rad_); + LateralOutput output; + output.control_cmd = cmd_msg; + output.sync_data.is_controller_ready_to_move = + predicted_trajectory.has_value(); // If not, not ready. + + return output; } AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() diff --git a/control/trajectory_follower_base/README.md b/control/trajectory_follower_base/README.md index 6bd3d74e75271..692cb104f6e3f 100644 --- a/control/trajectory_follower_base/README.md +++ b/control/trajectory_follower_base/README.md @@ -19,8 +19,8 @@ The interface class has the following base functions. - `isReady()`: Check if the control is ready to compute. - `run()`: Compute control commands and return to [Trajectory Follower Nodes](../trajectory_follower_node/README.md). This must be implemented by inherited algorithms. - `sync()`: Input the result of running the other controller. - - steer angle convergence - - allow keeping stopped until steer is converged. + - is lateral controller ready to move? + - allow keeping stopped until lateral controller output is stable. - velocity convergence(currently not used) See [the Design of Trajectory Follower Nodes](../trajectory_follower_node/README.md#Design) for how these functions work in the node. diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp index 60c91019d10c3..28b6b6e8002ec 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp @@ -19,7 +19,7 @@ namespace autoware::motion::control::trajectory_follower { struct LateralSyncData { - bool is_steer_converged{false}; + bool is_controller_ready_to_move{false}; }; struct LongitudinalSyncData diff --git a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp index f7779158f5791..32a47609c41ed 100644 --- a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp +++ b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp @@ -22,6 +22,6 @@ void LongitudinalControllerBase::sync(LateralSyncData const & lateral_sync_data) } void LongitudinalControllerBase::reset() { - lateral_sync_data_.is_steer_converged = false; + lateral_sync_data_.is_controller_ready_to_move = false; } } // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md index c3ea3ddf6b7e8..de32c455a8990 100644 --- a/control/trajectory_follower_node/README.md +++ b/control/trajectory_follower_node/README.md @@ -39,7 +39,7 @@ steering accel } struct LongitudinalSyncData { -is_steer_converged +is_controller_ready_to_move } struct LateralSyncData { } @@ -118,13 +118,6 @@ lateral_controller_->sync(lon_out.sync_data); control_cmd_pub_->publish(out); ``` -Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` - -- lateral controller - - `keep_steer_control_until_converged` -- longitudinal controller - - `enable_keep_stopped_until_steer_convergence` - ### Inputs / Outputs / API #### Inputs diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index c108bec2671b3..74a4bc9c8cf3e 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -78,7 +78,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_operation_mode_; rclcpp::Publisher::SharedPtr control_cmd_pub_; - rclcpp::Publisher::SharedPtr debug_marker_pub_; autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; @@ -109,9 +108,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; LongitudinalControllerMode getLongitudinalControllerMode( const std::string & algorithm_name) const; - void publishDebugMarker( - const trajectory_follower::InputData & input_data, - const trajectory_follower::LateralOutput & lat_out) const; std::unique_ptr logger_configure_; }; diff --git a/control/trajectory_follower_node/param/lateral/mpc.param.yaml b/control/trajectory_follower_node/param/lateral/mpc.param.yaml index 4222082d40deb..217968a2c18c7 100644 --- a/control/trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/trajectory_follower_node/param/lateral/mpc.param.yaml @@ -61,8 +61,6 @@ # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 - converged_steer_rad: 0.1 - keep_steer_control_until_converged: true new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] diff --git a/control/trajectory_follower_node/param/lateral/pure_pursuit.param.yaml b/control/trajectory_follower_node/param/lateral/pure_pursuit.param.yaml index 0b8b464e9f969..3b075cce326ec 100644 --- a/control/trajectory_follower_node/param/lateral/pure_pursuit.param.yaml +++ b/control/trajectory_follower_node/param/lateral/pure_pursuit.param.yaml @@ -6,7 +6,6 @@ long_ld_lateral_error_threshold: 0.5 min_lookahead_distance: 4.35 max_lookahead_distance: 15.0 - converged_steer_rad: 0.1 reverse_min_lookahead_distance: 7.0 prediction_ds: 0.3 prediction_distance_length: 21.0 diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index e35848e9495af..46aa63c824964 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -75,8 +75,6 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); - debug_marker_pub_ = - create_publisher("~/output/debug_marker", rclcpp::QoS{1}); // Timer { @@ -221,39 +219,7 @@ void Controller::callbackTimerControl() out.lateral = lat_out.control_cmd; out.longitudinal = lon_out.control_cmd; control_cmd_pub_->publish(out); - - // 6. publish debug marker - publishDebugMarker(*input_data, lat_out); } - -void Controller::publishDebugMarker( - const trajectory_follower::InputData & input_data, - const trajectory_follower::LateralOutput & lat_out) const -{ - visualization_msgs::msg::MarkerArray debug_marker_array{}; - - // steer converged marker - { - auto marker = tier4_autoware_utils::createDefaultMarker( - "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); - marker.pose = input_data.current_odometry.pose.pose; - - std::stringstream ss; - const double current = input_data.current_steering.steering_tire_angle; - const double cmd = lat_out.control_cmd.steering_tire_angle; - const double diff = current - cmd; - ss << "current:" << current << " cmd:" << cmd << " diff:" << diff - << (lat_out.sync_data.is_steer_converged ? " (converged)" : " (not converged)"); - marker.text = ss.str(); - - debug_marker_array.markers.push_back(marker); - } - - debug_marker_pub_->publish(debug_marker_array); -} - } // namespace autoware::motion::control::trajectory_follower_node #include "rclcpp_components/register_node_macro.hpp"