From dfa5a609dd6ef9f19bb795c4b26af6dce2a72da3 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 8 Aug 2022 15:31:08 +0900 Subject: [PATCH] feat(obstacle_cruise_planner): update obstacle cruise planner (#1531) * delete is on ego traj Signed-off-by: yutaka * update Signed-off-by: yutaka --- .../common_structs.hpp | 10 +++--- .../include/obstacle_cruise_planner/node.hpp | 2 +- planning/obstacle_cruise_planner/src/node.cpp | 33 ++++++++++--------- .../pid_based_planner/pid_based_planner.cpp | 6 +++- .../src/planner_interface.cpp | 6 ++-- .../obstacle_cruise_planner/src/utils.cpp | 4 +-- 6 files changed, 33 insertions(+), 28 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 1d8327556f729..43c74039c4447 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -50,8 +50,8 @@ struct TargetObstacle { TargetObstacle( const rclcpp::Time & arg_time_stamp, const PredictedObject & object, - const double aligned_velocity, const geometry_msgs::msg::PointStamped & arg_collision_point, - const bool arg_is_on_ego_trajectory) + const double aligned_velocity, + const std::vector & arg_collision_points) { time_stamp = arg_time_stamp; orientation_reliable = true; @@ -68,9 +68,8 @@ struct TargetObstacle predicted_paths.push_back(path); } - collision_point = arg_collision_point; + collision_points = arg_collision_points; has_stopped = false; - is_on_ego_trajectory = arg_is_on_ego_trajectory; } rclcpp::Time time_stamp; @@ -83,9 +82,8 @@ struct TargetObstacle Shape shape; std::string uuid; std::vector predicted_paths; - geometry_msgs::msg::PointStamped collision_point; + std::vector collision_points; bool has_stopped; - bool is_on_ego_trajectory; }; struct ObstacleCruisePlannerData diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 7ce6e578c0a9a..447c890798b46 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -90,7 +90,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, const Trajectory & traj, std::vector & target_obstacles); - geometry_msgs::msg::PointStamped calcNearestCollisionPoint( + std::vector calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, const Trajectory & decimated_traj, const bool is_driving_forward); diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 413c06f85f7ac..920f350177d94 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -686,17 +686,20 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // calculate collision points const auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(object_pose.pose, predicted_object.shape); - std::vector collision_points; + std::vector closest_collision_points; const auto first_within_idx = polygon_utils::getFirstCollisionIndex( - decimated_traj_polygons, obstacle_polygon, predicted_objects.header, collision_points); + decimated_traj_polygons, obstacle_polygon, predicted_objects.header, + closest_collision_points); // precise detection area filtering with polygons - geometry_msgs::msg::PointStamped nearest_collision_point; + std::vector collision_points; if (first_within_idx) { // obstacles inside the trajectory // calculate nearest collision point - nearest_collision_point = calcNearestCollisionPoint( - first_within_idx.get(), collision_points, decimated_traj, is_driving_forward); - debug_data.collision_points.push_back(nearest_collision_point.point); + collision_points = calcNearestCollisionPoint( + first_within_idx.get(), closest_collision_points, decimated_traj, is_driving_forward); + if (!collision_points.empty()) { + debug_data.collision_points.push_back(collision_points.front().point); + } const bool is_angle_aligned = isAngleAlignedWithTrajectory( decimated_traj, object_pose.pose, @@ -705,9 +708,9 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( std::abs(object_velocity) > obstacle_filtering_param_.crossing_obstacle_velocity_threshold; // ignore running vehicle crossing the ego trajectory with high speed with some condition - if (!is_angle_aligned && has_high_speed) { + if (!is_angle_aligned && has_high_speed && !collision_points.empty()) { const double collision_time_margin = calcCollisionTimeMargin( - current_pose, current_vel, nearest_collision_point.point, predicted_object, + current_pose, current_vel, collision_points.front().point, predicted_object, first_within_idx.get(), decimated_traj, decimated_traj_polygons, is_driving_forward); if (collision_time_margin > obstacle_filtering_param_.collision_time_margin) { // Ignore vehicle obstacles inside the trajectory, which is crossing the trajectory with @@ -765,18 +768,18 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( continue; } - nearest_collision_point = calcNearestCollisionPoint( + collision_points = calcNearestCollisionPoint( collision_traj_poly_idx.get(), future_collision_points, decimated_traj, is_driving_forward); - debug_data.collision_points.push_back(nearest_collision_point.point); + if (!collision_points.empty()) { + debug_data.collision_points.push_back(collision_points.front().point); + } } // convert to obstacle type - const bool is_on_ego_traj = first_within_idx ? true : false; const double trajectory_aligned_adaptive_cruise = calcAlignedAdaptiveCruise(predicted_object, traj); const auto target_obstacle = TargetObstacle( - time_stamp, predicted_object, trajectory_aligned_adaptive_cruise, nearest_collision_point, - is_on_ego_traj); + time_stamp, predicted_object, trajectory_aligned_adaptive_cruise, collision_points); target_obstacles.push_back(target_obstacle); } @@ -895,7 +898,7 @@ void ObstacleCruisePlannerNode::checkConsistency( } } -geometry_msgs::msg::PointStamped ObstacleCruisePlannerNode::calcNearestCollisionPoint( +std::vector ObstacleCruisePlannerNode::calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, const Trajectory & decimated_traj, const bool is_driving_forward) @@ -931,7 +934,7 @@ geometry_msgs::msg::PointStamped ObstacleCruisePlannerNode::calcNearestCollision } } - return collision_points.at(min_idx); + return {collision_points.at(min_idx)}; } double ObstacleCruisePlannerNode::calcCollisionTimeMargin( 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 a24a2501a231a..9f8150b49bd27 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 @@ -124,6 +124,10 @@ void PIDBasedPlanner::calcObstaclesToCruise( for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) { const auto & obstacle = planner_data.target_obstacles.at(o_idx); + if (obstacle.collision_points.empty()) { + continue; + } + // NOTE: from ego's front to obstacle's back const double dist_to_obstacle = calcDistanceToObstacle(planner_data, obstacle); @@ -167,7 +171,7 @@ double PIDBasedPlanner::calcDistanceToObstacle( const double offset = abs_ego_offset + segment_offset; return motion_utils::calcSignedArcLength( - planner_data.traj.points, ego_segment_idx, obstacle.collision_point.point) - + planner_data.traj.points, ego_segment_idx, obstacle.collision_points.front().point) - offset; } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index afddd8349cbb0..259e0e1f96c74 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -37,7 +37,7 @@ tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( // create stop factor tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = stop_pose; - stop_factor.stop_factor_points.emplace_back(stop_obstacle.collision_point.point); + stop_factor.stop_factor_points.emplace_back(stop_obstacle.collision_points.front().point); // create stop reason stamped tier4_planning_msgs::msg::StopReason stop_reason_msg; @@ -102,7 +102,7 @@ Trajectory PlannerInterface::generateStopTrajectory( // Get Closest Stop Obstacle const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj, planner_data.target_obstacles); - if (!closest_stop_obstacle) { + if (!closest_stop_obstacle && closest_stop_obstacle->collision_points.empty()) { // delete marker const auto markers = motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); @@ -113,7 +113,7 @@ Trajectory PlannerInterface::generateStopTrajectory( // Get Closest Obstacle Stop Distance const double closest_obstacle_dist = motion_utils::calcSignedArcLength( - planner_data.traj.points, 0, closest_stop_obstacle->collision_point.point); + planner_data.traj.points, 0, closest_stop_obstacle->collision_points.front().point); const auto negative_dist_to_ego = motion_utils::calcSignedArcLength( planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_, diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 9a0f959751af8..8aecbf393bcc0 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -154,12 +154,12 @@ boost::optional getClosestStopObstacle( double dist_to_closest_stop_obstacle = std::numeric_limits::max(); for (const auto & obstacle : target_obstacles) { // Ignore obstacle that has not stopped - if (!obstacle.has_stopped) { + if (!obstacle.has_stopped || obstacle.collision_points.empty()) { continue; } const double dist_to_stop_obstacle = - motion_utils::calcSignedArcLength(traj.points, 0, obstacle.collision_point.point); + motion_utils::calcSignedArcLength(traj.points, 0, obstacle.collision_points.front().point); if (dist_to_stop_obstacle < dist_to_closest_stop_obstacle) { dist_to_closest_stop_obstacle = dist_to_stop_obstacle; closest_stop_obstacle = obstacle;