diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 01b6b3d278a33..cb9a61b600e43 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -13,7 +13,7 @@ history_time_length: 1.0 #[s] lane_change_detection: - method: "time_to_change_lane" # choose from "lat_diff_distance" or "time_to_change_lane" + method: "lat_diff_distance" # choose from "lat_diff_distance" or "time_to_change_lane" time_to_change_lane: dist_threshold_for_lane_change_detection: 1.0 #[m] time_threshold_for_lane_change_detection: 5.0 #[s] @@ -23,5 +23,6 @@ dist_ratio_threshold_to_right_bound: 0.5 #[ratio diff_dist_threshold_to_left_bound: 0.29 #[m] diff_dist_threshold_to_right_bound: -0.29 #[m] + num_continuous_state_transition: 3 reference_path_resolution: 0.5 #[m] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 4ac5fd76babea..dc6c74c25a539 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -59,6 +59,13 @@ struct LateralKinematicsToLanelet double filtered_right_lateral_velocity; }; +enum class Maneuver { + UNINITIALIZED = 0, + LANE_FOLLOW = 1, + LEFT_LANE_CHANGE = 2, + RIGHT_LANE_CHANGE = 3, +}; + struct ObjectData { std_msgs::msg::Header header; @@ -69,12 +76,9 @@ struct ObjectData double time_delay; // for lane change prediction std::unordered_map lateral_kinematics_set; -}; - -enum class Maneuver { - LANE_FOLLOW = 0, - LEFT_LANE_CHANGE = 1, - RIGHT_LANE_CHANGE = 2, + Maneuver one_shot_maneuver{Maneuver::UNINITIALIZED}; + Maneuver output_maneuver{ + Maneuver::UNINITIALIZED}; // output maneuver considering previous one shot maneuvers }; struct LaneletData @@ -153,6 +157,7 @@ class MapBasedPredictionNode : public rclcpp::Node double dist_ratio_threshold_to_right_bound_; double diff_dist_threshold_to_left_bound_; double diff_dist_threshold_to_right_bound_; + int num_continuous_state_transition_; double reference_path_resolution_; // Stop watch diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index dd18083af88b9..68f31593116f7 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -517,6 +517,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_left_bound"); diff_dist_threshold_to_right_bound_ = declare_parameter( "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_right_bound"); + + num_continuous_state_transition_ = + declare_parameter("lane_change_detection.num_continuous_state_transition"); } reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5); @@ -1170,14 +1173,50 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time) { - if (lane_change_detection_method_ == "time_to_change_lane") { - return predictObjectManeuverByTimeToLaneChange( - object, current_lanelet_data, object_detected_time); - } else if (lane_change_detection_method_ == "lat_diff_distance") { - return predictObjectManeuverByLatDiffDistance( - object, current_lanelet_data, object_detected_time); - } - throw std::logic_error("Lane change detection method is invalid."); + // calculate maneuver + const auto current_maneuver = [&]() { + if (lane_change_detection_method_ == "time_to_change_lane") { + return predictObjectManeuverByTimeToLaneChange( + object, current_lanelet_data, object_detected_time); + } else if (lane_change_detection_method_ == "lat_diff_distance") { + return predictObjectManeuverByLatDiffDistance( + object, current_lanelet_data, object_detected_time); + } + throw std::logic_error("Lane change detection method is invalid."); + }(); + + const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + if (objects_history_.count(object_id) == 0) { + return current_maneuver; + } + auto & object_info = objects_history_.at(object_id); + + // update maneuver in object history + if (!object_info.empty()) { + object_info.back().one_shot_maneuver = current_maneuver; + } + + // decide maneuver considering previous results + if (object_info.size() < 2) { + object_info.back().output_maneuver = current_maneuver; + return current_maneuver; + } + // NOTE: The index of previous maneuver is not object_info.size() - 1 + const auto prev_output_maneuver = + object_info.at(static_cast(object_info.size()) - 2).output_maneuver; + + for (int i = 0; + i < std::min(num_continuous_state_transition_, static_cast(object_info.size())); ++i) { + const auto & tmp_maneuver = + object_info.at(static_cast(object_info.size()) - 1 - i).one_shot_maneuver; + if (tmp_maneuver != current_maneuver) { + object_info.back().output_maneuver = prev_output_maneuver; + return prev_output_maneuver; + } + } + + object_info.back().output_maneuver = current_maneuver; + return current_maneuver; } Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( @@ -1253,7 +1292,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( const TrackedObject & object, const LaneletData & current_lanelet_data, - const double object_detected_time) + const double /*object_detected_time*/) { // Step1. Check if we have the object in the buffer const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); @@ -1283,7 +1322,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( // Step3. Get closest previous lanelet ID const auto & prev_info = object_info.at(static_cast(prev_id)); - const auto prev_pose = compensateTimeDelay(prev_info.pose, prev_info.twist, prev_info.time_delay); + const auto prev_pose = prev_info.pose; const lanelet::ConstLanelets prev_lanelets = object_info.at(static_cast(prev_id)).current_lanelets; if (prev_lanelets.empty()) { @@ -1303,19 +1342,20 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( // Step4. Check if the vehicle has changed lane const auto current_lanelet = current_lanelet_data.lanelet; - const double current_time_delay = std::max(current_time - object_detected_time, 0.0); - const auto current_pose = compensateTimeDelay( - object.kinematics.pose_with_covariance.pose, object.kinematics.twist_with_covariance.twist, - current_time_delay); + const auto current_pose = object.kinematics.pose_with_covariance.pose; const double dist = tier4_autoware_utils::calcDistance2d(prev_pose, current_pose); lanelet::routing::LaneletPaths possible_paths = routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); bool has_lane_changed = true; - for (const auto & path : possible_paths) { - for (const auto & lanelet : path) { - if (lanelet == current_lanelet) { - has_lane_changed = false; - break; + if (prev_lanelet == current_lanelet) { + has_lane_changed = false; + } else { + for (const auto & path : possible_paths) { + for (const auto & lanelet : path) { + if (lanelet == current_lanelet) { + has_lane_changed = false; + break; + } } } } diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index a4ff684839321..0b8f35bac3131 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -12,21 +12,23 @@ motorcycle: true pedestrian: false - min_obstacle_vel: 1.0 # [m/s] + min_obstacle_vel: 0.0 # [m/s] + + successive_num_to_entry_dynamic_avoidance_condition: 5 drivable_area_generation: - lat_offset_from_obstacle: 1.3 # [m] + lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] # for same directional object overtaking_object: max_time_to_collision: 3.0 # [s] start_duration_to_avoid: 4.0 # [s] - end_duration_to_avoid: 5.0 # [s] + end_duration_to_avoid: 8.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: max_time_to_collision: 3.0 # [s] - start_duration_to_avoid: 9.0 # [s] + start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] 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..4898188c71319 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 @@ -47,6 +47,7 @@ struct DynamicAvoidanceParameters bool avoid_motorcycle{false}; bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; + int successive_num_to_entry_dynamic_avoidance_condition{0}; // drivable area generation double lat_offset_from_obstacle{0.0}; @@ -67,10 +68,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface public: struct DynamicAvoidanceObject { - explicit DynamicAvoidanceObject( - const PredictedObject & predicted_object, const double arg_path_projected_vel) - : pose(predicted_object.kinematics.initial_pose_with_covariance.pose), - path_projected_vel(arg_path_projected_vel), + DynamicAvoidanceObject( + const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel) + : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), + pose(predicted_object.kinematics.initial_pose_with_covariance.pose), + vel(arg_vel), + lat_vel(arg_lat_vel), shape(predicted_object.shape) { for (const auto & path : predicted_object.kinematics.predicted_paths) { @@ -78,13 +81,33 @@ 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 vel; + double lat_vel; + autoware_auto_perception_msgs::msg::Shape shape; std::vector predicted_paths{}; bool is_left; }; + struct DynamicAvoidanceObjectCandidate + { + DynamicAvoidanceObject object; + int alive_counter; + + static 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.object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; + } + }; #ifdef USE_OLD_ARCHITECTURE DynamicAvoidanceModule( @@ -117,14 +140,54 @@ class DynamicAvoidanceModule : public SceneModuleInterface private: bool isLabelTargetObstacle(const uint8_t label) const; - std::vector calcTargetObjects() const; + std::vector calcTargetObjectsCandidate() 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_; }; } // 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/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index d802f783226bf..430fbd20793f2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -721,6 +721,8 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam() p.avoid_motorcycle = declare_parameter(ns + "motorcycle"); p.avoid_pedestrian = declare_parameter(ns + "pedestrian"); p.min_obstacle_vel = declare_parameter(ns + "min_obstacle_vel"); + p.successive_num_to_entry_dynamic_avoidance_condition = + declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); } { // drivable_area_generation 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 98f4af759252e..42d630ee90043 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 @@ -72,20 +73,6 @@ geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & return geom_obj_point; } -double calcObstacleProjectedVelocity( - const std::vector & path_points, const PredictedObject & object) -{ - const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - - const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - - const double obj_yaw = tf2::getYaw(obj_pose.orientation); - const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); - - return obj_vel * std::cos(obj_yaw - path_yaw); -} - std::pair getMinMaxValues(const std::vector & vec) { const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end())); @@ -128,6 +115,34 @@ 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; +} + +std::pair projectObstacleVelocityToTrajectory( + const std::vector & path_points, const PredictedObject & object) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; + + const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); + + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); + + return std::make_pair( + obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw)); +} } // namespace #ifdef USE_OLD_ARCHITECTURE @@ -178,7 +193,19 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { - target_objects_ = calcTargetObjects(); + // calculate target objects candidate + const auto target_objects_candidate = calcTargetObjectsCandidate(); + prev_target_objects_candidate_ = target_objects_candidate; + + // 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); + } + } } ModuleStatus DynamicAvoidanceModule::updateState() @@ -213,6 +240,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) { @@ -220,8 +248,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; @@ -277,8 +308,8 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const return false; } -std::vector -DynamicAvoidanceModule::calcTargetObjects() const +std::vector +DynamicAvoidanceModule::calcTargetObjectsCandidate() const { const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; @@ -293,14 +324,14 @@ DynamicAvoidanceModule::calcTargetObjects() const continue; } - const double path_projected_vel = - calcObstacleProjectedVelocity(prev_module_path->points, predicted_object); + const auto [tangent_vel, normal_vel] = + projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); // check if velocity is high enough - if (std::abs(path_projected_vel) < parameters_->min_obstacle_vel) { + if (std::abs(tangent_vel) < parameters_->min_obstacle_vel) { continue; } - input_objects.push_back(DynamicAvoidanceObject(predicted_object, path_projected_vel)); + input_objects.push_back(DynamicAvoidanceObject(predicted_object, tangent_vel, normal_vel)); } // 2. calculate target lanes to filter obstacles @@ -310,10 +341,10 @@ DynamicAvoidanceModule::calcTargetObjects() const const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes); const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes); - // 4. check if object will cut into the ego lane. + // 4. check if object will cut into the ego lane or cut out to the next lane. // NOTE: The oncoming object will be ignored. constexpr double epsilon_path_lat_diff = 0.3; - std::vector output_objects; + std::vector output_objects_candidate; for (const bool is_left : {true, false}) { for (const auto & object : (is_left ? objects_in_left_lanes : objects_in_right_lanes)) { const auto reliable_predicted_path = std::max_element( @@ -324,7 +355,7 @@ DynamicAvoidanceModule::calcTargetObjects() const // Ignore object that will cut into the ego lane const bool will_object_cut_in = [&]() { - if (object.path_projected_vel < 0) { + if (object.vel < 0) { // Ignore oncoming object return false; } @@ -342,13 +373,47 @@ DynamicAvoidanceModule::calcTargetObjects() const continue; } + // Ignore object that will cut out to the next lane + const bool will_object_cut_out = [&]() { + if (object.vel < 0) { + // Ignore oncoming object + return false; + } + + constexpr double object_lat_vel_thresh = 0.3; + if (is_left) { + if (object_lat_vel_thresh < object.lat_vel) { + return true; + } + } else { + if (object.lat_vel < -object_lat_vel_thresh) { + return true; + } + } + return false; + }(); + if (will_object_cut_out) { + continue; + } + + // get previous object if it exists + const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid( + prev_target_objects_candidate_, object.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; + auto target_object = object; target_object.is_left = is_left; - output_objects.push_back(target_object); + output_objects_candidate.push_back( + DynamicAvoidanceObjectCandidate{target_object, alive_counter}); } } - return output_objects; + return output_objects_candidate; } std::pair DynamicAvoidanceModule::getAdjacentLanes( @@ -391,6 +456,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 { @@ -449,7 +515,7 @@ std::optional DynamicAvoidanceModule::calcDynam getMinMaxValues(obj_lon_offset_vec); // calculate time to collision and apply it to drivable area extraction - const double relative_velocity = getEgoSpeed() - object.path_projected_vel; + const double relative_velocity = getEgoSpeed() - object.vel; const double time_to_collision = [&]() { const auto prev_module_path = getPreviousModuleOutput().path; const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); @@ -457,29 +523,26 @@ 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) { return std::nullopt; } - if (0 <= object.path_projected_vel) { + if (0 <= object.vel) { const double limited_time_to_collision = std::min(parameters_->max_time_to_collision_overtaking_object, time_to_collision); return std::make_pair( - raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision, - raw_max_obj_lon_offset + object.path_projected_vel * limited_time_to_collision); + raw_min_obj_lon_offset + object.vel * limited_time_to_collision, + raw_max_obj_lon_offset + object.vel * limited_time_to_collision); } const double limited_time_to_collision = std::min(parameters_->max_time_to_collision_oncoming_object, time_to_collision); return std::make_pair( - raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision, - raw_max_obj_lon_offset); + raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset); }(); if (!obj_lon_offset) { @@ -489,15 +552,15 @@ std::optional DynamicAvoidanceModule::calcDynam const double max_obj_lon_offset = obj_lon_offset->second; // calculate bound start and end index - const bool is_object_overtaking = (0.0 <= object.path_projected_vel); + const bool is_object_overtaking = (0.0 <= object.vel); const double start_length_to_avoid = - std::abs(object.path_projected_vel) * - (is_object_overtaking ? parameters_->start_duration_to_avoid_overtaking_object - : parameters_->start_duration_to_avoid_oncoming_object); + std::abs(object.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.path_projected_vel) * (is_object_overtaking - ? parameters_->end_duration_to_avoid_overtaking_object - : parameters_->end_duration_to_avoid_oncoming_object); + 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 = @@ -537,13 +600,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( 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 d3212a7c0b1d4..b9ca4ffa964f7 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 @@ -49,6 +49,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); + + updateParam( + parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", + p->successive_num_to_entry_dynamic_avoidance_condition); } { // drivable_area_generation diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 77fd29f5e1382..778bc8bba92d6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -243,9 +243,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() out.path = std::make_shared(*prev_approved_path_); out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - - out.drivable_area_info.drivable_lanes = - getPreviousModuleOutput().drivable_area_info.drivable_lanes; + out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index ae192ddc47c72..ccc6a698a99d3 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -169,14 +169,49 @@ boost::optional> intersectBound( return boost::none; } +double calcDistanceFromPointToSegment( + const geometry_msgs::msg::Point & segment_start_point, + const geometry_msgs::msg::Point & segment_end_point, + const geometry_msgs::msg::Point & target_point) +{ + const auto & a = segment_start_point; + const auto & b = segment_end_point; + const auto & p = target_point; + + const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); + const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b); + if (0 <= dot_val && dot_val <= squared_segment_length) { + const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x)); + const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); + return numerator / denominator; + } + + // target_point is outside the segment. + return std::min( + tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p)); +} + PolygonPoint transformBoundFrenetCoordinate( - const std::vector & points, const geometry_msgs::msg::Point & point) + const std::vector & bound_points, + const geometry_msgs::msg::Point & target_point) { - const size_t seg_idx = motion_utils::findNearestSegmentIndex(points, point); + // NOTE: findNearestSegmentIndex cannot be used since a bound's interval is sometimes too large to + // find wrong nearest index. + std::vector dist_to_bound_segment_vec; + for (size_t i = 0; i < bound_points.size() - 1; ++i) { + const double dist_to_bound_segment = + calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point); + dist_to_bound_segment_vec.push_back(dist_to_bound_segment); + } + + const size_t min_dist_seg_idx = std::distance( + dist_to_bound_segment_vec.begin(), + std::min_element(dist_to_bound_segment_vec.begin(), dist_to_bound_segment_vec.end())); const double lon_dist_to_segment = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, point); - const double lat_dist = motion_utils::calcLateralOffset(points, point, seg_idx); - return PolygonPoint{point, seg_idx, lon_dist_to_segment, lat_dist}; + motion_utils::calcLongitudinalOffsetToSegment(bound_points, min_dist_seg_idx, target_point); + const double lat_dist_to_segment = + motion_utils::calcLateralOffset(bound_points, target_point, min_dist_seg_idx); + return PolygonPoint{target_point, min_dist_seg_idx, lon_dist_to_segment, lat_dist_to_segment}; } std::vector generatePolygonInsideBounds( @@ -198,7 +233,9 @@ std::vector generatePolygonInsideBounds( full_polygon.push_back(polygon_point); } + // 1. check the case where the polygon intersects the bound std::vector inside_poly; + bool has_intersection = false; // NOTE: between obstacle polygon and bound for (int i = 0; i < static_cast(full_polygon.size()); ++i) { const auto & curr_poly = full_polygon.at(i); const auto & prev_poly = full_polygon.at(i == 0 ? full_polygon.size() - 1 : i - 1); @@ -223,6 +260,7 @@ std::vector generatePolygonInsideBounds( bound, intersection->first, intersection->second); const auto intersect_point = PolygonPoint{intersection->second, intersection->first, lon_dist, 0.0}; + has_intersection = true; if (is_prev_outside && !is_curr_outside) { inside_poly.push_back(intersect_point); @@ -234,8 +272,25 @@ std::vector generatePolygonInsideBounds( inside_poly.push_back(intersect_point); continue; } + if (has_intersection) { + return inside_poly; + } + + // 2. check the case where the polygon does not intersect the bound + const bool is_polygon_fully_inside_bounds = [&]() { + for (const auto & curr_poly : full_polygon) { + const bool is_curr_outside = curr_poly.is_outside_bounds(is_object_right); + if (is_curr_outside) { + return false; + } + } + return true; + }(); + if (is_polygon_fully_inside_bounds) { + return full_polygon; + } - return inside_poly; + return std::vector{}; } std::vector convertToGeometryPoints( @@ -2878,8 +2933,7 @@ void extractObstaclesFromDrivableArea( } } - for (size_t i = 0; i < 2; ++i) { // for loop for right and left - const bool is_object_right = (i == 0); + for (const bool is_object_right : {true, false}) { const auto & polygons = is_object_right ? right_polygons : left_polygons; if (polygons.empty()) { continue; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp index 14a0bacff334d..efc75f4c90bba 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp @@ -54,7 +54,7 @@ namespace trajectory_utils template std::optional getPointIndexAfter( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double offset) + const double max_offset, const double min_offset) { if (points.empty()) { return std::nullopt; @@ -63,28 +63,35 @@ std::optional getPointIndexAfter( double sum_length = -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + std::optional output_idx{std::nullopt}; + // search forward - if (sum_length < offset) { + if (sum_length < min_offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); - if (offset < sum_length) { - return i - 1; + if (min_offset < sum_length) { + output_idx = i - 1; + } + if (max_offset < sum_length) { + break; } } - - return std::nullopt; + return output_idx; } // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); - if (sum_length < offset) { - return i - 1; + if (sum_length < min_offset) { + output_idx = i - 1; + } + if (sum_length < max_offset) { + break; } } - return 0; + return output_idx; } template diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 5171dfddc88e6..439b68e9032dd 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -921,7 +921,7 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin // Only the Lower bound is cut out. Widen the bounds towards the lower bound since cut out too // much. b.lower_bound = - std::min(b.lower_bound, original_b.upper_bound + mpt_param_.min_drivable_width); + std::min(b.lower_bound, original_b.upper_bound - mpt_param_.min_drivable_width); continue; } // extend longitudinal if it overlaps out_of_upper_bound_sections @@ -971,7 +971,7 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin // Only the Upper bound is cut out. Widen the bounds towards the upper bound since cut out too // much. b.upper_bound = - std::max(b.upper_bound, original_b.lower_bound - mpt_param_.min_drivable_width); + std::max(b.upper_bound, original_b.lower_bound + mpt_param_.min_drivable_width); continue; } // extend longitudinal if it overlaps out_of_lower_bound_sections diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index f6628ab5204a3..25b65b92c0924 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -512,26 +512,37 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( trajectory_utils::findEgoSegmentIndex(traj_points, joint_start_pose, ego_nearest_param_); // crop trajectory for extension - constexpr double joint_traj_length_for_smoothing = 5.0; + constexpr double joint_traj_max_length_for_smoothing = 15.0; + constexpr double joint_traj_min_length_for_smoothing = 5.0; const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( traj_points, joint_start_pose.position, joint_start_traj_seg_idx, - joint_traj_length_for_smoothing); + joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing); // calculate full trajectory points const auto full_traj_points = [&]() { if (!joint_end_traj_point_idx) { return optimized_traj_points; } + const auto extended_traj_points = std::vector{ traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; - return concatVectors(optimized_traj_points, extended_traj_points); + + // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is + // zero velocity, the zero velocity will be inserted in the whole joint trajectory. + auto modified_optimized_traj_points = optimized_traj_points; + if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) { + modified_optimized_traj_points.back().longitudinal_velocity_mps = + extended_traj_points.front().longitudinal_velocity_mps; + } + + return concatVectors(modified_optimized_traj_points, extended_traj_points); }(); // resample trajectory points auto resampled_traj_points = trajectory_utils::resampleTrajectoryPoints( full_traj_points, traj_param_.output_delta_arc_length); - // update velocity on joint + // update stop velocity on joint for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { if (hasZeroVelocity(traj_points.at(i))) { if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) {