Skip to content

Commit

Permalink
feat(control_validator): add hold and lpf (autowarefoundation#9120)
Browse files Browse the repository at this point in the history 
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
  • Loading branch information
@yuki-takagi-66
yuki-takagi-66 authored Oct 23, 2024
1 parent be12c10 commit c7d9b9b
Show file tree
Hide file tree
Showing 6 changed files with 71 additions and 49 deletions.
19 changes: 10 additions & 9 deletions control/autoware_control_validator/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@ The `control_validator` is a module that checks the validity of the output of th
The following features are supported for the validation and can have thresholds set by parameters.
The listed features below does not always correspond to the latest implementation.

| Description | Arguments | Diagnostic equation | Implemented function name |
| ---------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | :-----------------------------------------------: | ------------------------------- |
| Inverse velocity: Measured velocity has a different sign from the target velocity. | measured velocity $v$, target velocity $\hat{v}$, and threshold velocity parameter $k$ | $v \hat{v} < 0, \quad \lvert v \rvert > k$ | `checkValidVelocityDeviation()` |
| Overspeed: Measured speed exceeds target speed significantly. | measured velocity $v$, target velocity $\hat{v}$, and threshold ratio parameter $r$ | $\lvert v \rvert > (1 + r) \lvert \hat{v} \rvert$ | `checkValidVelocityDeviation()` |
| Description | Arguments | Diagnostic equation |
| ---------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------- | :---------------------------------------------------: |
| Inverse velocity: Measured velocity has a different sign from the target velocity. | measured velocity $v$, target velocity $\hat{v}$, and velocity parameter $c$ | $v \hat{v} < 0, \quad \lvert v \rvert > c$ |
| Overspeed: Measured speed exceeds target speed significantly. | measured velocity $v$, target velocity $\hat{v}$, ratio parameter $r$, and offset parameter $c$ | $\lvert v \rvert > (1 + r) \lvert \hat{v} \rvert + c$ |

- **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold.

Expand Down Expand Up @@ -57,8 +57,9 @@ 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.max_reverse_velocity` | double | threshold velocity to valid the vehicle velocity [m/s] | WIP |
| `thresholds.max_over_velocity_ratio` | double | threshold ratio to valid the vehicle velocity [*] | WIP |
| 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 |
8 changes: 6 additions & 2 deletions control/autoware_control_validator/config/control_validator.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,5 +9,9 @@

thresholds:
max_distance_deviation: 1.0
max_reverse_velocity: 0.2
max_over_velocity_ratio: 0.1
rolling_back_velocity: 0.5
over_velocity_offset: 2.0
over_velocity_ratio: 0.2

vel_lpf_gain: 0.9 # Time constant 0.33
hold_velocity_error_until_stop: true
22 changes: 10 additions & 12 deletions control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include "autoware_vehicle_info_utils/vehicle_info.hpp"
#include "diagnostic_updater/diagnostic_updater.hpp"

#include <autoware/signal_processing/lowpass_filter_1d.hpp>
#include <autoware/universe_utils/system/stop_watch.hpp>
#include <autoware_control_validator/msg/control_validator_status.hpp>
#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -47,8 +48,9 @@ using nav_msgs::msg::Odometry;
struct ValidationParams
{
double max_distance_deviation_threshold;
double max_reverse_velocity_threshold;
double max_over_velocity_ratio_threshold;
double rolling_back_velocity;
double over_velocity_ratio;
double over_velocity_offset;
};

/**
Expand Down Expand Up @@ -81,16 +83,8 @@ class ControlValidator : public rclcpp::Node
std::pair<double, bool> calc_lateral_deviation_status(
const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const;

/**
* @brief Calculate the velocity deviation between the reference trajectory and the current
* vehicle kinematics.
* @param reference_trajectory Reference trajectory
* @param kinematics Current vehicle kinematics
* @return A tuple containing the current velocity, desired velocity, and a boolean indicating
* validity
*/
std::tuple<double, double, bool> calc_velocity_deviation_status(
const Trajectory & reference_trajectory, const Odometry & kinematics) const;
void calc_velocity_deviation_status(
const Trajectory & reference_trajectory, const Odometry & kinematics);

private:
/**
Expand Down Expand Up @@ -155,6 +149,10 @@ class ControlValidator : public rclcpp::Node

ControlValidatorStatus validation_status_;
ValidationParams validation_params_; // for thresholds
bool is_velocity_valid_{true};
autoware::signal_processing::LowpassFilter1d vehicle_vel_{0.0};
autoware::signal_processing::LowpassFilter1d target_vel_{0.0};
bool hold_velocity_error_until_stop_{false};

vehicle_info_utils::VehicleInfo vehicle_info_;

Expand Down
7 changes: 4 additions & 3 deletions control/autoware_control_validator/msg/ControlValidatorStatus.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,12 @@ builtin_interfaces/Time stamp

# states
bool is_valid_max_distance_deviation
bool is_valid_velocity_deviation
bool is_rolling_back
bool is_over_velocity

# values
float64 max_distance_deviation
float64 desired_velocity
float64 current_velocity
float64 target_vel
float64 vehicle_vel

int64 invalid_count
1 change: 1 addition & 0 deletions control/autoware_control_validator/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

<depend>autoware_motion_utils</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_signal_processing</depend>
<depend>autoware_universe_utils</depend>
<depend>autoware_vehicle_info_utils</depend>
<depend>diagnostic_updater</depend>
Expand Down
63 changes: 40 additions & 23 deletions control/autoware_control_validator/src/control_validator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,15 @@ void ControlValidator::setup_parameters()
auto & p = validation_params_;
const std::string t = "thresholds.";
p.max_distance_deviation_threshold = declare_parameter<double>(t + "max_distance_deviation");
p.max_reverse_velocity_threshold = declare_parameter<double>(t + "max_reverse_velocity");
p.max_over_velocity_ratio_threshold = declare_parameter<double>(t + "max_over_velocity_ratio");
p.rolling_back_velocity = declare_parameter<double>(t + "rolling_back_velocity");
p.over_velocity_offset = declare_parameter<double>(t + "over_velocity_offset");
p.over_velocity_ratio = declare_parameter<double>(t + "over_velocity_ratio");
}
const auto lpf_gain = declare_parameter<double>("vel_lpf_gain");
vehicle_vel_.setGain(lpf_gain);
target_vel_.setGain(lpf_gain);

hold_velocity_error_until_stop_ = declare_parameter<bool>("hold_velocity_error_until_stop");

try {
vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
Expand Down Expand Up @@ -109,10 +115,15 @@ void ControlValidator::setup_diag()
stat, validation_status_.is_valid_max_distance_deviation,
"control output is deviated from trajectory");
});
d.add(ns + "velocity_deviation", [&](auto & stat) {
d.add(ns + "rolling_back", [&](auto & stat) {
set_status(
stat, !validation_status_.is_rolling_back,
"The vehicle is rolling back. The velocity has the opposite sign to the target.");
});
d.add(ns + "over_velocity", [&](auto & stat) {
set_status(
stat, validation_status_.is_valid_velocity_deviation,
"current velocity is deviated from the desired velocity");
stat, !validation_status_.is_over_velocity,
"The vehicle is over-speeding against the target.");
});
}

Expand Down Expand Up @@ -198,10 +209,7 @@ void ControlValidator::validate(
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_.current_velocity, validation_status_.desired_velocity,
validation_status_.is_valid_velocity_deviation) =
calc_velocity_deviation_status(*current_reference_trajectory_, kinematics);
calc_velocity_deviation_status(*current_reference_trajectory_, kinematics);

validation_status_.invalid_count =
is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1;
Expand All @@ -217,28 +225,37 @@ std::pair<double, bool> ControlValidator::calc_lateral_deviation_status(
max_distance_deviation <= validation_params_.max_distance_deviation_threshold};
}

std::tuple<double, double, bool> ControlValidator::calc_velocity_deviation_status(
const Trajectory & reference_trajectory, const Odometry & kinematics) const
void ControlValidator::calc_velocity_deviation_status(
const Trajectory & reference_trajectory, const Odometry & kinematics)
{
const double current_vel = kinematics.twist.twist.linear.x;
const double desired_vel =
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;
.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(current_vel) >
std::abs(desired_vel) * (1.0 + validation_params_.max_over_velocity_ratio_threshold) +
validation_params_.max_reverse_velocity_threshold;
const bool is_reverse_velocity =
std::signbit(current_vel * desired_vel) &&
std::abs(current_vel) > validation_params_.max_reverse_velocity_threshold;

return {current_vel, desired_vel, !(is_over_velocity || is_reverse_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;
}
}

bool ControlValidator::is_all_valid(const ControlValidatorStatus & s)
{
return s.is_valid_max_distance_deviation && s.is_valid_velocity_deviation;
return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity;
}

void ControlValidator::display_status()
Expand Down

0 comments on commit c7d9b9b

Please sign in to comment.