diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 2b5e7317a90ee..7ef57ca67e14b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -35,6 +35,12 @@ namespace behavior_path_planner { +struct MinMaxValue +{ + double min_value; + double max_value; +}; + struct DynamicAvoidanceParameters { // common @@ -51,6 +57,7 @@ struct DynamicAvoidanceParameters bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; + int successive_num_to_exit_dynamic_avoidance_condition{0}; double min_obj_lat_offset_to_ego_path{0.0}; double max_obj_lat_offset_to_ego_path{0.0}; @@ -82,14 +89,17 @@ class DynamicAvoidanceModule : public SceneModuleInterface { DynamicAvoidanceObject( const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, - const bool arg_is_collision_left, const double arg_time_to_collision) + const bool arg_is_collision_left, const double arg_time_to_collision, + const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid) : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), shape(predicted_object.shape), vel(arg_vel), lat_vel(arg_lat_vel), is_collision_left(arg_is_collision_left), - time_to_collision(arg_time_to_collision) + time_to_collision(arg_time_to_collision), + lon_offset_to_avoid(arg_lon_offset_to_avoid), + lat_offset_to_avoid(arg_lat_offset_to_avoid) { for (const auto & path : predicted_object.kinematics.predicted_paths) { predicted_paths.push_back(path); @@ -103,25 +113,103 @@ class DynamicAvoidanceModule : public SceneModuleInterface double lat_vel; bool is_collision_left; double time_to_collision; + MinMaxValue lon_offset_to_avoid; + MinMaxValue lat_offset_to_avoid; std::vector predicted_paths{}; }; - struct DynamicAvoidanceObjectCandidate + + struct TargetObjectsManager { - DynamicAvoidanceObject object; - int alive_counter; + TargetObjectsManager(const int arg_max_count, const int arg_min_count) + : max_count_(arg_max_count), min_count_(arg_min_count) + { + } + int max_count_; + int min_count_; - static std::optional getObjectFromUuid( - const std::vector & objects, const std::string & target_uuid) + void initialize() { current_uuids_.clear(); } + void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) { - const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { - return object.object.uuid == target_uuid; - }); + // add/update object + if (object_map_.count(uuid) != 0) { + object_map_.at(uuid) = object; + } else { + object_map_.emplace(uuid, object); + } - if (itr == objects.end()) { + // increase counter + if (counter_map_.count(uuid) != 0) { + counter_map_.at(uuid) = std::min(max_count_ + 1, std::max(1, counter_map_.at(uuid) + 1)); + } else { + counter_map_.emplace(uuid, 1); + } + + // memorize uuid + current_uuids_.push_back(uuid); + } + + void finalize() + { + // decrease counter for not updated uuids + std::vector not_updated_uuids; + for (const auto & object : object_map_) { + if ( + std::find(current_uuids_.begin(), current_uuids_.end(), object.first) == + current_uuids_.end()) { + not_updated_uuids.push_back(object.first); + } + } + for (const auto & uuid : not_updated_uuids) { + if (counter_map_.count(uuid) != 0) { + counter_map_.at(uuid) = std::max(min_count_ - 1, std::min(-1, counter_map_.at(uuid) - 1)); + } else { + counter_map_.emplace(uuid, -1); + } + } + + // remove objects whose counter is lower than threshold + std::vector obsolete_uuids; + for (const auto & counter : counter_map_) { + if (counter.second < min_count_) { + obsolete_uuids.push_back(counter.first); + } + } + for (const auto & obsolete_uuid : obsolete_uuids) { + counter_map_.erase(obsolete_uuid); + object_map_.erase(obsolete_uuid); + } + } + std::vector getValidObjects() const + { + std::vector objects; + for (const auto & object : object_map_) { + if (counter_map_.count(object.first) != 0) { + if (max_count_ <= counter_map_.at(object.first)) { + objects.push_back(object.second); + } + } + } + return objects; + } + std::optional getValidObject(const std::string & uuid) const + { + // add/update object + if (counter_map_.count(uuid) == 0) { + return std::nullopt; + } + if (counter_map_.at(uuid) < max_count_) { return std::nullopt; } - return *itr; + if (object_map_.count(uuid) == 0) { + return std::nullopt; + } + return object_map_.at(uuid); } + + std::vector current_uuids_; + // NOTE: positive is for meeting entrying condition, and negative is for exiting. + std::unordered_map counter_map_; + std::unordered_map object_map_; }; #ifdef USE_OLD_ARCHITECTURE @@ -164,7 +252,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface }; bool isLabelTargetObstacle(const uint8_t label) const; - std::vector calcTargetObjectsCandidate(); + void updateTargetObjects(); + std::optional> calcPathForObjectPolygon() const; bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; @@ -179,54 +268,24 @@ class DynamicAvoidanceModule : public SceneModuleInterface const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( const std::vector & ego_path, const PredictedObject & object) const; + MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, + const double time_to_collision) const; + MinMaxValue calcMinMaxLateralOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const Polygon2d & obj_points, const bool is_collision_left, + const std::optional & prev_object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; std::optional calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::vector - prev_target_objects_candidate_; std::vector target_objects_; // std::vector prev_target_objects_; std::shared_ptr parameters_; - - struct ObjectsVariable - { - void resetCurrentUuids() { current_uuids_.clear(); } - void addCurrentUuid(const std::string & uuid) { current_uuids_.push_back(uuid); } - void removeCounterUnlessUpdated() - { - std::vector obsolete_uuids; - for (const auto & key_and_value : variable_) { - if ( - std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) == - current_uuids_.end()) { - obsolete_uuids.push_back(key_and_value.first); - } - } - - for (const auto & obsolete_uuid : obsolete_uuids) { - variable_.erase(obsolete_uuid); - } - } - - std::optional get(const std::string & uuid) const - { - if (variable_.count(uuid) != 0) { - return variable_.at(uuid); - } - return std::nullopt; - } - void update(const std::string & uuid, const double new_variable) - { - variable_.emplace(uuid, new_variable); - } - - std::unordered_map variable_; - std::vector current_uuids_; - }; - mutable ObjectsVariable prev_objects_min_bound_lat_offset_; + TargetObjectsManager target_objects_manager_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index adf9c557e690b..c6a85302dc4c1 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -41,13 +41,13 @@ geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & return geom_obj_point; } -std::pair getMinMaxValues(const std::vector & vec) +MinMaxValue getMinMaxValues(const std::vector & vec) { const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end())); const size_t max_idx = std::distance(vec.begin(), std::max_element(vec.begin(), vec.end())); - return std::make_pair(vec.at(min_idx), vec.at(max_idx)); + return MinMaxValue{vec.at(min_idx), vec.at(max_idx)}; } void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose) @@ -194,7 +194,11 @@ DynamicAvoidanceModule::DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{std::move(parameters)} parameters_{std::move(parameters)}, + target_objects_manager_{TargetObjectsManager( + parameters_->successive_num_to_entry_dynamic_avoidance_condition, + parameters_->successive_num_to_exit_dynamic_avoidance_condition)} #endif { } @@ -231,18 +235,10 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { - // calculate target objects candidate - const auto target_objects_candidate = calcTargetObjectsCandidate(); + // calculate target objects + updateTargetObjects(); - // calculate target objects considering flickering suppress - target_objects_.clear(); - for (const auto & target_object_candidate : target_objects_candidate) { - if ( - parameters_->successive_num_to_entry_dynamic_avoidance_condition <= - target_object_candidate.alive_counter) { - target_objects_.push_back(target_object_candidate.object); - } - } + target_objects_ = target_objects_manager_.getValidObjects(); } ModuleStatus DynamicAvoidanceModule::updateState() @@ -274,7 +270,6 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; - prev_objects_min_bound_lat_offset_.resetCurrentUuids(); for (const auto & object : target_objects_) { const auto obstacle_poly = calcDynamicObstaclePolygon(object); if (obstacle_poly) { @@ -283,11 +278,8 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() appendObjectMarker(info_marker_, object.pose); appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); - - prev_objects_min_bound_lat_offset_.addCurrentUuid(object.uuid); } } - prev_objects_min_bound_lat_offset_.removeCounterUnlessUpdated(); BehaviorModuleOutput output; output.path = prev_module_path; @@ -342,14 +334,17 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const return false; } -std::vector -DynamicAvoidanceModule::calcTargetObjectsCandidate() +void DynamicAvoidanceModule::updateTargetObjects() { const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - // convert predicted objects to dynamic avoidance objects - std::vector output_objects_candidate; + const auto path_points_for_object_polygon = calcPathForObjectPolygon(); + if (!path_points_for_object_polygon) { + return; + } + + target_objects_manager_.initialize(); for (const auto & predicted_object : predicted_objects) { const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; @@ -358,6 +353,7 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() predicted_object.kinematics.predicted_paths.begin(), predicted_object.kinematics.predicted_paths.end(), [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + const auto prev_object = target_objects_manager_.getValidObject(obj_uuid); // 1. check label const bool is_label_target_obstacle = @@ -417,7 +413,7 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const auto lat_lon_offset = getLateralLongitudinalOffset(prev_module_path->points, predicted_object); - // 6. check if object will not cut in or cut out + // 7. check if object will not cut in or cut out const bool will_object_cut_in = willObjectCutIn(prev_module_path->points, obj_path, obj_tangent_vel, lat_lon_offset); const bool will_object_cut_out = @@ -435,7 +431,7 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() continue; } - // 7. check if time to collision + // 8. check if time to collision const double time_to_collision = calcTimeToCollision(prev_module_path->points, obj_pose, obj_tangent_vel); if ( @@ -449,33 +445,60 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() obj_uuid.c_str()); continue; } + if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision is a small negative " + "value.", + obj_uuid.c_str()); + continue; + } - // calculate which side object will be against ego's path + // 9. calculate which side object will be against ego's path const auto future_obj_pose = object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); const bool is_collision_left = future_obj_pose ? isLeft(prev_module_path->points, future_obj_pose->position) : is_object_left; - // 8. calculate alive counter for filtering objects - const auto prev_target_object_candidate = - DynamicAvoidanceObjectCandidate::getObjectFromUuid(prev_target_objects_candidate_, obj_uuid); - const int alive_counter = - prev_target_object_candidate - ? std::min( - parameters_->successive_num_to_entry_dynamic_avoidance_condition, - prev_target_object_candidate->alive_counter + 1) - : 0; + // 10. calculate longitudinal and lateral offset to avoid + const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, predicted_object.shape); + const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( + *path_points_for_object_polygon, obj_pose, obj_points, obj_tangent_vel, time_to_collision); + const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( + *path_points_for_object_polygon, obj_points, is_collision_left, prev_object); const auto target_object = DynamicAvoidanceObject( - predicted_object, obj_tangent_vel, obj_normal_vel, is_collision_left, time_to_collision); - const auto target_object_candidate = - DynamicAvoidanceObjectCandidate{target_object, alive_counter}; - output_objects_candidate.push_back(target_object_candidate); + predicted_object, obj_tangent_vel, obj_normal_vel, is_collision_left, time_to_collision, + lon_offset_to_avoid, lat_offset_to_avoid); + + target_objects_manager_.updateObject(obj_uuid, target_object); } + target_objects_manager_.finalize(); +} - prev_target_objects_candidate_ = output_objects_candidate; - return output_objects_candidate; +std::optional> DynamicAvoidanceModule::calcPathForObjectPolygon() + const +{ + const auto & ego_pose = getEgoPose(); + const auto & rh = planner_data_->route_handler; + + // get path with backward margin + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), + "failed to find closest lanelet within route!!!"); + return std::nullopt; + } + + constexpr double forward_length = 100.0; + const double backward_length = 50.0; + const auto current_lanes = + rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length); + const auto path = utils::getCenterLinePath( + *rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters); + return path.points; } [[maybe_unused]] std::optional> @@ -666,128 +689,85 @@ DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudi const auto obj_lon_min_max_offset = getMinMaxValues(obj_lon_offset_vec); return LatLonOffset{ - obj_seg_idx, obj_lat_min_max_offset.second, obj_lat_min_max_offset.first, - obj_lon_min_max_offset.second, obj_lon_min_max_offset.first}; + obj_seg_idx, obj_lat_min_max_offset.max_value, obj_lat_min_max_offset.min_value, + obj_lon_min_max_offset.max_value, obj_lon_min_max_offset.min_value}; } -// NOTE: object does not have const only to update min_bound_lat_offset. -std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( - const DynamicAvoidanceObject & object) const +MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, + const double time_to_collision) const { - const auto ego_pose = getEgoPose(); - const auto & rh = planner_data_->route_handler; - - // get path with backward margin - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), - "failed to find closest lanelet within route!!!"); - return std::nullopt; - } - - auto path_with_backward_margin = [&]() { - constexpr double forward_length = 100.0; - const double backward_length = 50.0; - const auto current_lanes = - rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length); - return utils::getCenterLinePath( - *rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters); - }(); - const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(path_with_backward_margin.points, object.pose.position); - const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); - - // calculate min/max lateral offset from object to path - const auto [min_obj_lat_abs_offset, max_obj_lat_abs_offset] = [&]() { - std::vector obj_lat_abs_offset_vec; - for (size_t i = 0; i < obj_points.outer().size(); ++i) { - const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(path_with_backward_margin.points, geom_obj_point); - const double obj_point_lat_offset = motion_utils::calcLateralOffset( - path_with_backward_margin.points, geom_obj_point, obj_point_seg_idx); - obj_lat_abs_offset_vec.push_back(std::abs(obj_point_lat_offset)); - } - return getMinMaxValues(obj_lat_abs_offset_vec); - }(); - const double min_obj_lat_offset = - min_obj_lat_abs_offset * (object.is_collision_left ? 1.0 : -1.0); - const double max_obj_lat_offset = - max_obj_lat_abs_offset * (object.is_collision_left ? 1.0 : -1.0); + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, obj_pose.position); // calculate min/max longitudinal offset from object to path - const auto obj_lon_offset = [&]() -> std::optional> { + const auto obj_lon_offset = [&]() { std::vector obj_lon_offset_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( - path_with_backward_margin.points, obj_seg_idx, geom_obj_point); + path_points_for_object_polygon, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } - const auto [raw_min_obj_lon_offset, raw_max_obj_lon_offset] = - getMinMaxValues(obj_lon_offset_vec); - if (object.time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { - return std::nullopt; - } + const auto raw_obj_lon_offset = getMinMaxValues(obj_lon_offset_vec); - if (object.vel < 0) { - return std::make_pair( - raw_min_obj_lon_offset + object.vel * object.time_to_collision, raw_max_obj_lon_offset); + if (obj_vel < 0) { + return MinMaxValue{ + raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value}; } // NOTE: If time to collision is considered here, the ego is close to the object when starting // avoidance. // The ego should start avoidance before overtaking. - return std::make_pair(raw_min_obj_lon_offset, raw_max_obj_lon_offset); + return raw_obj_lon_offset; }(); - if (!obj_lon_offset) { - return std::nullopt; - } - const double min_obj_lon_offset = obj_lon_offset->first; - const double max_obj_lon_offset = obj_lon_offset->second; - // calculate bound start and end index - const bool is_object_overtaking = (0.0 <= object.vel); - // TODO(murooka) use getEgoSpeed() instead of object.vel + const bool is_object_overtaking = (0.0 <= obj_vel); + // TODO(murooka) use getEgoSpeed() instead of obj_vel const double start_length_to_avoid = - std::abs(object.vel) * (is_object_overtaking - ? parameters_->start_duration_to_avoid_overtaking_object - : parameters_->start_duration_to_avoid_oncoming_object); + std::abs(obj_vel) * (is_object_overtaking + ? parameters_->start_duration_to_avoid_overtaking_object + : parameters_->start_duration_to_avoid_oncoming_object); const double end_length_to_avoid = - std::abs(object.vel) * (is_object_overtaking - ? parameters_->end_duration_to_avoid_overtaking_object - : parameters_->end_duration_to_avoid_oncoming_object); - const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, min_obj_lon_offset - start_length_to_avoid, path_with_backward_margin.points); - const size_t updated_obj_seg_idx = - (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 - : obj_seg_idx; - const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, max_obj_lon_offset + end_length_to_avoid, - path_with_backward_margin.points); + std::abs(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object + : parameters_->end_duration_to_avoid_oncoming_object); - if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { - // NOTE: The obstacle is longitudinally out of the ego's trajectory. - return std::nullopt; - } - const size_t lon_bound_start_idx = - lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); - const size_t lon_bound_end_idx = - lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(path_with_backward_margin.points.size() - 1); + return MinMaxValue{ + obj_lon_offset.min_value - start_length_to_avoid, + obj_lon_offset.max_value + end_length_to_avoid}; +} + +MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const Polygon2d & obj_points, const bool is_collision_left, + const std::optional & prev_object) const +{ + // calculate min/max lateral offset from object to path + const auto obj_lat_abs_offset = [&]() { + std::vector obj_lat_abs_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const size_t obj_point_seg_idx = + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, geom_obj_point); + const double obj_point_lat_offset = motion_utils::calcLateralOffset( + path_points_for_object_polygon, geom_obj_point, obj_point_seg_idx); + obj_lat_abs_offset_vec.push_back(std::abs(obj_point_lat_offset)); + } + return getMinMaxValues(obj_lat_abs_offset_vec); + }(); + const double min_obj_lat_offset = obj_lat_abs_offset.min_value * (is_collision_left ? 1.0 : -1.0); + const double max_obj_lat_offset = obj_lat_abs_offset.max_value * (is_collision_left ? 1.0 : -1.0); // calculate bound min and max lateral offset const double min_bound_lat_offset = [&]() { const double raw_min_bound_lat_offset = - min_obj_lat_offset - - parameters_->lat_offset_from_obstacle * (object.is_collision_left ? 1.0 : -1.0); + min_obj_lat_offset - parameters_->lat_offset_from_obstacle * (is_collision_left ? 1.0 : -1.0); const double min_bound_lat_abs_offset_limit = planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid; - if (object.is_collision_left) { + if (is_collision_left) { if (raw_min_bound_lat_offset < min_bound_lat_abs_offset_limit) { return min_bound_lat_abs_offset_limit; } @@ -799,29 +779,66 @@ std::optional DynamicAvoidanceModule::calcDynam return raw_min_bound_lat_offset; }(); const double max_bound_lat_offset = - max_obj_lat_offset + - parameters_->lat_offset_from_obstacle * (object.is_collision_left ? 1.0 : -1.0); + max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (is_collision_left ? 1.0 : -1.0); // filter min_bound_lat_offset - const auto prev_min_bound_lat_offset = prev_objects_min_bound_lat_offset_.get(object.uuid); + const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional { + if (!prev_object) { + return std::nullopt; + } + return prev_object->lat_offset_to_avoid.min_value; + }(); const double filtered_min_bound_lat_offset = - prev_min_bound_lat_offset - ? signal_processing::lowpassFilter(min_bound_lat_offset, *prev_min_bound_lat_offset, 0.3) + prev_min_lat_avoid_to_offset + ? signal_processing::lowpassFilter(min_bound_lat_offset, *prev_min_lat_avoid_to_offset, 0.3) : min_bound_lat_offset; - prev_objects_min_bound_lat_offset_.update(object.uuid, filtered_min_bound_lat_offset); + + return MinMaxValue{filtered_min_bound_lat_offset, max_bound_lat_offset}; +} + +// NOTE: object does not have const only to update min_bound_lat_offset. +std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const +{ + auto path_points_for_object_polygon = calcPathForObjectPolygon(); + if (!path_points_for_object_polygon) { + return std::nullopt; + } + + const size_t obj_seg_idx = + motion_utils::findNearestSegmentIndex(*path_points_for_object_polygon, object.pose.position); + const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + + const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( + obj_seg_idx, object.lon_offset_to_avoid.min_value, *path_points_for_object_polygon); + const size_t updated_obj_seg_idx = + (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 + : obj_seg_idx; + const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( + updated_obj_seg_idx, object.lon_offset_to_avoid.max_value, *path_points_for_object_polygon); + + if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { + // NOTE: The obstacle is longitudinally out of the ego's trajectory. + return std::nullopt; + } + const size_t lon_bound_start_idx = + lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); + const size_t lon_bound_end_idx = + lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() + : static_cast(path_points_for_object_polygon->size() - 1); // create inner/outer bound points std::vector obj_inner_bound_points; std::vector obj_outer_bound_points; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( - path_with_backward_margin.points.at(i).point.pose, 0.0, filtered_min_bound_lat_offset, 0.0) - .position); - obj_outer_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( - path_with_backward_margin.points.at(i).point.pose, 0.0, max_bound_lat_offset, 0.0) - .position); + obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( + path_points_for_object_polygon->at(i).point.pose, 0.0, + object.lat_offset_to_avoid.min_value, 0.0) + .position); + obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( + path_points_for_object_polygon->at(i).point.pose, 0.0, + object.lat_offset_to_avoid.max_value, 0.0) + .position); } // create obj_polygon from inner/outer bound points diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 746aea4038497..7644a7e8a5007 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -48,6 +48,8 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); + p.successive_num_to_exit_dynamic_avoidance_condition = + node->declare_parameter(ns + "successive_num_to_exit_dynamic_avoidance_condition"); p.min_obj_lat_offset_to_ego_path = node->declare_parameter(ns + "min_obj_lat_offset_to_ego_path"); @@ -121,6 +123,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", p->successive_num_to_entry_dynamic_avoidance_condition); + updateParam( + parameters, ns + "successive_num_to_exit_dynamic_avoidance_condition", + p->successive_num_to_exit_dynamic_avoidance_condition); updateParam( parameters, ns + "min_obj_lat_offset_to_ego_path", p->min_obj_lat_offset_to_ego_path);