From cde973161e3603993099960c6ff0067f2c1d8157 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 2 Nov 2023 18:49:16 +0900 Subject: [PATCH] complete to implementation Signed-off-by: Yuki Takagi --- .../motion_utils/trajectory/trajectory.hpp | 1 + planning/obstacle_cruise_planner/src/node.cpp | 2 +- .../src/planner_interface.cpp | 383 ++++++++---------- 3 files changed, 179 insertions(+), 207 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 5d773c5be32d9..11f83a6d306f4 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -220,6 +220,7 @@ boost::optional searchZeroVelocityIndex( constexpr double epsilon = 1e-3; for (size_t i = src_idx; i < dst_idx; ++i) { + std::cerr << std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) << std::endl; if (std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) < epsilon) { return i; } diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index adbafc308620d..e7c47f960b48b 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1106,7 +1106,7 @@ std::optional ObstacleCruisePlannerNode::calcTrajDistToSecurePolysDist( } last_dist = sec_i_dist; } - return 0.0; + return last_dist - dist_to_be_secured; } } return std::nullopt; diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index a4b8ce6c25678..6236c6165f200 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -288,17 +288,17 @@ std::vector PlannerInterface::generateStopTrajectory( "stop planning"); // Get Closest Stop Obstacle - // const auto closest_stop_obstacle = - // obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj_points, stop_obstacles); - - std::optional closest_stop_obstacle = std::nullopt; - double dist_to_closest_stop_obstacle = std::numeric_limits::max(); - for (const auto & stop_obstacle : stop_obstacles) { - if (stop_obstacle.traj_dist_to_stop_point < dist_to_closest_stop_obstacle) { - dist_to_closest_stop_obstacle = stop_obstacle.traj_dist_to_stop_point; - closest_stop_obstacle = stop_obstacle; + const auto closest_stop_obstacle = [&]() { + std::optional candidate_obstacle = std::nullopt; + for (const auto & stop_obstacle : stop_obstacles) { + if ( + !candidate_obstacle || + stop_obstacle.traj_dist_to_stop_point < candidate_obstacle->traj_dist_to_stop_point) { + candidate_obstacle = stop_obstacle; + } } - } + return candidate_obstacle; + }(); if (!closest_stop_obstacle) { // delete marker @@ -310,117 +310,86 @@ std::vector PlannerInterface::generateStopTrajectory( return planner_data.traj_points; } - // Get Closest Obstacle Stop Distance - // const double closest_obstacle_dist = motion_utils::calcSignedArcLength( - // planner_data.traj_points, 0, closest_stop_obstacle->collision_point); - const auto ego_segment_idx = ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); - const auto negative_dist_to_ego = motion_utils::calcSignedArcLength( - planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0); - const double dist_to_ego = -negative_dist_to_ego; - - debug(dist_to_ego); - - const double zero_vel_dist = dist_to_closest_stop_obstacle + dist_to_ego; - - const double margin_from_obstacle = - calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); - - // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin - // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - // const double margin_from_obstacle_considering_behavior_module = [&]() { - // const size_t nearest_segment_idx = - // findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose); - // const auto closest_behavior_stop_idx = - // motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1); - - // if (!closest_behavior_stop_idx) { - // return margin_from_obstacle; - // } - - // const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength( - // planner_data.traj_points, planner_data.ego_pose.position, nearest_segment_idx, - // *closest_behavior_stop_idx); - - // if (*closest_behavior_stop_idx == planner_data.traj_points.size() - 1) { - // // Closest behavior stop point is the end point - // const double closest_obstacle_stop_dist_from_ego = - // closest_obstacle_dist - dist_to_ego - longitudinal_info_.terminal_safe_distance_margin - - // abs_ego_offset; - // const double stop_dist_diff = - // closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - // if (stop_dist_diff < margin_from_obstacle) { - // // Use terminal margin (terminal_safe_distance_margin) for obstacle stop - // return longitudinal_info_.terminal_safe_distance_margin; - // } - // } else { - // const double closest_obstacle_stop_dist_from_ego = - // closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset; - // const double stop_dist_diff = - // closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - // if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { - // // Use shorter margin (min_behavior_stop_margin) for obstacle stop - // return min_behavior_stop_margin_; - // } - // } - // return margin_from_obstacle; - // }(); - - // const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() { - // // Check feasibility to stop - // if (suppress_sudden_obstacle_stop_) { - // const double closest_obstacle_stop_dist = - // closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset; - - // // Calculate feasible stop margin (Check the feasibility) - // const double feasible_stop_dist = calcMinimumDistanceToStop( - // planner_data.ego_vel, longitudinal_info_.limit_max_accel, - // longitudinal_info_.limit_min_accel) + - // dist_to_ego; - - // if (closest_obstacle_stop_dist < feasible_stop_dist) { - // const auto feasible_margin_from_obstacle = - // margin_from_obstacle_considering_behavior_module - - // (feasible_stop_dist - closest_obstacle_stop_dist); - // return std::make_pair(feasible_margin_from_obstacle, true); - // } - // } - // return std::make_pair(margin_from_obstacle_considering_behavior_module, false); - // }(); - - // const double zero_vel_dist = [&]() { - // const double current_zero_vel_dist = - // std::max(0.0, closest_obstacle_dist - abs_ego_offset - stop_margin_from_obstacle); - - // // Hold previous stop distance if necessary - // if ( - // std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && - // prev_stop_distance_info_) { - // // NOTE: We assume that the current trajectory's front point is ahead of the previous - // // trajectory's front point. - // const size_t traj_front_point_prev_seg_idx = - // motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - // prev_stop_distance_info_->first, planner_data.traj_points.front().pose); - // const double diff_dist_front_points = motion_utils::calcSignedArcLength( - // prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, - // traj_front_point_prev_seg_idx); - - // const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; - // if ( - // std::abs(prev_zero_vel_dist - current_zero_vel_dist) < - // longitudinal_info_.hold_stop_distance_threshold) { - // return prev_zero_vel_dist; - // } - // } - - // return current_zero_vel_dist; - // }(); + double candidate_stop_dist_on_ref_traj = + motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + + closest_stop_obstacle->traj_dist_to_stop_point; + debug(candidate_stop_dist_on_ref_traj); + + const auto closest_behavior_stop_idx = + motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); + + if (closest_behavior_stop_idx) { + debug(*closest_behavior_stop_idx); + const double closest_behavior_stop_dist_from_ego = + motion_utils::calcSignedArcLength(planner_data.traj_points, 0, *closest_behavior_stop_idx); + const double stop_dist_diff = + closest_behavior_stop_dist_from_ego - candidate_stop_dist_on_ref_traj; + + if (*closest_behavior_stop_idx == planner_data.traj_points.size() - 1) { + std::cerr << "check goal point" << std::endl; + // Closest behavior stop point is the end point + if ( + stop_dist_diff < longitudinal_info_.safe_distance_margin && + stop_dist_diff > longitudinal_info_.terminal_safe_distance_margin) { + std::cerr << "modifiy stop dist" << std::endl; + // Use terminal margin (terminal_safe_distance_margin) for obstacle stop + candidate_stop_dist_on_ref_traj += longitudinal_info_.safe_distance_margin - + longitudinal_info_.terminal_safe_distance_margin; + } + } else { + if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) { + // Use shorter margin (min_behavior_stop_margin) for obstacle stop + candidate_stop_dist_on_ref_traj += + longitudinal_info_.safe_distance_margin - min_behavior_stop_margin_; + } + } + } + + // Check feasibility to stop + bool will_collide_with_obstacle = false; + if (suppress_sudden_obstacle_stop_) { + // Calculate feasible stop margin (Check the feasibility) + const double feasible_stop_dist = + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, + longitudinal_info_.limit_min_accel) + + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position); + + if (candidate_stop_dist_on_ref_traj < feasible_stop_dist) { + candidate_stop_dist_on_ref_traj = feasible_stop_dist; + will_collide_with_obstacle = true; + } + } + + // Hold previous stop distance if necessary + if ( + std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, planner_data.traj_points.front().pose); + const double diff_dist_front_points = motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - candidate_stop_dist_on_ref_traj) < + longitudinal_info_.hold_stop_distance_threshold) { + candidate_stop_dist_on_ref_traj = prev_zero_vel_dist; + } + } // Insert stop point auto output_traj_points = planner_data.traj_points; // const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); - const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); + const auto zero_vel_idx = + motion_utils::insertStopPoint(0, candidate_stop_dist_on_ref_traj, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( @@ -437,16 +406,18 @@ std::vector PlannerInterface::generateStopTrajectory( velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); // Publish if ego vehicle collides with the obstacle with a limit acceleration - // const auto stop_speed_exceeded_msg = - // createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); - // stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); + const auto stop_speed_exceeded_msg = + createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); + stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); - prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist); + prev_stop_distance_info_ = std::make_pair(output_traj_points, candidate_stop_dist_on_ref_traj); } - // stop_planning_debug_info_.set( - // StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, - // closest_obstacle_dist - abs_ego_offset); // TODO(murooka) + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, + candidate_stop_dist_on_ref_traj - + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position)); // TODO(murooka) stop_planning_debug_info_.set( StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); @@ -463,90 +434,90 @@ std::vector PlannerInterface::generateStopTrajectory( return output_traj_points; } -double PlannerInterface::calculateMarginFromObstacleOnCurve( - const PlannerData & planner_data, const StopObstacle & stop_obstacle) const -{ - if (!enable_approaching_on_curve_) { - return longitudinal_info_.safe_distance_margin; - } - - const double abs_ego_offset = planner_data.is_driving_forward - ? std::abs(vehicle_info_.max_longitudinal_offset_m) - : std::abs(vehicle_info_.min_longitudinal_offset_m); - - // calculate short trajectory points towards obstacle - const size_t obj_segment_idx = - motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point); - std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; - double sum_short_traj_length{0.0}; - for (int i = obj_segment_idx; 0 <= i; --i) { - short_traj_points.push_back(planner_data.traj_points.at(i)); - - if ( - 1 < short_traj_points.size() && - longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { - break; - } - sum_short_traj_length += tier4_autoware_utils::calcDistance2d( - planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); - } - std::reverse(short_traj_points.begin(), short_traj_points.end()); - if (short_traj_points.size() < 2) { - return longitudinal_info_.safe_distance_margin; - } - - // calculate collision index between straight line from ego pose and object - const auto calculate_distance_from_straight_ego_path = - [&](const auto & ego_pose, const auto & object_polygon) { - const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose( - ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); - const auto ego_straight_segment = tier4_autoware_utils::Segment2d{ - convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; - return boost::geometry::distance(ego_straight_segment, object_polygon); - }; - const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); - const auto object_polygon = - tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); - const auto collision_idx = [&]() -> std::optional { - for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { - const double dist_to_obj = calculate_distance_from_straight_ego_path( - resampled_short_traj_points.at(i).pose, object_polygon); - if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) { - return i; - } - } - return std::nullopt; - }(); - if (!collision_idx) { - return min_safe_distance_margin_on_curve_; - } - if (*collision_idx == 0) { - return longitudinal_info_.safe_distance_margin; - } - - // calculate margin from obstacle - const double partial_segment_length = [&]() { - const double collision_segment_length = tier4_autoware_utils::calcDistance2d( - resampled_short_traj_points.at(*collision_idx - 1), - resampled_short_traj_points.at(*collision_idx)); - const double prev_dist = calculate_distance_from_straight_ego_path( - resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); - const double next_dist = calculate_distance_from_straight_ego_path( - resampled_short_traj_points.at(*collision_idx).pose, object_polygon); - return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) * - collision_segment_length; - }(); - - const double short_margin_from_obstacle = - partial_segment_length + - motion_utils::calcSignedArcLength( - resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - - abs_ego_offset + additional_safe_distance_margin_on_curve_; - - return std::min( - longitudinal_info_.safe_distance_margin, - std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle)); -} +// double PlannerInterface::calculateMarginFromObstacleOnCurve( +// const PlannerData & planner_data, const StopObstacle & stop_obstacle) const +// { +// if (!enable_approaching_on_curve_) { +// return longitudinal_info_.safe_distance_margin; +// } + +// const double abs_ego_offset = planner_data.is_driving_forward +// ? std::abs(vehicle_info_.max_longitudinal_offset_m) +// : std::abs(vehicle_info_.min_longitudinal_offset_m); + +// // calculate short trajectory points towards obstacle +// const size_t obj_segment_idx = +// motion_utils::findNearestSegmentIndex(planner_data.traj_points, +// stop_obstacle.collision_point); +// std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + +// 1)}; double sum_short_traj_length{0.0}; for (int i = obj_segment_idx; 0 <= i; --i) { +// short_traj_points.push_back(planner_data.traj_points.at(i)); + +// if ( +// 1 < short_traj_points.size() && +// longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { +// break; +// } +// sum_short_traj_length += tier4_autoware_utils::calcDistance2d( +// planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); +// } +// std::reverse(short_traj_points.begin(), short_traj_points.end()); +// if (short_traj_points.size() < 2) { +// return longitudinal_info_.safe_distance_margin; +// } + +// // calculate collision index between straight line from ego pose and object +// const auto calculate_distance_from_straight_ego_path = +// [&](const auto & ego_pose, const auto & object_polygon) { +// const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose( +// ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); +// const auto ego_straight_segment = tier4_autoware_utils::Segment2d{ +// convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; +// return boost::geometry::distance(ego_straight_segment, object_polygon); +// }; +// const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); +// const auto object_polygon = +// tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); +// const auto collision_idx = [&]() -> std::optional { +// for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { +// const double dist_to_obj = calculate_distance_from_straight_ego_path( +// resampled_short_traj_points.at(i).pose, object_polygon); +// if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) { +// return i; +// } +// } +// return std::nullopt; +// }(); +// if (!collision_idx) { +// return min_safe_distance_margin_on_curve_; +// } +// if (*collision_idx == 0) { +// return longitudinal_info_.safe_distance_margin; +// } + +// // calculate margin from obstacle +// const double partial_segment_length = [&]() { +// const double collision_segment_length = tier4_autoware_utils::calcDistance2d( +// resampled_short_traj_points.at(*collision_idx - 1), +// resampled_short_traj_points.at(*collision_idx)); +// const double prev_dist = calculate_distance_from_straight_ego_path( +// resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); +// const double next_dist = calculate_distance_from_straight_ego_path( +// resampled_short_traj_points.at(*collision_idx).pose, object_polygon); +// return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) * +// collision_segment_length; +// }(); + +// const double short_margin_from_obstacle = +// partial_segment_length + +// motion_utils::calcSignedArcLength( +// resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - +// abs_ego_offset + additional_safe_distance_margin_on_curve_; + +// return std::min( +// longitudinal_info_.safe_distance_margin, +// std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle)); +// } double PlannerInterface::calcDistanceToCollisionPoint( const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point)