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

feat(dynamic_avoidance): add a guard of LPF for reference path changed by avoidance/LC #1102

Merged
merged 1 commit into from
Jan 18, 2024
Merged

feat(dynamic_avoidance): add a guard of LPF for reference path changed by avoidance/LC #1102

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
5 changes: 3 additions & 2 deletions ...or_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -382,8 +382,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface
const TimeWhileCollision & time_while_collision) const;
std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoid(
const std::vector<PathPointWithLaneId> & 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<DynamicAvoidanceObject> & 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<DynamicAvoidanceObject> & prev_object) const;

std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
const double forward_distance, const double backward_distance) const;
Expand Down
48 changes: 26 additions & 22 deletions planning/behavior_path_dynamic_avoidance_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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 /
Expand Down Expand Up @@ -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<PathPointWithLaneId> cropped_ego_path_points;
Expand Down Expand Up @@ -1204,28 +1205,31 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid(

std::optional<MinMaxValue> DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid(
const std::vector<PathPointWithLaneId> & 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<DynamicAvoidanceObject> & 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<DynamicAvoidanceObject> & 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 = [&]() {
Expand Down
Loading