From 4c7efdefbf9bd2889e049f2d7659ed9f2195a16e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 31 May 2023 00:59:37 +0900 Subject: [PATCH] feat(dynamic_avoidance): apply LPF to shift length, and positive relative velocity Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 50 +++++++++++++++++-- planning/behavior_path_planner/package.xml | 1 + .../dynamic_avoidance_module.cpp | 35 +++++++++++-- 3 files changed, 76 insertions(+), 10 deletions(-) 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 1dadf8c02ad5e..217104eb7c938 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 @@ -67,9 +67,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface public: struct DynamicAvoidanceObject { - explicit DynamicAvoidanceObject( + DynamicAvoidanceObject( const PredictedObject & predicted_object, const double arg_path_projected_vel) - : pose(predicted_object.kinematics.initial_pose_with_covariance.pose), + : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), + pose(predicted_object.kinematics.initial_pose_with_covariance.pose), path_projected_vel(arg_path_projected_vel), shape(predicted_object.shape) { @@ -78,9 +79,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface } } - const geometry_msgs::msg::Pose pose; - const double path_projected_vel; - const autoware_auto_perception_msgs::msg::Shape shape; + std::string uuid; + geometry_msgs::msg::Pose pose; + double path_projected_vel; + autoware_auto_perception_msgs::msg::Shape shape; std::vector predicted_paths{}; bool is_left; @@ -124,7 +126,45 @@ class DynamicAvoidanceModule : public SceneModuleInterface const DynamicAvoidanceObject & object) const; 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_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index cd7311f3308bb..97453b0325cef 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -62,6 +62,7 @@ route_handler rtc_interface sensor_msgs + signal_processing tf2 tf2_eigen tf2_geometry_msgs 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 47e8c73c394ae..d8860c86ae112 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 @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "signal_processing/lowpass_filter_1d.hpp" #include #include @@ -128,6 +129,19 @@ void appendExtractedPolygonMarker( marker_array.markers.push_back(marker); } + +template +std::optional getObjectFromUuid(const std::vector & objects, const std::string & target_uuid) +{ + const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { + return object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; +} } // namespace #ifdef USE_OLD_ARCHITECTURE @@ -211,6 +225,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() // 3. 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) { @@ -218,8 +233,11 @@ 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; @@ -389,6 +407,7 @@ std::pair DynamicAvoidanceModule return std::make_pair(right_lanes, left_lanes); } +// NOTE: object does not have const only to update min_bound_lat_offset. std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -455,10 +474,8 @@ std::optional DynamicAvoidanceModule::calcDynam motion_utils::findNearestSegmentIndex(prev_module_path->points, object.pose.position); const double signed_lon_length = motion_utils::calcSignedArcLength( prev_module_path->points, getEgoPosition(), ego_seg_idx, object.pose.position, obj_seg_idx); - if (relative_velocity == 0.0) { - return std::numeric_limits::max(); - } - return signed_lon_length / relative_velocity; + const double positive_relative_velocity = std::max(relative_velocity, 1.0); + return signed_lon_length / positive_relative_velocity; }(); if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { @@ -535,13 +552,21 @@ std::optional DynamicAvoidanceModule::calcDynam const double max_bound_lat_offset = max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (object.is_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 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) + : min_bound_lat_offset; + prev_objects_min_bound_lat_offset_.update(object.uuid, filtered_min_bound_lat_offset); + // 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, min_bound_lat_offset, 0.0) + 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(