From e39ae798305c9d2f4786c8c11bfea007d5db2d7e Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Tue, 4 Mar 2025 16:46:57 +0900 Subject: [PATCH 1/6] po Signed-off-by: yuki-takagi-66 --- .../control_validator/control_validator.hpp | 4 +++ .../msg/ControlValidatorStatus.msg | 3 ++ .../src/control_validator.cpp | 32 +++++++++++++++++++ 3 files changed, 39 insertions(+) diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index 61cadf3f36fe8..d50e168c18f8b 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -53,6 +53,7 @@ struct ValidationParams double rolling_back_velocity; double over_velocity_ratio; double over_velocity_offset; + double overrun_stop_point_dist; double nominal_latency_threshold; }; @@ -95,6 +96,9 @@ class ControlValidator : public rclcpp::Node void calc_velocity_deviation_status( const Trajectory & reference_trajectory, const Odometry & kinematics); + void calc_stop_point_overrun_status( + const Trajectory & reference_trajectory, const Odometry & kinematics); + private: /** * @brief Setup diagnostic updater diff --git a/control/autoware_control_validator/msg/ControlValidatorStatus.msg b/control/autoware_control_validator/msg/ControlValidatorStatus.msg index c40f160510d89..934d855d0691b 100644 --- a/control/autoware_control_validator/msg/ControlValidatorStatus.msg +++ b/control/autoware_control_validator/msg/ControlValidatorStatus.msg @@ -4,12 +4,15 @@ builtin_interfaces/Time stamp bool is_valid_max_distance_deviation bool is_rolling_back bool is_over_velocity +bool has_overrun_stop_point bool is_valid_latency # values float64 max_distance_deviation float64 target_vel float64 vehicle_vel +float64 dist_to_stop +float64 nearest_trajectory_vel float64 latency int64 invalid_count diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 528bfdfcc1df2..1d520c0bce6ff 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -16,6 +16,7 @@ #include "autoware/control_validator/utils.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include @@ -268,6 +269,37 @@ void ControlValidator::calc_velocity_deviation_status( } } +void ControlValidator::calc_stop_point_overrun_status( + const Trajectory & reference_trajectory, const Odometry & kinematics) +{ + auto & status = validation_status_; + const auto & params = validation_params_; + + status.dist_to_stop = [](const Trajectory & traj, const geometry_msgs::msg::Pose & pose) { + const auto stop_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(traj.points); + + const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; + const size_t seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(traj.points, pose); + const double signed_length_on_traj = autoware::motion_utils::calcSignedArcLength( + traj.points, pose.position, seg_idx, traj.points.at(end_idx).pose.position, + std::min(end_idx, traj.points.size() - 2)); + + if (std::isnan(signed_length_on_traj)) { + return 0.0; + } + return signed_length_on_traj; + }(reference_trajectory, kinematics.pose.pose); + + status.nearest_trajectory_vel = + autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) + .longitudinal_velocity_mps; + + // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity + status.has_overrun_stop_point = + status.dist_to_stop < -params.overrun_stop_point_dist && status.nearest_trajectory_vel < 1e-3; +} + bool ControlValidator::is_all_valid(const ControlValidatorStatus & s) { return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity && From 064267031126474d878059fe3f79e395046562c8 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Tue, 4 Mar 2025 17:35:36 +0900 Subject: [PATCH 2/6] po Signed-off-by: yuki-takagi-66 --- .../config/control_validator.param.yaml | 1 + .../src/control_validator.cpp | 272 +++++++++--------- 2 files changed, 143 insertions(+), 130 deletions(-) diff --git a/control/autoware_control_validator/config/control_validator.param.yaml b/control/autoware_control_validator/config/control_validator.param.yaml index 95f9c9be22a12..538981fdb43bd 100644 --- a/control/autoware_control_validator/config/control_validator.param.yaml +++ b/control/autoware_control_validator/config/control_validator.param.yaml @@ -12,6 +12,7 @@ rolling_back_velocity: 0.5 over_velocity_offset: 2.0 over_velocity_ratio: 0.2 + overrun_stop_point_dist: 0.8 nominal_latency: 0.01 vel_lpf_gain: 0.9 # Time constant 0.33 diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 1d520c0bce6ff..d28367de69711 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -73,6 +73,7 @@ void ControlValidator::setup_parameters() p.rolling_back_velocity = declare_parameter(t + "rolling_back_velocity"); p.over_velocity_offset = declare_parameter(t + "over_velocity_offset"); p.over_velocity_ratio = declare_parameter(t + "over_velocity_ratio"); + p.overrun_stop_point_dist = declare_parameter(t + "overrun_stop_point_dist"); p.nominal_latency_threshold = declare_parameter(t + "nominal_latency"); } const auto lpf_gain = declare_parameter("vel_lpf_gain"); @@ -130,198 +131,209 @@ void ControlValidator::setup_diag() stat, !validation_status_.is_over_velocity, "The vehicle is over-speeding against the target."); }); - d.add(ns + "latency", [&](auto & stat) { + d.add(ns + "overrun_stop_point", [&](auto & stat) { set_status( - stat, validation_status_.is_valid_latency, "The latency is larger than expected value."); - }); + stat, !validation_status_.has_overrun_stop_point, + "The vehicle has overrun the front stop point on the trajectory."); + d.add(ns + "latency", [&](auto & stat) { + set_status( + stat, validation_status_.is_valid_latency, "The latency is larger than expected value."); + }); } bool ControlValidator::is_data_ready() { - const auto waiting = [this](const auto topic_name) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", topic_name); - return false; - }; - - if (!current_kinematics_) { - return waiting(sub_kinematics_->subscriber()->get_topic_name()); - } - if (!current_reference_trajectory_) { - return waiting(sub_reference_traj_->subscriber()->get_topic_name()); - } - if (!current_predicted_trajectory_) { - return waiting(sub_predicted_traj_->get_topic_name()); - } - return true; + const auto waiting = [this](const auto topic_name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for %s", topic_name); + return false; + }; + + if (!current_kinematics_) { + return waiting(sub_kinematics_->subscriber()->get_topic_name()); + } + if (!current_reference_trajectory_) { + return waiting(sub_reference_traj_->subscriber()->get_topic_name()); + } + if (!current_predicted_trajectory_) { + return waiting(sub_predicted_traj_->get_topic_name()); + } + return true; } void ControlValidator::on_control_cmd(const Control::ConstSharedPtr msg) { - validation_status_.latency = (this->now() - msg->stamp).seconds(); - validation_status_.is_valid_latency = - validation_status_.latency < validation_params_.nominal_latency_threshold; - validation_status_.invalid_count = - is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; + validation_status_.latency = (this->now() - msg->stamp).seconds(); + validation_status_.is_valid_latency = + validation_status_.latency < validation_params_.nominal_latency_threshold; + validation_status_.invalid_count = + is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; } void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg) { - stop_watch.tic(); + stop_watch.tic(); - current_predicted_trajectory_ = msg; - current_reference_trajectory_ = sub_reference_traj_->take_data(); - current_kinematics_ = sub_kinematics_->take_data(); + current_predicted_trajectory_ = msg; + current_reference_trajectory_ = sub_reference_traj_->take_data(); + current_kinematics_ = sub_kinematics_->take_data(); - if (!is_data_ready()) return; + if (!is_data_ready()) return; - debug_pose_publisher_->clear_markers(); + debug_pose_publisher_->clear_markers(); - validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_); + validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_); - diag_updater_.force_update(); + diag_updater_.force_update(); - // for debug - publish_debug_info(); - display_status(); + // for debug + publish_debug_info(); + display_status(); } void ControlValidator::publish_debug_info() { - pub_status_->publish(validation_status_); + pub_status_->publish(validation_status_); - if (!is_all_valid(validation_status_)) { - geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose; - shift_pose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m); - debug_pose_publisher_->push_virtual_wall(front_pose); - debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL"); - } - debug_pose_publisher_->publish(); + if (!is_all_valid(validation_status_)) { + geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose; + shift_pose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m); + debug_pose_publisher_->push_virtual_wall(front_pose); + debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL"); + } + debug_pose_publisher_->publish(); - // Publish ProcessingTime - autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; - processing_time_msg.stamp = get_clock()->now(); - processing_time_msg.data = stop_watch.toc(); - pub_processing_time_->publish(processing_time_msg); + // Publish ProcessingTime + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_->publish(processing_time_msg); } void ControlValidator::validate( const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory, const Odometry & kinematics) { - if (predicted_trajectory.points.size() < 2) { - RCLCPP_DEBUG(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); - return; - } - if (reference_trajectory.points.size() < 2) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 1000, - "reference_trajectory size is less than 2. Cannot validate."); - return; - } + if (predicted_trajectory.points.size() < 2) { + RCLCPP_DEBUG(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); + return; + } + if (reference_trajectory.points.size() < 2) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "reference_trajectory size is less than 2. Cannot validate."); + return; + } - validation_status_.stamp = get_clock()->now(); + validation_status_.stamp = get_clock()->now(); - std::tie( - validation_status_.max_distance_deviation, validation_status_.is_valid_max_distance_deviation) = - calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_); + std::tie( + validation_status_.max_distance_deviation, + validation_status_.is_valid_max_distance_deviation) = + calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_); - calc_velocity_deviation_status(*current_reference_trajectory_, kinematics); + calc_velocity_deviation_status(*current_reference_trajectory_, kinematics); + calc_stop_point_overrun_status(*current_reference_trajectory_, kinematics); - validation_status_.invalid_count = - is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; + validation_status_.invalid_count = + is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; } std::pair ControlValidator::calc_lateral_deviation_status( const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const { - auto max_distance_deviation = - calc_max_lateral_distance(reference_trajectory, predicted_trajectory); - return { - max_distance_deviation, - max_distance_deviation <= validation_params_.max_distance_deviation_threshold}; + auto max_distance_deviation = + calc_max_lateral_distance(reference_trajectory, predicted_trajectory); + return { + max_distance_deviation, + max_distance_deviation <= validation_params_.max_distance_deviation_threshold}; } void ControlValidator::calc_velocity_deviation_status( const Trajectory & reference_trajectory, const Odometry & kinematics) { - auto & status = validation_status_; - const auto & params = validation_params_; - status.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x); - status.target_vel = target_vel_.filter( - autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) - .longitudinal_velocity_mps); - - const bool is_rolling_back = std::signbit(status.vehicle_vel * status.target_vel) && - std::abs(status.vehicle_vel) > params.rolling_back_velocity; - if ( - !hold_velocity_error_until_stop_ || !status.is_rolling_back || - std::abs(status.vehicle_vel) < 0.05) { - status.is_rolling_back = is_rolling_back; - } + auto & status = validation_status_; + const auto & params = validation_params_; + status.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x); + status.target_vel = target_vel_.filter( + autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) + .longitudinal_velocity_mps); + + const bool is_rolling_back = std::signbit(status.vehicle_vel * status.target_vel) && + std::abs(status.vehicle_vel) > params.rolling_back_velocity; + if ( + !hold_velocity_error_until_stop_ || !status.is_rolling_back || + std::abs(status.vehicle_vel) < 0.05) { + status.is_rolling_back = is_rolling_back; + } - const bool is_over_velocity = - std::abs(status.vehicle_vel) > - std::abs(status.target_vel) * (1.0 + params.over_velocity_ratio) + params.over_velocity_offset; - if ( - !hold_velocity_error_until_stop_ || !status.is_over_velocity || - std::abs(status.vehicle_vel) < 0.05) { - status.is_over_velocity = is_over_velocity; - } + const bool is_over_velocity = std::abs(status.vehicle_vel) > + std::abs(status.target_vel) * (1.0 + params.over_velocity_ratio) + + params.over_velocity_offset; + if ( + !hold_velocity_error_until_stop_ || !status.is_over_velocity || + std::abs(status.vehicle_vel) < 0.05) { + status.is_over_velocity = is_over_velocity; + } } void ControlValidator::calc_stop_point_overrun_status( const Trajectory & reference_trajectory, const Odometry & kinematics) { - auto & status = validation_status_; - const auto & params = validation_params_; - - status.dist_to_stop = [](const Trajectory & traj, const geometry_msgs::msg::Pose & pose) { - const auto stop_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(traj.points); - - const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; - const size_t seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(traj.points, pose); - const double signed_length_on_traj = autoware::motion_utils::calcSignedArcLength( - traj.points, pose.position, seg_idx, traj.points.at(end_idx).pose.position, - std::min(end_idx, traj.points.size() - 2)); - - if (std::isnan(signed_length_on_traj)) { - return 0.0; - } - return signed_length_on_traj; - }(reference_trajectory, kinematics.pose.pose); - - status.nearest_trajectory_vel = - autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) - .longitudinal_velocity_mps; - - // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity - status.has_overrun_stop_point = - status.dist_to_stop < -params.overrun_stop_point_dist && status.nearest_trajectory_vel < 1e-3; + auto & status = validation_status_; + const auto & params = validation_params_; + + status.dist_to_stop = [](const Trajectory & traj, const geometry_msgs::msg::Pose & pose) { + const auto stop_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(traj.points); + + const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; + const size_t seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(traj.points, pose); + const double signed_length_on_traj = autoware::motion_utils::calcSignedArcLength( + traj.points, pose.position, seg_idx, traj.points.at(end_idx).pose.position, + std::min(end_idx, traj.points.size() - 2)); + + if (std::isnan(signed_length_on_traj)) { + return 0.0; + } + return signed_length_on_traj; + }(reference_trajectory, kinematics.pose.pose); + + status.nearest_trajectory_vel = + autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) + .longitudinal_velocity_mps; + + // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity + status.has_overrun_stop_point = + status.dist_to_stop < -params.overrun_stop_point_dist && status.nearest_trajectory_vel < 1e-3; } bool ControlValidator::is_all_valid(const ControlValidatorStatus & s) { - return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity && - s.is_valid_latency; + return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity && +<<<<<<< HEAD + s.is_valid_latency; +======= + !s.has_overrun_stop_point; +>>>>>>> a20ef4b38b (po) } void ControlValidator::display_status() { - if (!display_on_terminal_) return; - rclcpp::Clock clock{RCL_ROS_TIME}; + if (!display_on_terminal_) return; + rclcpp::Clock clock{RCL_ROS_TIME}; - const auto warn = [this, &clock](const bool status, const std::string & msg) { - if (!status) { - RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "%s", msg.c_str()); - } - }; + const auto warn = [this, &clock](const bool status, const std::string & msg) { + if (!status) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "%s", msg.c_str()); + } + }; - const auto & s = validation_status_; + const auto & s = validation_status_; - warn( - s.is_valid_max_distance_deviation, - "predicted trajectory is too far from planning trajectory!!"); + warn( + s.is_valid_max_distance_deviation, + "predicted trajectory is too far from planning trajectory!!"); } } // namespace autoware::control_validator From d537e94d2ec60e40c325dc49c9395055adf76cd8 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Wed, 5 Mar 2025 11:50:00 +0900 Subject: [PATCH 3/6] po Signed-off-by: yuki-takagi-66 --- .../src/control_validator.cpp | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index d28367de69711..a0ec061400797 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -225,7 +225,8 @@ void ControlValidator::validate( return; } - validation_status_.stamp = get_clock()->now(); + validation_status_.stamp = get_clock()->now(); + validation_status_.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x); std::tie( validation_status_.max_distance_deviation, @@ -252,12 +253,11 @@ std::pair ControlValidator::calc_lateral_deviation_status( void ControlValidator::calc_velocity_deviation_status( const Trajectory & reference_trajectory, const Odometry & kinematics) { - auto & status = validation_status_; - const auto & params = validation_params_; - status.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x); - status.target_vel = target_vel_.filter( - autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) - .longitudinal_velocity_mps); + auto & status = validation_status_; + const auto & params = validation_params_; + status.target_vel = target_vel_.filter( + autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) + .longitudinal_velocity_mps); const bool is_rolling_back = std::signbit(status.vehicle_vel * status.target_vel) && std::abs(status.vehicle_vel) > params.rolling_back_velocity; @@ -303,19 +303,15 @@ void ControlValidator::calc_stop_point_overrun_status( autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) .longitudinal_velocity_mps; - // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity - status.has_overrun_stop_point = - status.dist_to_stop < -params.overrun_stop_point_dist && status.nearest_trajectory_vel < 1e-3; + // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity + status.has_overrun_stop_point = status.dist_to_stop < -params.overrun_stop_point_dist && + status.nearest_trajectory_vel < 1e-3 && status.vehicle_vel > 0.1; } bool ControlValidator::is_all_valid(const ControlValidatorStatus & s) { return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity && -<<<<<<< HEAD - s.is_valid_latency; -======= !s.has_overrun_stop_point; ->>>>>>> a20ef4b38b (po) } void ControlValidator::display_status() From 07a049a5120636a332ef3322335f2ef2d42b83bc Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Thu, 6 Mar 2025 18:27:58 +0900 Subject: [PATCH 4/6] po Signed-off-by: yuki-takagi-66 --- control/autoware_control_validator/src/control_validator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index a0ec061400797..50fed827c9591 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -305,7 +305,7 @@ void ControlValidator::calc_stop_point_overrun_status( // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity status.has_overrun_stop_point = status.dist_to_stop < -params.overrun_stop_point_dist && - status.nearest_trajectory_vel < 1e-3 && status.vehicle_vel > 0.1; + status.nearest_trajectory_vel < 1e-3 && status.vehicle_vel > 1e-3; } bool ControlValidator::is_all_valid(const ControlValidatorStatus & s) From def48d2106903a0d4c68a6d4ac2844b453961d07 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Fri, 7 Mar 2025 15:22:09 +0900 Subject: [PATCH 5/6] po Signed-off-by: yuki-takagi-66 --- control/autoware_control_validator/README.md | 13 +++++++------ .../control_validator/control_validator.hpp | 5 +++++ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/control/autoware_control_validator/README.md b/control/autoware_control_validator/README.md index cc3d2069c940f..471003259d5a3 100644 --- a/control/autoware_control_validator/README.md +++ b/control/autoware_control_validator/README.md @@ -57,9 +57,10 @@ The following parameters can be set for the `control_validator`: The input trajectory is detected as invalid if the index exceeds the following thresholds. -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | -| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | -| `thresholds.rolling_back_velocity` | double | threshold velocity to valid the vehicle velocity [m/s] | 0.5 | -| `thresholds.over_velocity_offset` | double | threshold velocity offset to valid the vehicle velocity [m/s] | 2.0 | -| `thresholds.over_velocity_ratio` | double | threshold ratio to valid the vehicle velocity [*] | 0.2 | +| Name | Type | Description | Default value | +| :----------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | +| `thresholds.rolling_back_velocity` | double | threshold velocity to valid the vehicle velocity [m/s] | 0.5 | +| `thresholds.over_velocity_offset` | double | threshold velocity offset to valid the vehicle velocity [m/s] | 2.0 | +| `thresholds.over_velocity_ratio` | double | threshold ratio to valid the vehicle velocity [*] | 0.2 | +| `thresholds.overrun_stop_point_dist` | double | threshold distance to overrun stop point [m] | 0.8 | diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index d50e168c18f8b..2cbfeade8bc01 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -96,6 +96,11 @@ class ControlValidator : public rclcpp::Node void calc_velocity_deviation_status( const Trajectory & reference_trajectory, const Odometry & kinematics); + /** + * @brief Calculate whether the vehicle has overrun a stop point in the trajectory. + * @param reference_trajectory Reference trajectory + * @param kinematics Current vehicle odometry including pose and twist + */ void calc_stop_point_overrun_status( const Trajectory & reference_trajectory, const Odometry & kinematics); From 5197aa8725aa7434ced866997c0454c6eb8cb3e6 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Tue, 11 Mar 2025 14:06:49 +0900 Subject: [PATCH 6/6] po Signed-off-by: yuki-takagi-66 --- .../src/control_validator.cpp | 238 +++++++++--------- 1 file changed, 119 insertions(+), 119 deletions(-) diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 50fed827c9591..f176f13a18bc6 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -21,6 +21,7 @@ #include +#include #include #include #include @@ -131,123 +132,122 @@ void ControlValidator::setup_diag() stat, !validation_status_.is_over_velocity, "The vehicle is over-speeding against the target."); }); - d.add(ns + "overrun_stop_point", [&](auto & stat) { + d.add(ns + "overrun_stop_point", [&](auto & stat) { set_status( stat, !validation_status_.has_overrun_stop_point, "The vehicle has overrun the front stop point on the trajectory."); - d.add(ns + "latency", [&](auto & stat) { - set_status( - stat, validation_status_.is_valid_latency, "The latency is larger than expected value."); - }); + }); + d.add(ns + "latency", [&](auto & stat) { + set_status( + stat, validation_status_.is_valid_latency, "The latency is larger than expected value."); + }); } bool ControlValidator::is_data_ready() { - const auto waiting = [this](const auto topic_name) { - RCLCPP_INFO_SKIPFIRST_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting for %s", topic_name); - return false; - }; - - if (!current_kinematics_) { - return waiting(sub_kinematics_->subscriber()->get_topic_name()); - } - if (!current_reference_trajectory_) { - return waiting(sub_reference_traj_->subscriber()->get_topic_name()); - } - if (!current_predicted_trajectory_) { - return waiting(sub_predicted_traj_->get_topic_name()); - } - return true; + const auto waiting = [this](const auto topic_name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", topic_name); + return false; + }; + + if (!current_kinematics_) { + return waiting(sub_kinematics_->subscriber()->get_topic_name()); + } + if (!current_reference_trajectory_) { + return waiting(sub_reference_traj_->subscriber()->get_topic_name()); + } + if (!current_predicted_trajectory_) { + return waiting(sub_predicted_traj_->get_topic_name()); + } + return true; } void ControlValidator::on_control_cmd(const Control::ConstSharedPtr msg) { - validation_status_.latency = (this->now() - msg->stamp).seconds(); - validation_status_.is_valid_latency = - validation_status_.latency < validation_params_.nominal_latency_threshold; - validation_status_.invalid_count = - is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; + validation_status_.latency = (this->now() - msg->stamp).seconds(); + validation_status_.is_valid_latency = + validation_status_.latency < validation_params_.nominal_latency_threshold; + validation_status_.invalid_count = + is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; } void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg) { - stop_watch.tic(); + stop_watch.tic(); - current_predicted_trajectory_ = msg; - current_reference_trajectory_ = sub_reference_traj_->take_data(); - current_kinematics_ = sub_kinematics_->take_data(); + current_predicted_trajectory_ = msg; + current_reference_trajectory_ = sub_reference_traj_->take_data(); + current_kinematics_ = sub_kinematics_->take_data(); - if (!is_data_ready()) return; + if (!is_data_ready()) return; - debug_pose_publisher_->clear_markers(); + debug_pose_publisher_->clear_markers(); - validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_); + validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_); - diag_updater_.force_update(); + diag_updater_.force_update(); - // for debug - publish_debug_info(); - display_status(); + // for debug + publish_debug_info(); + display_status(); } void ControlValidator::publish_debug_info() { - pub_status_->publish(validation_status_); + pub_status_->publish(validation_status_); - if (!is_all_valid(validation_status_)) { - geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose; - shift_pose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m); - debug_pose_publisher_->push_virtual_wall(front_pose); - debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL"); - } - debug_pose_publisher_->publish(); + if (!is_all_valid(validation_status_)) { + geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose; + shift_pose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m); + debug_pose_publisher_->push_virtual_wall(front_pose); + debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL"); + } + debug_pose_publisher_->publish(); - // Publish ProcessingTime - autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; - processing_time_msg.stamp = get_clock()->now(); - processing_time_msg.data = stop_watch.toc(); - pub_processing_time_->publish(processing_time_msg); + // Publish ProcessingTime + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_->publish(processing_time_msg); } void ControlValidator::validate( const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory, const Odometry & kinematics) { - if (predicted_trajectory.points.size() < 2) { - RCLCPP_DEBUG(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); - return; - } - if (reference_trajectory.points.size() < 2) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 1000, - "reference_trajectory size is less than 2. Cannot validate."); - return; - } + if (predicted_trajectory.points.size() < 2) { + RCLCPP_DEBUG(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); + return; + } + if (reference_trajectory.points.size() < 2) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "reference_trajectory size is less than 2. Cannot validate."); + return; + } validation_status_.stamp = get_clock()->now(); validation_status_.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x); - std::tie( - validation_status_.max_distance_deviation, - validation_status_.is_valid_max_distance_deviation) = - calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_); + std::tie( + validation_status_.max_distance_deviation, validation_status_.is_valid_max_distance_deviation) = + calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_); - calc_velocity_deviation_status(*current_reference_trajectory_, kinematics); - calc_stop_point_overrun_status(*current_reference_trajectory_, kinematics); + calc_velocity_deviation_status(*current_reference_trajectory_, kinematics); + calc_stop_point_overrun_status(*current_reference_trajectory_, kinematics); - validation_status_.invalid_count = - is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; + validation_status_.invalid_count = + is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; } std::pair ControlValidator::calc_lateral_deviation_status( const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const { - auto max_distance_deviation = - calc_max_lateral_distance(reference_trajectory, predicted_trajectory); - return { - max_distance_deviation, - max_distance_deviation <= validation_params_.max_distance_deviation_threshold}; + auto max_distance_deviation = + calc_max_lateral_distance(reference_trajectory, predicted_trajectory); + return { + max_distance_deviation, + max_distance_deviation <= validation_params_.max_distance_deviation_threshold}; } void ControlValidator::calc_velocity_deviation_status( @@ -259,49 +259,49 @@ void ControlValidator::calc_velocity_deviation_status( autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) .longitudinal_velocity_mps); - const bool is_rolling_back = std::signbit(status.vehicle_vel * status.target_vel) && - std::abs(status.vehicle_vel) > params.rolling_back_velocity; - if ( - !hold_velocity_error_until_stop_ || !status.is_rolling_back || - std::abs(status.vehicle_vel) < 0.05) { - status.is_rolling_back = is_rolling_back; - } + const bool is_rolling_back = std::signbit(status.vehicle_vel * status.target_vel) && + std::abs(status.vehicle_vel) > params.rolling_back_velocity; + if ( + !hold_velocity_error_until_stop_ || !status.is_rolling_back || + std::abs(status.vehicle_vel) < 0.05) { + status.is_rolling_back = is_rolling_back; + } - const bool is_over_velocity = std::abs(status.vehicle_vel) > - std::abs(status.target_vel) * (1.0 + params.over_velocity_ratio) + - params.over_velocity_offset; - if ( - !hold_velocity_error_until_stop_ || !status.is_over_velocity || - std::abs(status.vehicle_vel) < 0.05) { - status.is_over_velocity = is_over_velocity; - } + const bool is_over_velocity = + std::abs(status.vehicle_vel) > + std::abs(status.target_vel) * (1.0 + params.over_velocity_ratio) + params.over_velocity_offset; + if ( + !hold_velocity_error_until_stop_ || !status.is_over_velocity || + std::abs(status.vehicle_vel) < 0.05) { + status.is_over_velocity = is_over_velocity; + } } void ControlValidator::calc_stop_point_overrun_status( const Trajectory & reference_trajectory, const Odometry & kinematics) { - auto & status = validation_status_; - const auto & params = validation_params_; + auto & status = validation_status_; + const auto & params = validation_params_; - status.dist_to_stop = [](const Trajectory & traj, const geometry_msgs::msg::Pose & pose) { - const auto stop_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(traj.points); + status.dist_to_stop = [](const Trajectory & traj, const geometry_msgs::msg::Pose & pose) { + const auto stop_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(traj.points); - const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; - const size_t seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(traj.points, pose); - const double signed_length_on_traj = autoware::motion_utils::calcSignedArcLength( - traj.points, pose.position, seg_idx, traj.points.at(end_idx).pose.position, - std::min(end_idx, traj.points.size() - 2)); + const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; + const size_t seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(traj.points, pose); + const double signed_length_on_traj = autoware::motion_utils::calcSignedArcLength( + traj.points, pose.position, seg_idx, traj.points.at(end_idx).pose.position, + std::min(end_idx, traj.points.size() - 2)); - if (std::isnan(signed_length_on_traj)) { - return 0.0; - } - return signed_length_on_traj; - }(reference_trajectory, kinematics.pose.pose); + if (std::isnan(signed_length_on_traj)) { + return 0.0; + } + return signed_length_on_traj; + }(reference_trajectory, kinematics.pose.pose); - status.nearest_trajectory_vel = - autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) - .longitudinal_velocity_mps; + status.nearest_trajectory_vel = + autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) + .longitudinal_velocity_mps; // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity status.has_overrun_stop_point = status.dist_to_stop < -params.overrun_stop_point_dist && @@ -310,26 +310,26 @@ void ControlValidator::calc_stop_point_overrun_status( bool ControlValidator::is_all_valid(const ControlValidatorStatus & s) { - return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity && - !s.has_overrun_stop_point; + return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity && + !s.has_overrun_stop_point; } void ControlValidator::display_status() { - if (!display_on_terminal_) return; - rclcpp::Clock clock{RCL_ROS_TIME}; + if (!display_on_terminal_) return; + rclcpp::Clock clock{RCL_ROS_TIME}; - const auto warn = [this, &clock](const bool status, const std::string & msg) { - if (!status) { - RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "%s", msg.c_str()); - } - }; + const auto warn = [this, &clock](const bool status, const std::string & msg) { + if (!status) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "%s", msg.c_str()); + } + }; - const auto & s = validation_status_; + const auto & s = validation_status_; - warn( - s.is_valid_max_distance_deviation, - "predicted trajectory is too far from planning trajectory!!"); + warn( + s.is_valid_max_distance_deviation, + "predicted trajectory is too far from planning trajectory!!"); } } // namespace autoware::control_validator