Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(autoware_autonomous_emergency_braking): update longitudinal offset parameter name #9463

Merged
merged 1 commit into from
Nov 28, 2024
Merged

refactor(autoware_autonomous_emergency_braking): update longitudinal offset parameter name #9463

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion control/autoware_autonomous_emergency_braking/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t
| min_generated_imu_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 |
| max_generated_imu_path_length | [m] | double | maximum distance for a predicted path generated by sensors | 10.0 |
| expand_width | [m] | double | expansion width of the ego vehicle for collision checking, path cropping and speed calculation | 0.1 |
| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 |
| longitudinal_offset_margin | [m] | double | longitudinal offset distance for collision check | 2.0 |
| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 |
| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 |
| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 |
Expand Down
3 changes: 2 additions & 1 deletion control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
use_predicted_trajectory: true
use_imu_path: true
limit_imu_path_lat_dev: false
limit_imu_path_length: true
use_pointcloud_data: true
use_predicted_object_data: false
use_object_velocity_calculation: true
Expand Down Expand Up @@ -39,7 +40,7 @@
maximum_cluster_size: 10000

# RSS distance collision check
longitudinal_offset: 1.0
longitudinal_offset_margin: 1.0
t_response: 1.0
a_ego_min: -3.0
a_obj_min: -1.0
Expand Down
3 changes: 2 additions & 1 deletion ...oware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -556,6 +556,7 @@ class AEB : public rclcpp::Node
bool use_predicted_trajectory_;
bool use_imu_path_;
bool limit_imu_path_lat_dev_;
bool limit_imu_path_length_;
bool use_pointcloud_data_;
bool use_predicted_object_data_;
bool use_object_velocity_calculation_;
Expand All @@ -571,7 +572,7 @@ class AEB : public rclcpp::Node
double min_generated_imu_path_length_;
double max_generated_imu_path_length_;
double expand_width_;
double longitudinal_offset_;
double longitudinal_offset_margin_;
double t_response_;
double a_ego_min_;
double a_obj_min_;
Expand Down
43 changes: 26 additions & 17 deletions control/autoware_autonomous_emergency_braking/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,16 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

namespace
{
using autoware::motion::control::autonomous_emergency_braking::colorTuple;
constexpr double MIN_MOVING_VELOCITY_THRESHOLD = 0.1;
// Sky blue (RGB: 0, 148, 205) - A medium-bright blue color
constexpr colorTuple IMU_PATH_COLOR = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999};
// Forest green (RGB: 0, 100, 0) - A deep, dark green color
constexpr colorTuple MPC_PATH_COLOR = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999};
} // namespace

