diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 50e9d44a0a7a4..e299cafe8e64b 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 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/README.md b/control/pid_longitudinal_controller/README.md index 9d25322c793e0..ea2b5931f5151 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -160,23 +160,22 @@ Then, the velocity can be calculated by providing the current error and time ste The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the AutonomouStuff Lexus RX 450h for under 40 km/h driving. -| Name | Type | Description | Default value | -| :------------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | -| enable_smooth_stop | bool | flag to enable transition to STOPPING | true | -| enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | -| enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | -| enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | -| enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | -| enable_keep_stopped_until_steer_convergence | bool | flag to keep stopped condition until until the steer converges. | true | -| max_acc | double | max value of output acceleration [m/s^2] | 3.0 | -| min_acc | double | min value of output acceleration [m/s^2] | -5.0 | -| max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | -| min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | -| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | -| lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | -| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | -| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | +| Name | Type | Description | Default value | +| :------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | +| enable_smooth_stop | bool | flag to enable transition to STOPPING | true | +| enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | +| enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | +| enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | +| enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | +| max_acc | double | max value of output acceleration [m/s^2] | 3.0 | +| min_acc | double | min value of output acceleration [m/s^2] | -5.0 | +| max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | +| min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | +| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | +| lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | +| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | +| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | ### State transition 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 d13e628f2e1d4..bac8244a830e0 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 { @@ -500,11 +498,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity static constexpr double vel_epsilon = 1e-3; - // Let vehicle start after the steering is converged for control accuracy - const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon && - m_enable_keep_stopped_until_steer_convergence && - !lateral_sync_data_.is_steer_converged; - const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && @@ -563,7 +556,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return changeState(ControlState::EMERGENCY); } - if (!is_under_control && stopped_condition && keep_stopped_condition) { + if (!is_under_control && stopped_condition) { // NOTE: When the ego is stopped on manual driving, since the driving state may transit to // autonomous, keep_stopped_condition should be checked. return changeState(ControlState::STOPPED); @@ -614,14 +607,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (has_nonzero_target_vel && !departure_condition_from_stopped) { info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED."); } - if (has_nonzero_target_vel && keep_stopped_condition) { - info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED."); - } // --------------- - if (keep_stopped_condition) { - return changeState(ControlState::STOPPED); - } if (departure_condition_from_stopped) { m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md index 48e2a13ef664d..9c9e0e22c2ae1 100644 --- a/control/pure_pursuit/README.md +++ b/control/pure_pursuit/README.md @@ -15,5 +15,5 @@ Return LateralOutput which contains the following to the controller node - `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle - LateralSyncData - - steer angle convergence + - controller can create predicted path successfully or not - `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 cc43da7114630..fe43baa4bea24 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 @@ -84,7 +84,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node control_cmd_pub_; rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; - rclcpp::Publisher::SharedPtr debug_marker_pub_; autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; @@ -115,9 +114,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 322aa9eef5a79..d9deca90c5450 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -79,8 +79,6 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control create_publisher("~/lateral/debug/processing_time_ms", 1); pub_processing_time_lon_ms_ = create_publisher("~/longitudinal/debug/processing_time_ms", 1); - debug_marker_pub_ = - create_publisher("~/output/debug_marker", rclcpp::QoS{1}); // Timer { @@ -230,39 +228,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); -} - void Controller::publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub) {