From 09170454a121ac848c2c2855dc4482a9a60d7e4b Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 26 Nov 2024 00:48:44 +0900 Subject: [PATCH] refactor(autoware_autonomous_emergency_braking): update longitudinal offset parameter name 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. Refactor the code to use the new parameter name consistently throughout the module. This includes updating the README.md file, the configuration file, the node.hpp file, and the node.cpp file. This change ensures that the parameter name accurately represents its functionality and improves code readability. 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 f0f4a6c9f7014..62ddce7bdaaee 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -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 | 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 7c4cf58cbc6e7..dd4fbc2da8da7 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_; @@ -1101,9 +1110,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);