namespace autoware::motion::control::autonomous_emergency_braking
{
using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon;
Expand Down Expand Up @@ -156,6 +166,7 @@
use_predicted_trajectory_ = declare_parameter<bool>("use_predicted_trajectory");
use_imu_path_ = declare_parameter<bool>("use_imu_path");
limit_imu_path_lat_dev_ = declare_parameter<bool>("limit_imu_path_lat_dev");
limit_imu_path_length_ = declare_parameter<bool>("limit_imu_path_length");
use_pointcloud_data_ = declare_parameter<bool>("use_pointcloud_data");
use_predicted_object_data_ = declare_parameter<bool>("use_predicted_object_data");
use_object_velocity_calculation_ = declare_parameter<bool>("use_object_velocity_calculation");
Expand All @@ -173,7 +184,7 @@
min_generated_imu_path_length_ = declare_parameter<double>("min_generated_imu_path_length");
max_generated_imu_path_length_ = declare_parameter<double>("max_generated_imu_path_length");
expand_width_ = declare_parameter<double>("expand_width");
longitudinal_offset_ = declare_parameter<double>("longitudinal_offset");
longitudinal_offset_margin_ = declare_parameter<double>("longitudinal_offset_margin");
t_response_ = declare_parameter<double>("t_response");
a_ego_min_ = declare_parameter<double>("a_ego_min");
a_obj_min_ = declare_parameter<double>("a_obj_min");
Expand Down Expand Up @@ -220,6 +231,7 @@
updateParam<bool>(parameters, "use_predicted_trajectory", use_predicted_trajectory_);
updateParam<bool>(parameters, "use_imu_path", use_imu_path_);
updateParam<bool>(parameters, "limit_imu_path_lat_dev", limit_imu_path_lat_dev_);
updateParam<bool>(parameters, "limit_imu_path_length", limit_imu_path_length_);
updateParam<bool>(parameters, "use_pointcloud_data", use_pointcloud_data_);
updateParam<bool>(parameters, "use_predicted_object_data", use_predicted_object_data_);
updateParam<bool>(
Expand All @@ -238,7 +250,7 @@
updateParam<double>(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_);
updateParam<double>(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_);
updateParam<double>(parameters, "expand_width", expand_width_);
updateParam<double>(parameters, "longitudinal_offset", longitudinal_offset_);
updateParam<double>(parameters, "longitudinal_offset_margin", longitudinal_offset_margin_);
updateParam<double>(parameters, "t_response", t_response_);
updateParam<double>(parameters, "a_ego_min", a_ego_min_);
updateParam<double>(parameters, "a_obj_min", a_obj_min_);
Expand Down Expand Up @@ -460,9 +472,8 @@
}

// step2. create velocity data check if the vehicle stops or not
constexpr double min_moving_velocity_th{0.1};
const double current_v = current_velocity_ptr_->longitudinal_velocity;
if (std::abs(current_v) < min_moving_velocity_th) {
if (std::abs(current_v) < MIN_MOVING_VELOCITY_THRESHOLD) {
return false;
}

Expand Down Expand Up @@ -570,18 +581,16 @@
getPointsBelongingToClusterHulls(
filtered_objects, points_belonging_to_cluster_hulls, debug_markers);

const auto imu_path_objects = (!use_imu_path_ || !angular_velocity_ptr_)
? std::vector<ObjectData>{}
: get_objects_on_path(
ego_imu_path, points_belonging_to_cluster_hulls,
{0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}, "imu");
const auto imu_path_objects =
(!use_imu_path_ || !angular_velocity_ptr_)
? std::vector<ObjectData>{}
: get_objects_on_path(ego_imu_path, points_belonging_to_cluster_hulls, IMU_PATH_COLOR, "imu");

const auto mpc_path_objects =
(!use_predicted_trajectory_ || !predicted_traj_ptr_ || !ego_mpc_path.has_value())
? std::vector<ObjectData>{}
: get_objects_on_path(
ego_mpc_path.value(), points_belonging_to_cluster_hulls,
{0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}, "mpc");
ego_mpc_path.value(), points_belonging_to_cluster_hulls, MPC_PATH_COLOR, "mpc");

// merge object data which comes from the ego (imu) path and predicted path
auto merge_objects =
Expand Down Expand Up @@ -632,7 +641,7 @@
const double obj_braking_distance = (obj_v > 0.0)
? -(obj_v * obj_v) / (2 * std::fabs(a_obj_min_))
: (obj_v * obj_v) / (2 * std::fabs(a_obj_min_));
return ego_stopping_distance + obj_braking_distance + longitudinal_offset_;
return ego_stopping_distance + obj_braking_distance + longitudinal_offset_margin_;
});

tier4_debug_msgs::msg::Float32Stamped rss_distance_msg;
Expand All @@ -655,10 +664,9 @@
const double & dt = imu_prediction_time_interval_;
const double distance_between_points = std::abs(curr_v) * dt;
constexpr double minimum_distance_between_points{1e-2};
// if current velocity is too small, assume it stops at the same point
// if distance between points is too small, arc length calculation is unreliable, so we skip
// creating the path
if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) {
if (distance_between_points < minimum_distance_between_points) {
return {};
}

Expand Down Expand Up @@ -700,7 +708,8 @@

const bool basic_path_conditions_satisfied =
(t > horizon) && (path_arc_length > min_generated_imu_path_length_);
const bool path_length_threshold_surpassed = path_arc_length > max_generated_imu_path_length_;
const bool path_length_threshold_surpassed =
limit_imu_path_length_ && path_arc_length > max_generated_imu_path_length_;
const bool lat_dev_threshold_surpassed =
limit_imu_path_lat_dev_ && std::abs(edge_of_ego_vehicle.y) > imu_path_lat_dev_threshold_;

Expand Down Expand Up @@ -1101,9 +1110,9 @@
});

if (ego_map_pose.has_value()) {
const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m;
const double base_link_to_front_offset = vehicle_info_.max_longitudinal_offset_m;

Check warning on line 1113 in control/autoware_autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autoware_autonomous_emergency_braking/src/node.cpp#L1113 

Added line #L1113 was not covered by tests
const auto ego_front_pose = autoware::universe_utils::calcOffsetPose(
ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0);
ego_map_pose.value(), base_link_to_front_offset, 0.0, 0.0, 0.0);
const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker(
ego_front_pose, "autonomous_emergency_braking", this->now(), 0);
autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &markers);
Expand Down
Loading