diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 7b2e3fa0e3d04..324f023d31b77 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -112,6 +112,17 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( } } + // calculate orientation of each point + if (predicted_path.path.size() >= 2) { + for (size_t i = 0; i < predicted_path.path.size() - 1; i++) { + const auto yaw = tier4_autoware_utils::calcAzimuthAngle( + predicted_path.path.at(i).position, predicted_path.path.at(i + 1).position); + predicted_path.path.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + } + predicted_path.path.back().orientation = + predicted_path.path.at(predicted_path.path.size() - 2).orientation; + } + predicted_path.confidence = 1.0; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); diff --git a/planning/behavior_velocity_planner/config/blind_spot.param.yaml b/planning/behavior_velocity_planner/config/blind_spot.param.yaml index 31f75d7f7c5df..5af1c99f8a0c8 100644 --- a/planning/behavior_velocity_planner/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/blind_spot.param.yaml @@ -6,3 +6,4 @@ backward_length: 15.0 # [m] ignore_width_from_center_line: 0.7 # [m] max_future_movement_time: 10.0 # [second] + threshold_yaw_diff: 0.523 # [rad] diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index e3cacf3a362ba..4b1c672ff2b08 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -65,6 +65,7 @@ class BlindSpotModule : public SceneModuleInterface double ignore_width_from_center_line; //! ignore width from center line from detection_area double max_future_movement_time; //! maximum time[second] for considering future movement of object + double threshold_yaw_diff; //! threshold of yaw difference between ego and target object }; BlindSpotModule( @@ -153,7 +154,7 @@ class BlindSpotModule : public SceneModuleInterface */ bool isPredictedPathInArea( const autoware_auto_perception_msgs::msg::PredictedObject & object, - const lanelet::CompoundPolygon3d & area) const; + const lanelet::CompoundPolygon3d & area, geometry_msgs::msg::Pose ego_pose) const; /** * @brief Generate a stop line and insert it into the path. diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp index df5223d12f7d9..2d0ecee9cee30 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp @@ -36,6 +36,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".ignore_width_from_center_line", 1.0); planner_param_.max_future_movement_time = node.declare_parameter(ns + ".max_future_movement_time", 10.0); + planner_param_.threshold_yaw_diff = + node.declare_parameter(ns + ".threshold_yaw_diff", M_PI / 6.0); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 1b64566611cfe..4b81d11e4bcbb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -389,7 +389,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot( bool exist_in_detection_area = bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), lanelet::utils::to2D(areas_opt.get().detection_area)); - bool exist_in_conflict_area = isPredictedPathInArea(object, areas_opt.get().conflict_area); + bool exist_in_conflict_area = isPredictedPathInArea( + object, areas_opt.get().conflict_area, planner_data_->current_pose.pose); if (exist_in_detection_area || exist_in_conflict_area) { obstacle_detected = true; debug_data_.conflicting_targets.objects.push_back(object); @@ -403,16 +404,23 @@ bool BlindSpotModule::checkObstacleInBlindSpot( bool BlindSpotModule::isPredictedPathInArea( const autoware_auto_perception_msgs::msg::PredictedObject & object, - const lanelet::CompoundPolygon3d & area) const + const lanelet::CompoundPolygon3d & area, geometry_msgs::msg::Pose ego_pose) const { const auto area_2d = lanelet::utils::to2D(area); + const auto ego_yaw = tf2::getYaw(ego_pose.orientation); + const auto threshold_yaw_diff = planner_param_.threshold_yaw_diff; // NOTE: iterating all paths including those of low confidence return std::any_of( object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(), - [&area_2d](const auto & path) { - return std::any_of(path.path.begin(), path.path.end(), [&area_2d](const auto & point) { - return bg::within(to_bg2d(point.position), area_2d); - }); + [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & path) { + return std::any_of( + path.path.begin(), path.path.end(), + [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & point) { + const auto is_in_area = bg::within(to_bg2d(point.position), area_2d); + const auto match_yaw = + std::fabs(ego_yaw - tf2::getYaw(point.orientation)) < threshold_yaw_diff; + return is_in_area && match_yaw; + }); }); }