Skip to content

Commit

Permalink
refactor(autoware_autonomous_emergency_braking): update longitudinal …
Browse files Browse the repository at this point in the history 
…offset parameter name (#9463)

Update the parameter name for the longitudinal offset distance used for collision check in the autonomous emergency braking control module. The parameter name has been changed from "longitudinal_offset" to "longitudinal_offset_margin" to better reflect its purpose.

Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara authored Nov 28, 2024
1 parent 710dacc commit 659b6b3
Show file tree
Hide file tree
Showing 4 changed files with 31 additions and 20 deletions.
2 changes: 1 addition & 1 deletion control/autoware_autonomous_emergency_braking/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -308,7 +308,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
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
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
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 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
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 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
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 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter(
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 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter(
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 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
}

// 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 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
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 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object
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 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w)
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 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w)

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 @@ -1073,9 +1082,9 @@ void AEB::addVirtualStopWallMarker(MarkerArray & markers)
});

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;
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

0 comments on commit 659b6b3

Please sign in to comment.