From 4b77983570d35d786e8840f3c925f377c0a50011 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 Jan 2024 21:23:12 +0900 Subject: [PATCH] feat(dynamic_avoidance): add a guard of LPF for reference path changed by avoidance/LC Signed-off-by: Takayuki Murooka --- .../scene.hpp | 5 +- .../src/scene.cpp | 48 ++++++++++--------- 2 files changed, 29 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index d2f9cd95066d9..a5380b628387d 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -382,8 +382,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, - const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, - const double obj_normal_vel, const std::optional & prev_object) const; + const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, + const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index ca58bbf0adcaa..9388a0a354d50 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -676,8 +676,8 @@ void DynamicAvoidanceModule::updateTargetObjects() ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_while_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel, - prev_object); + ref_path_points_for_obj_poly, obj_points, object.pose.position, object.vel, is_collision_left, + object.lat_vel, prev_object); if (!lat_offset_to_avoid) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -808,9 +808,9 @@ TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const double signed_time_to_start_collision = [&]() { - const double lon_offset_ego_front_to_obj_back = lon_offset_ego_to_obj_idx + - lat_lon_offset.min_lon_offset - - planner_data_->parameters.front_overhang; + const double lon_offset_ego_front_to_obj_back = + lon_offset_ego_to_obj_idx + lat_lon_offset.min_lon_offset - + (planner_data_->parameters.wheel_base + planner_data_->parameters.front_overhang); const double lon_offset_obj_front_to_ego_back = -lon_offset_ego_to_obj_idx - lat_lon_offset.max_lon_offset - planner_data_->parameters.rear_overhang; @@ -1070,7 +1070,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; } // The ego and object are the opposite directional. - const double obj_acc = -1.0; + const double obj_acc = -0.5; const double decel_time = 1.0; const double obj_moving_dist = (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / @@ -1120,9 +1120,10 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); - const double dist_threshold_paths = planner_data_->parameters.vehicle_width / 2.0 + - parameters_->lat_offset_from_obstacle + - calcObstacleMaxLength(obj_shape); + constexpr double dist_threshold_additional_margin = 0.5; + const double dist_threshold_paths = + planner_data_->parameters.vehicle_width / 2.0 + parameters_->lat_offset_from_obstacle + + calcObstacleWidth(obj_shape) / 2.0 + dist_threshold_additional_margin; // crop the ego's path by object position std::vector cropped_ego_path_points; @@ -1204,28 +1205,31 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, - const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, - const double obj_normal_vel, const std::optional & prev_object) const + const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, + const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const { - const bool enable_lowpass_filter = true; - /* const bool enable_lowpass_filter = [&]() { - if (!prev_ref_path_points_for_obj_poly_ || prev_ref_path_points_for_obj_poly_->size() < 2) { + if ( + !prev_object || prev_object->ref_path_points_for_obj_poly.size() < 2 || + ref_path_points_for_obj_poly.size() < 2) { return true; } - const size_t prev_front_seg_idx = motion_utils::findNearestSegmentIndex( - *prev_ref_path_points_for_obj_poly_, - ref_path_points_for_obj_poly.front().point.pose.position); constexpr double - min_lane_change_path_lat_offset = 1.0; if ( motion_utils::calcLateralOffset( - *prev_ref_path_points_for_obj_poly_, - ref_path_points_for_obj_poly.front().point.pose.position, prev_front_seg_idx) < - min_lane_change_path_lat_offset) { return true; + const size_t obj_point_idx = + motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); + const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset( + prev_object->ref_path_points_for_obj_poly, + ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); + + std::cerr << paths_lat_diff << std::endl; + constexpr double min_paths_lat_diff = 0.3; + if (paths_lat_diff < min_paths_lat_diff) { + return true; } // NOTE: When the input reference path laterally changes, the low-pass filter is disabled not to // shift the obstacle polygon suddenly. return false; }(); - */ // calculate min/max lateral offset from object to path const auto obj_lat_abs_offset = [&]() {