diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 7aa48b22cfeb7..54c905a6c0abf 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -130,37 +130,33 @@ Trajectory PlannerInterface::generateStopTrajectory( will_collide_with_obstacle = true; } - const size_t collision_idx = motion_utils::findNearestIndex( - planner_data.traj.points, closest_stop_obstacle->collision_point); - const size_t zero_vel_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( - planner_data.traj.points, -abs_ego_offset - feasible_margin_from_obstacle, collision_idx); - const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( - planner_data.traj.points, -feasible_margin_from_obstacle, collision_idx); - - // TODO(shimizu) insert stop point with interpolation // Generate Output Trajectory auto output_traj = planner_data.traj; - for (size_t o_idx = zero_vel_idx; o_idx < output_traj.points.size(); ++o_idx) { - output_traj.points.at(o_idx).longitudinal_velocity_mps = 0.0; + const double zero_vel_dist = + std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle); + const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj.points); + if (zero_vel_idx) { + const auto wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + output_traj.points, abs_ego_offset, *zero_vel_idx); + + // virtual wall marker for stop obstacle + const auto marker_pose = output_traj.points.at(wall_idx).pose; + const auto markers = motion_utils::createStopVirtualWallMarker( + marker_pose, "obstacle stop", planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker); + debug_data.obstacles_to_stop.push_back(*closest_stop_obstacle); + + // Publish Stop Reason + const auto stop_pose = output_traj.points.at(*zero_vel_idx).pose; + const auto stop_reasons_msg = + makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); + stop_reasons_pub_->publish(stop_reasons_msg); + + // 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); } - // virtual wall marker for stop obstacle - const auto marker_pose = planner_data.traj.points.at(wall_idx).pose; - const auto markers = motion_utils::createStopVirtualWallMarker( - marker_pose, "obstacle stop", planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker); - debug_data.obstacles_to_stop.push_back(*closest_stop_obstacle); - - // Publish Stop Reason - const auto stop_pose = output_traj.points.at(zero_vel_idx).pose; - const auto stop_reasons_msg = - makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); - stop_reasons_pub_->publish(stop_reasons_msg); - - // 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); - return output_traj; } diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index d048b265aa382..176a9f8455832 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -152,69 +152,6 @@ geometry_msgs::msg::Pose getCurrentObjectPose( return interpolated_pose.get(); } -autoware_auto_planning_msgs::msg::Trajectory insertStopPoint( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, - const double distance_to_stop_point) -{ - if (trajectory.points.size() < 2) { - return trajectory; - } - - const double traj_length = motion_utils::calcArcLength(trajectory.points); - if (traj_length < distance_to_stop_point) { - return trajectory; - } - - autoware_auto_planning_msgs::msg::Trajectory output; - output.header = trajectory.header; - - double accumulated_length = 0; - size_t insert_idx = trajectory.points.size(); - for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { - const auto curr_traj_point = trajectory.points.at(i); - const auto next_traj_point = trajectory.points.at(i + 1); - const auto curr_pose = curr_traj_point.pose; - const auto next_pose = next_traj_point.pose; - const double segment_length = tier4_autoware_utils::calcDistance2d(curr_pose, next_pose); - accumulated_length += segment_length; - - if (accumulated_length > distance_to_stop_point) { - const double ratio = 1 - (accumulated_length - distance_to_stop_point) / segment_length; - - autoware_auto_planning_msgs::msg::TrajectoryPoint stop_point; - stop_point.pose = tier4_autoware_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); - stop_point.lateral_velocity_mps = 0.0; - const double front_dist = tier4_autoware_utils::calcDistance2d(curr_pose, stop_point.pose); - const double back_dist = tier4_autoware_utils::calcDistance2d(stop_point.pose, next_pose); - if (front_dist < 1e-3) { - auto traj_point = trajectory.points.at(i); - traj_point.longitudinal_velocity_mps = 0.0; - output.points.push_back(traj_point); - } else if (back_dist < 1e-3) { - output.points.push_back(curr_traj_point); - } else { - output.points.push_back(curr_traj_point); - output.points.push_back(stop_point); - } - insert_idx = i + 1; - break; - } - - output.points.push_back(curr_traj_point); - } - - for (size_t i = insert_idx; i < trajectory.points.size() - 1; ++i) { - auto traj_point = trajectory.points.at(i); - traj_point.longitudinal_velocity_mps = 0.0; - output.points.push_back(traj_point); - } - - // Terminal Velocity Should be zero - output.points.back().longitudinal_velocity_mps = 0.0; - - return output; -} - boost::optional getClosestStopObstacle( const autoware_auto_planning_msgs::msg::Trajectory & traj, const std::vector & target_obstacles)