From 659b6b3836406bc33f257798c1ae41374a72c501 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Thu, 28 Nov 2024 15:33:11 +0900 Subject: [PATCH] refactor(autoware_autonomous_emergency_braking): update longitudinal 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 --- .../README.md | 2 +- .../autonomous_emergency_braking.param.yaml | 3 +- .../autonomous_emergency_braking/node.hpp | 3 +- .../src/node.cpp | 43 +++++++++++-------- 4 files changed, 31 insertions(+), 20 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index d3ee7d4e1aa29..7133e4ea6b7f7 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -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 | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 99ca4d4ef52cb..b23c3bbdd4d59 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -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 @@ -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 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 96efb5dc9ff1f..3de8a52c70643 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -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_; @@ -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_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 7d019023b1656..21a690cd0eec3 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -57,6 +57,16 @@ #include #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; @@ -156,6 +166,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); limit_imu_path_lat_dev_ = declare_parameter("limit_imu_path_lat_dev"); + limit_imu_path_length_ = declare_parameter("limit_imu_path_length"); use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); @@ -173,7 +184,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) min_generated_imu_path_length_ = declare_parameter("min_generated_imu_path_length"); max_generated_imu_path_length_ = declare_parameter("max_generated_imu_path_length"); expand_width_ = declare_parameter("expand_width"); - longitudinal_offset_ = declare_parameter("longitudinal_offset"); + longitudinal_offset_margin_ = declare_parameter("longitudinal_offset_margin"); t_response_ = declare_parameter("t_response"); a_ego_min_ = declare_parameter("a_ego_min"); a_obj_min_ = declare_parameter("a_obj_min"); @@ -220,6 +231,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); updateParam(parameters, "limit_imu_path_lat_dev", limit_imu_path_lat_dev_); + updateParam(parameters, "limit_imu_path_length", limit_imu_path_length_); updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); updateParam( @@ -238,7 +250,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); updateParam(parameters, "expand_width", expand_width_); - updateParam(parameters, "longitudinal_offset", longitudinal_offset_); + updateParam(parameters, "longitudinal_offset_margin", longitudinal_offset_margin_); updateParam(parameters, "t_response", t_response_); updateParam(parameters, "a_ego_min", a_ego_min_); updateParam(parameters, "a_obj_min", a_obj_min_); @@ -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; } @@ -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{} - : 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{} + : 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{} : 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 = @@ -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; @@ -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 {}; } @@ -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_; @@ -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);