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 8e6ab24929a00..1f5cf8c2a9500 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 @@ -107,14 +107,15 @@ std::pair projectObstacleVelocityToTrajectory( const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - const double obj_vel_yaw = std::atan2( - object.kinematics.initial_twist_with_covariance.twist.linear.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x); + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double obj_vel_yaw = + obj_yaw + std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); - return std::make_pair( - obj_vel_norm * std::cos(obj_vel_yaw - path_yaw), - obj_vel_norm * std::sin(obj_vel_yaw - path_yaw)); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_vel_yaw - path_yaw); + return std::make_pair(obj_vel_norm * std::cos(diff_yaw), obj_vel_norm * std::sin(diff_yaw)); } double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) @@ -517,7 +518,31 @@ void DynamicAvoidanceModule::updateTargetObjects() ? isLeft(prev_module_path->points, future_obj_pose->position) : is_object_left; - // 2.g. calculate longitudinal and lateral offset to avoid + // 2.g. check if the ego is not ahead of the object. + const double signed_dist_ego_to_obj = [&]() { + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); + const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( + prev_module_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + if (0 < lon_offset_ego_to_obj) { + return std::max( + 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + + lat_lon_offset.min_lon_offset); + } + return std::min( + 0.0, lon_offset_ego_to_obj + planner_data_->parameters.rear_overhang + + lat_lon_offset.max_lon_offset); + }(); + if (signed_dist_ego_to_obj < 0) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since distance from ego to object (%f) is less " + "than 0.", + obj_uuid.c_str(), signed_dist_ego_to_obj); + continue; + } + + // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by + // "object_path_base" const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision);