From 5439fb4647a6a79c23762c9520b6a22a982465eb Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 17 Jun 2022 11:50:27 +0900 Subject: [PATCH] fix(obstacle_cruise_planner): fix some bugs Signed-off-by: Takayuki Murooka --- .../obstacle_cruise_planner/polygon_utils.hpp | 5 ++- planning/obstacle_cruise_planner/src/node.cpp | 21 ++++++----- .../pid_based_planner/pid_based_planner.cpp | 7 ++-- .../src/polygon_utils.cpp | 37 +++++++++++++++---- .../obstacle_cruise_planner/src/utils.cpp | 1 - 5 files changed, 48 insertions(+), 23 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 8a34f49172cd2..f4994edf87427 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -46,13 +46,14 @@ boost::optional getFirstNonCollisionIndex( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const autoware_auto_perception_msgs::msg::Shape & shape, const size_t start_idx); -bool willCollideWithSurroundObstacle( +boost::optional willCollideWithSurroundObstacle( const autoware_auto_planning_msgs::msg::Trajectory & traj, const std::vector & traj_polygons, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const autoware_auto_perception_msgs::msg::Shape & shape, const double max_dist, const double ego_obstacle_overlap_time_threshold, - const double max_prediction_time_for_collision_check); + const double max_prediction_time_for_collision_check, + std::vector & collision_geom_points); std::vector createOneStepPolygons( const autoware_auto_planning_msgs::msg::Trajectory & traj, diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 5b0e67ce745e0..693ff60f93e74 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -67,7 +67,7 @@ bool isFrontObstacle( const double ego_to_obj_distance = tier4_autoware_utils::calcSignedArcLength(traj.points, ego_idx, obj_idx); - if (obj_idx == 0 && ego_to_obj_distance < 0) { + if (ego_to_obj_distance < 0) { return false; } @@ -537,7 +537,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( }(); if ( std::fabs(dist_from_obstacle_to_traj) > - obstacle_filtering_param_.rough_detection_area_expand_width) { + vehicle_info_.vehicle_width_m + obstacle_filtering_param_.rough_detection_area_expand_width) { RCLCPP_INFO_EXPRESSION( get_logger(), is_showing_debug_info_, "Ignore obstacles since it is far from the trajectory."); @@ -593,16 +593,15 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( const auto predicted_path_with_highest_confidence = getHighestConfidencePredictedPath(predicted_object); - const bool will_collide = polygon_utils::willCollideWithSurroundObstacle( + std::vector future_collision_points; + const auto collision_traj_poly_idx = polygon_utils::willCollideWithSurroundObstacle( decimated_traj, decimated_traj_polygons, predicted_path_with_highest_confidence, - predicted_object.shape, obstacle_filtering_param_.rough_detection_area_expand_width, + predicted_object.shape, + vehicle_info_.vehicle_width_m + obstacle_filtering_param_.rough_detection_area_expand_width, obstacle_filtering_param_.ego_obstacle_overlap_time_threshold, - obstacle_filtering_param_.max_prediction_time_for_collision_check); + obstacle_filtering_param_.max_prediction_time_for_collision_check, future_collision_points); - // TODO(murooka) think later - nearest_collision_point = object_pose.position; - - if (!will_collide) { + if (!collision_traj_poly_idx) { // Ignore condition 2 // Ignore vehicle obstacles outside the trajectory, whose predicted path // overlaps the ego trajectory in a certain time. @@ -613,6 +612,10 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( debug_data.intentionally_ignored_obstacles.push_back(predicted_object); continue; } + + nearest_collision_point = calcNearestCollisionPoint( + collision_traj_poly_idx.get(), future_collision_points, decimated_traj); + debug_data.collision_points.push_back(nearest_collision_point); } // convert to obstacle type diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index b3b771a8fea00..e55fc2b5d791c 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -231,7 +231,7 @@ void PIDBasedPlanner::calcObstaclesToCruiseAndStop( const double error_dist = dist_to_obstacle - longitudinal_info_.safe_distance_margin; if (stop_obstacle_info) { if (error_dist > stop_obstacle_info->dist_to_stop) { - return; + continue; } } stop_obstacle_info = StopObstacleInfo(obstacle, error_dist); @@ -253,7 +253,7 @@ void PIDBasedPlanner::calcObstaclesToCruiseAndStop( const double error_dist = dist_to_obstacle - rss_dist; if (cruise_obstacle_info) { if (error_dist > cruise_obstacle_info->dist_to_cruise) { - return; + continue; } } const double normalized_dist_to_cruise = error_dist / dist_to_obstacle; @@ -400,8 +400,7 @@ boost::optional PIDBasedPlanner::doStop( const auto marker_pose = obstacle_cruise_utils::calcForwardPose( planner_data.traj, ego_idx, modified_dist_to_stop + vehicle_info_.max_longitudinal_offset_m); if (marker_pose) { - visualization_msgs::msg::MarkerArray wall_msg; - const auto markers = tier4_autoware_utils::createStopVirtualWallMarker( + auto markers = tier4_autoware_utils::createStopVirtualWallMarker( marker_pose.get(), "obstacle stop", planner_data.current_time, 0); tier4_autoware_utils::appendMarkerArray(markers, &debug_wall_marker); } diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 444e9007303d3..d3758b7258835 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -235,13 +235,14 @@ boost::optional getFirstNonCollisionIndex( return {}; } -bool willCollideWithSurroundObstacle( +boost::optional willCollideWithSurroundObstacle( const autoware_auto_planning_msgs::msg::Trajectory & traj, const std::vector & traj_polygons, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const autoware_auto_perception_msgs::msg::Shape & shape, const double max_dist, const double ego_obstacle_overlap_time_threshold, - const double max_prediction_time_for_collision_check) + const double max_prediction_time_for_collision_check, + std::vector & collision_geom_points) { constexpr double epsilon = 1e-3; @@ -252,7 +253,7 @@ bool willCollideWithSurroundObstacle( if ( max_prediction_time_for_collision_check < rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(i)) { - return false; + return {}; } for (size_t j = 0; j < traj.points.size(); ++j) { @@ -269,13 +270,33 @@ bool willCollideWithSurroundObstacle( if (dist < epsilon) { if (!is_found) { - start_predicted_path_idx = i; - is_found = true; + // calculate collision point by polygon collision + std::deque collision_polygons; + boost::geometry::intersection(traj_polygon, obj_polygon, collision_polygons); + + bool has_collision = false; + for (const auto & collision_polygon : collision_polygons) { + if (boost::geometry::area(collision_polygon) > 0.0) { + has_collision = true; + + for (const auto & collision_point : collision_polygon.outer()) { + geometry_msgs::msg::Point collision_geom_point; + collision_geom_point.x = collision_point.x(); + collision_geom_point.y = collision_point.y(); + collision_geom_points.push_back(collision_geom_point); + } + } + } + + if (has_collision) { + start_predicted_path_idx = i; + is_found = true; + } } else { const double overlap_time = (static_cast(i) - start_predicted_path_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); if (ego_obstacle_overlap_time_threshold < overlap_time) { - return true; + return j; } } } else { @@ -284,8 +305,10 @@ bool willCollideWithSurroundObstacle( } } - return false; + collision_geom_points.clear(); + return {}; } + std::vector createOneStepPolygons( const autoware_auto_planning_msgs::msg::Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 5198d02b9074e..37da3103ba50b 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -35,7 +35,6 @@ visualization_msgs::msg::Marker getObjectMarker( tier4_autoware_utils::createMarkerScale(2.0, 2.0, 2.0), tier4_autoware_utils::createMarkerColor(r, g, b, 0.8)); - marker.lifetime = rclcpp::Duration::from_seconds(0.8); marker.pose = obstacle_pose; return marker;