diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 126e66772920a..0ce42fbe1080f 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f); + } // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index baf1c534a8a00..1f29a4577e428 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -21,6 +21,8 @@ #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" +#include + namespace autoware::motion_utils { std::vector resamplePointVector( @@ -601,11 +603,13 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( rear_wheel_angle.push_back(input_trajectory.points.front().rear_wheel_angle_rad); time_from_start.push_back( rclcpp::Duration(input_trajectory.points.front().time_from_start).seconds()); + for (size_t i = 1; i < input_trajectory.points.size(); ++i) { const auto & prev_pt = input_trajectory.points.at(i - 1); const auto & curr_pt = input_trajectory.points.at(i); const double ds = autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -617,6 +621,19 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( time_from_start.push_back(rclcpp::Duration(curr_pt.time_from_start).seconds()); } + // Set Zero Velocity After Stop Point + // If the longitudinal velocity is zero, set the velocity to zero after that point. + bool stop_point_found_in_v_lon = false; + constexpr double epsilon = 1e-4; + for (size_t i = 0; i < v_lon.size(); ++i) { + if (std::abs(v_lon.at(i)) < epsilon) { + stop_point_found_in_v_lon = true; + } + if (stop_point_found_in_v_lon) { + v_lon.at(i) = 0.0; + } + } + // Interpolate const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index ce26952c3d634..080d8ca8c7437 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -599,4 +599,27 @@ template bool isTargetPointFront & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity) +{ + const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point); + if (nearest_segment_idx + 1 == trajectory.size()) { + return; + } + for (auto & p : trajectory) { + p.time_from_start = rclcpp::Duration::from_seconds(0); + } + // TODO(Maxime): some points can have very low velocities which introduce huge time errors + // Temporary solution: use a minimum velocity + for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) { + const auto & from = trajectory[idx - 1]; + const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps); + if (velocity != 0.0) { + auto & to = trajectory[idx]; + const auto t = universe_utils::calcDistance2d(from, to) / velocity; + to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start; + } + } +} } // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp index 62db1b665d07a..68ead4b8c1520 100644 --- a/common/autoware_motion_utils/test/src/resample/test_resample.cpp +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -173,6 +173,24 @@ std::vector generateArclength(const size_t num_points, const double inte return resampled_arclength; } + +template +std::vector setZeroVelocityAfterStop(const std::vector & traj_points) +{ + std::vector resampled_traj_points; + bool stop_point_found = false; + for (auto p : traj_points) { + if (!stop_point_found && p.longitudinal_velocity_mps < std::numeric_limits::epsilon()) { + stop_point_found = true; + } + if (stop_point_found) { + p.longitudinal_velocity_mps = 0.0; + } + resampled_traj_points.push_back(p); + } + return resampled_traj_points; +} + } // namespace TEST(resample_vector_pose, resample_by_same_interval) @@ -2643,7 +2661,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); for (size_t i = 0; i < resampled_traj.points.size(); ++i) { const auto p = resampled_traj.points.at(i); - const auto ans_p = traj.points.at(i); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -2665,7 +2683,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) const auto resampled_path = resampleTrajectory(traj, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { const auto p = resampled_path.points.at(i); - const auto ans_p = traj.points.at(i); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -2680,7 +2698,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) } const auto p = resampled_path.points.back(); - const auto ans_p = traj.points.back(); + const auto ans_p = setZeroVelocityAfterStop(traj.points).back(); const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); @@ -2724,7 +2742,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); @@ -2735,7 +2753,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); @@ -2746,7 +2764,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); @@ -2757,7 +2775,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.35, epsilon); @@ -2768,7 +2786,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); @@ -2920,7 +2938,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); @@ -2931,7 +2949,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); @@ -2942,7 +2960,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); @@ -2984,7 +3002,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 1.2, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); @@ -2995,7 +3013,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 5.3, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); @@ -3006,7 +3024,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 9.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); @@ -3050,7 +3068,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.2, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 0.6, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.06, epsilon); @@ -3061,7 +3079,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.3, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 2.65, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.265, epsilon); @@ -3072,7 +3090,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); @@ -3105,7 +3123,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) const auto resampled_traj = resampleTrajectory(traj, 1.0); for (size_t i = 0; i < resampled_traj.points.size(); ++i) { const auto p = resampled_traj.points.at(i); - const auto ans_p = traj.points.at(i); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -3127,7 +3145,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) const auto resampled_path = resampleTrajectory(traj, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { const auto p = resampled_path.points.at(i); - const auto ans_p = traj.points.at(i); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -3142,7 +3160,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) } const auto p = resampled_path.points.back(); - const auto ans_p = traj.points.back(); + const auto ans_p = setZeroVelocityAfterStop(traj.points).back(); const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); @@ -3180,11 +3198,11 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); const size_t idx = i / 10; - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(idx).lateral_velocity_mps, epsilon); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(idx).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } } @@ -3210,11 +3228,11 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); const size_t idx = i / 2.5; - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(idx).lateral_velocity_mps, epsilon); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.04 * i, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(idx).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } { @@ -3228,11 +3246,11 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); const size_t idx = 9; - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(idx).lateral_velocity_mps, epsilon); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(idx).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } } @@ -3259,11 +3277,11 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); const size_t idx = i == 0 ? 0 : i - 1; - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(idx).lateral_velocity_mps, epsilon); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, ds / 10.0 * i, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(idx).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } { @@ -3277,11 +3295,11 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); const size_t idx = 9; - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(idx).lateral_velocity_mps, epsilon); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(idx).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } } @@ -3309,11 +3327,11 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); const size_t idx = i / 10; - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(idx).lateral_velocity_mps, epsilon); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(idx).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } } @@ -3416,6 +3434,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) { const auto p = resampled_traj.points.at(5); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(6); EXPECT_NEAR(p.pose.position.x, 6.0, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); @@ -3424,15 +3443,15 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(6).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(6).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.60, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(6).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } { const auto p = resampled_traj.points.at(6); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(7); EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); @@ -3441,15 +3460,15 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(7).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(7).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(7).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } { const auto p = resampled_traj.points.at(7); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(9); EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); @@ -3458,11 +3477,10 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(9).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(9).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); EXPECT_NEAR(p.heading_rate_rps, 0.90, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(9).acceleration_mps2, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); } } @@ -3568,3 +3586,36 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) } } } + +TEST(resample_trajectory, resample_with_middle_stop_point) +{ + // This test is to check the behavior when the stop point is unstably resampled by zero-order hold + // interpolation. + + using autoware::motion_utils::resampleTrajectory; + + autoware_planning_msgs::msg::Trajectory traj; + traj.points.reserve(10); + + traj.points.push_back(generateTestTrajectoryPoint(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(1.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(2.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(3.1, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(6.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(7.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + + std::vector interpolated_axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; + + const auto resampled_traj = resampleTrajectory(traj, interpolated_axis); + + EXPECT_NEAR(resampled_traj.points.at(0).longitudinal_velocity_mps, 10.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(1).longitudinal_velocity_mps, 10.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(2).longitudinal_velocity_mps, 10.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(3).longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(4).longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(5).longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(6).longitudinal_velocity_mps, 0.0, epsilon); +} diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp index 69c52b9c2eafb..2097c0576c545 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -17,6 +17,7 @@ #include "utility_functions.hpp" #include +#include #include #include #include @@ -27,10 +28,13 @@ #include #include +#include +#include + +#include #include +#include #include -#include -#include #include #include @@ -216,7 +220,7 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( - autoware::universe_utils::LinearRing2d goal_footprint) const + autoware::universe_utils::LinearRing2d goal_footprint) { visualization_msgs::msg::MarkerArray msg; auto marker = autoware::universe_utils::createDefaultMarker( @@ -244,52 +248,58 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( return msg; } -bool DefaultPlanner::check_goal_footprint_inside_lanes( - const lanelet::ConstLanelet & current_lanelet, - const lanelet::ConstLanelet & combined_prev_lanelet, - const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length, - const double search_margin) +lanelet::ConstLanelets next_lanelets_up_to( + const lanelet::ConstLanelet & start_lanelet, const double up_to_distance, + const route_handler::RouteHandler & route_handler) { - // check if goal footprint is in current lane - if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) { - return true; + lanelet::ConstLanelets lanelets; + if (up_to_distance <= 0.0) { + return lanelets; } - const auto following = route_handler_.getNextLanelets(current_lanelet); - // check if goal footprint is in between many lanelets in depth-first search manner - for (const auto & next_lane : following) { - next_lane_length += lanelet::utils::getLaneletLength2d(next_lane); - lanelet::ConstLanelets lanelets; - lanelets.push_back(combined_prev_lanelet); + for (const auto & next_lane : route_handler.getNextLanelets(start_lanelet)) { lanelets.push_back(next_lane); - lanelet::ConstLanelet combined_lanelets = - combine_lanelets_with_shoulder(lanelets, route_handler_); - - // if next lanelet length is longer than vehicle longitudinal offset - if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { - next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); - // and if the goal_footprint is within the (accumulated) combined_lanelets, terminate the - // query - if (boost::geometry::within(goal_footprint, combined_lanelets.polygon2d().basicPolygon())) { - return true; - } - // if not, iteration continues to next next_lane, and this subtree is terminated - } else { // if next lanelet length is shorter than vehicle longitudinal offset, check the - // overlap with the polygon including the next_lane(s) until the additional lanes get - // longer than ego vehicle length - if (!check_goal_footprint_inside_lanes( - next_lane, combined_lanelets, goal_footprint, next_lane_length)) { - next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); - continue; - } else { - return true; - } + const auto next_lanelets = next_lanelets_up_to( + next_lane, up_to_distance - lanelet::geometry::length2d(next_lane), route_handler); + lanelets.insert(lanelets.end(), next_lanelets.begin(), next_lanelets.end()); + } + return lanelets; +} + +bool DefaultPlanner::check_goal_footprint_inside_lanes( + const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets, + const universe_utils::Polygon2d & goal_footprint) const +{ + universe_utils::MultiPolygon2d ego_lanes; + universe_utils::Polygon2d poly; + for (const auto & ll : path_lanelets) { + const auto left_shoulder = route_handler_.getLeftShoulderLanelet(ll); + if (left_shoulder) { + boost::geometry::convert(left_shoulder->polygon2d().basicPolygon(), poly); + ego_lanes.push_back(poly); + } + const auto right_shoulder = route_handler_.getRightShoulderLanelet(ll); + if (right_shoulder) { + boost::geometry::convert(right_shoulder->polygon2d().basicPolygon(), poly); + ego_lanes.push_back(poly); } + boost::geometry::convert(ll.polygon2d().basicPolygon(), poly); + ego_lanes.push_back(poly); } - return false; + const auto next_lanelets = + next_lanelets_up_to(current_lanelet, vehicle_info_.max_longitudinal_offset_m, route_handler_); + for (const auto & ll : next_lanelets) { + boost::geometry::convert(ll.polygon2d().basicPolygon(), poly); + ego_lanes.push_back(poly); + } + + // check if goal footprint is in the ego lane + universe_utils::MultiPolygon2d difference; + boost::geometry::difference(goal_footprint, ego_lanes, difference); + return boost::geometry::is_empty(difference); } bool DefaultPlanner::is_goal_valid( - const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets) { const auto logger = node_->get_logger(); @@ -337,16 +347,10 @@ bool DefaultPlanner::is_goal_valid( pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); - double next_lane_length = 0.0; - // combine calculated route lanelets - const lanelet::ConstLanelet combined_prev_lanelet = - combine_lanelets_with_shoulder(path_lanelets, route_handler_); - // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( param_.check_footprint_inside_lanes && - !check_goal_footprint_inside_lanes( - closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && + !check_goal_footprint_inside_lanes(closest_lanelet, path_lanelets, polygon_footprint) && !is_in_parking_lot( lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()), lanelet::utils::conversion::toLaneletPoint(goal.position))) { @@ -375,11 +379,7 @@ bool DefaultPlanner::is_goal_valid( // check if goal is in parking lot const auto parking_lots = lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()); - if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) { - return true; - } - - return false; + return is_in_parking_lot(parking_lots, goal_lanelet_pt); } PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) @@ -429,7 +429,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) return route_msg; } - if (route_handler_.isRouteLooped(route_sections)) { + if (route_handler::RouteHandler::isRouteLooped(route_sections)) { RCLCPP_WARN(logger, "Loop detected within route!"); return route_msg; } diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp index 60c2a53b82123..39b7fa8909a6a 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -27,7 +27,6 @@ #include #include -#include #include namespace autoware::mission_planner::lanelet2 @@ -44,15 +43,17 @@ struct DefaultPlannerParameters class DefaultPlanner : public mission_planner::PlannerPlugin { public: + DefaultPlanner() : vehicle_info_(), is_graph_ready_(false), param_(), node_(nullptr) {} + void initialize(rclcpp::Node * node) override; void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override; - bool ready() const override; + [[nodiscard]] bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; void updateRoute(const PlannerPlugin::LaneletRoute & route) override; void clearRoute() override; - MarkerArray visualize(const LaneletRoute & route) const override; - MarkerArray visualize_debug_footprint( - autoware::universe_utils::LinearRing2d goal_footprint_) const; + [[nodiscard]] MarkerArray visualize(const LaneletRoute & route) const override; + [[nodiscard]] static MarkerArray visualize_debug_footprint( + autoware::universe_utils::LinearRing2d goal_footprint); autoware::vehicle_info_utils::VehicleInfo vehicle_info_; private: @@ -83,17 +84,16 @@ class DefaultPlanner : public mission_planner::PlannerPlugin * @param next_lane_length the accumulated total length from the start lanelet of the search to * the lanelet of current goal query */ - bool check_goal_footprint_inside_lanes( - const lanelet::ConstLanelet & current_lanelet, - const lanelet::ConstLanelet & combined_prev_lanelet, - const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length, - const double search_margin = 2.0); + [[nodiscard]] bool check_goal_footprint_inside_lanes( + const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets, + const universe_utils::Polygon2d & goal_footprint) const; /** * @brief return true if (1)the goal is in parking area or (2)the goal is on the lanes and the * footprint around the goal does not overlap the lanes */ - bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets); + bool is_goal_valid( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets); /** * @brief project the specified goal pose onto the goal lanelet(the last preferred lanelet of diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 5046b81c72b96..34b16ef35f603 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -18,9 +18,6 @@ #include -#include -#include - autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( autoware::universe_utils::LinearRing2d footprint) { @@ -41,63 +38,6 @@ void insert_marker_array( a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, - const autoware::route_handler::RouteHandler & route_handler) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - lanelet::Points3d centers; - std::vector left_bound_ids; - std::vector right_bound_ids; - - for (const auto & llt : lanelets) { - if (llt.id() != lanelet::InvalId) { - left_bound_ids.push_back(llt.leftBound().id()); - right_bound_ids.push_back(llt.rightBound().id()); - } - } - - // lambda to add bound to target_bound - const auto add_bound = [](const auto & bound, auto & target_bound) { - std::transform( - bound.begin(), bound.end(), std::back_inserter(target_bound), - [](const auto & pt) { return lanelet::Point3d(pt); }); - }; - for (const auto & llt : lanelets) { - // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist - const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt); - if (left_shared_shoulder) { - // if exist, add left bound of SHOULDER lanelet to lefts - add_bound(left_shared_shoulder->leftBound(), lefts); - } else if ( - // if not exist, add left bound of lanelet to lefts - // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`, - // then its left bound constitutes the left boundary of the entire merged lanelet - std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { - add_bound(llt.leftBound(), lefts); - } - - // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist - const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt); - if (right_shared_shoulder) { - // if exist, add right bound of SHOULDER lanelet to rights - add_bound(right_shared_shoulder->rightBound(), rights); - } else if ( - // if not exist, add right bound of lanelet to rights - // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`, - // then its right bound constitutes the right boundary of the entire merged lanelet - std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { - add_bound(llt.rightBound(), rights); - } - } - - const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); - const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); - auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); - return std::move(combined_lanelet); -} - std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet) { std::vector centerline_points; diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp index c8812e2c76dd6..6e3d2a0e88941 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -27,8 +27,6 @@ #include #include -#include -#include #include template @@ -47,16 +45,6 @@ autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); -/** - * @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost - * bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively - * @param lanelets topologically sorted sequence of lanelets - * @param route_handler route handler to query the lanelet map - */ -lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, - const autoware::route_handler::RouteHandler & route_handler); - std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 4c9f139365a06..fffb86767b0a8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -31,13 +31,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, + steering_factor_interface_ptr, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index c7fbba34b1adb..4d200411904b0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -40,7 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 8096d2944ee2b..ddcfda50d18c4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -192,7 +192,7 @@ AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index 9c85a463067de..a7f470002b01b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -42,7 +42,7 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 8f19613b50e6d..48c6084a4f424 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -343,7 +343,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index f195b91a54f21..debd13acd31dd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -331,8 +331,9 @@ DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{std::move(parameters)}, target_objects_manager_{TargetObjectsManager( parameters_->successive_num_to_entry_dynamic_avoidance_condition, diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index c04914ed1c72b..3c730e6d36376 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -26,7 +26,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_, std::make_unique(parameters_, direction_)); } @@ -35,7 +35,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_, std::make_unique(parameters_, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 749ed57f95cc9..4c23292f63e61 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -307,7 +307,8 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); ~GoalPlannerModule() { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp index 32d816d9a37e8..1ab7b6cb265a8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp @@ -38,7 +38,7 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 3078414c63213..226b3d17e07a9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -57,8 +57,9 @@ GoalPlannerModule::GoalPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, thread_safe_data_{mutex_, clock_}, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index bd309dd35a260..fd9375c7e9f75 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -52,6 +52,7 @@ class LaneChangeInterface : public SceneModuleInterface const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr, std::unique_ptr && module_type); LaneChangeInterface(const LaneChangeInterface &) = delete; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 3dc5e7ee62a57..ae0a094e038a4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -58,6 +58,44 @@ double calc_dist_from_pose_to_terminal_end( * @return The required backward buffer distance in meters. */ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); + +/** + * @brief Calculates the maximum preparation longitudinal distance for lane change. + * + * This function computes the maximum distance that the ego vehicle can travel during + * the preparation phase of a lane change. The distance is calculated as the product + * of the maximum lane change preparation duration and the maximum velocity of the ego vehicle. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which should include: + * - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change + * preparation. + * - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle. + * + * @return The maximum preparation longitudinal distance in meters. + */ +double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr); + +/** + * @brief Calculates the distance from the ego vehicle to the start of the target lanes. + * + * This function computes the shortest distance from the current position of the ego vehicle + * to the start of the target lanes by measuring the arc length to the front points of + * the left and right boundaries of the target lane. If the target lanes are empty or other + * required data is unavailable, the function returns numeric_limits::max() preventing lane + * change being executed. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which should include: + * - `route_handler_ptr`: Pointer to the route handler that manages the route. + * - Other necessary parameters for ego vehicle positioning. + * @param current_lanes The set of lanelets representing the current lanes of the ego vehicle. + * @param target_lanes The set of lanelets representing the target lanes for lane changing. + * + * @return The distance from the ego vehicle to the start of the target lanes in meters, + * or numeric_limits::max() if the target lanes are empty or data is unavailable. + */ +double calc_ego_dist_to_lanes_start( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 6451d6abadfa2..6db83e9011632 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -38,14 +38,14 @@ LaneChangeInterface::LaneChangeInterface( const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr, std::unique_ptr && module_type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{std::move(parameters)}, module_type_{std::move(module_type)}, prev_approved_path_{std::make_unique()} { module_type_->setTimeKeeper(getTimeKeeper()); - steering_factor_interface_ptr_ = std::make_unique(&node, name); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } @@ -257,11 +257,17 @@ bool LaneChangeInterface::canTransitFailureState() if (!module_type_->isValidPath()) { log_debug_throttled("Transit to failure state due not to find valid path"); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { log_debug_throttled("Abort process has completed."); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 42166c4dff0e0..c197d0d63aa37 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -299,7 +299,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 42b22f9b1631c..28efae2549d1e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -153,6 +153,8 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool NormalLaneChange::isLaneChangeRequired() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto current_lanes = utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); @@ -162,7 +164,15 @@ bool NormalLaneChange::isLaneChangeRequired() const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - return !target_lanes.empty(); + if (target_lanes.empty()) { + return false; + } + + const auto ego_dist_to_target_start = + calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); + const auto maximum_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + + return ego_dist_to_target_start <= maximum_prepare_length; } bool NormalLaneChange::isStoppedAtRedTrafficLight() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index ac205ceedb34b..2dc65a8b78045 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -54,4 +54,44 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; return std::max(min_back_dist, param_back_dist); } + +double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) +{ + const auto max_prepare_duration = common_data_ptr->lc_param_ptr->lane_change_prepare_duration; + const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel; + + return max_prepare_duration * ego_max_speed; +} + +double calc_ego_dist_to_lanes_start( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + + if (!route_handler_ptr || target_lanes.empty() || current_lanes.empty()) { + return std::numeric_limits::max(); + } + + const auto & target_bound = + common_data_ptr->direction == autoware::route_handler::Direction::RIGHT + ? target_lanes.front().leftBound() + : target_lanes.front().rightBound(); + + if (target_bound.empty()) { + return std::numeric_limits::max(); + } + + const auto path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + + if (path.points.empty()) { + return std::numeric_limits::max(); + } + + const auto target_front_pt = lanelet::utils::conversion::toGeomMsgPt(target_bound.front()); + const auto ego_position = common_data_ptr->get_ego_pose().position; + + return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index c4a05d171654d..54254bf13e07f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -90,15 +90,15 @@ class SceneModuleInterface const std::string & name, rclcpp::Node & node, std::unordered_map> rtc_interface_ptr_map, std::unordered_map> - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), - steering_factor_interface_ptr_( - std::make_unique(&node, utils::convertToSnakeCase(name))), + steering_factor_interface_ptr_{steering_factor_interface_ptr}, time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { @@ -340,7 +340,7 @@ class SceneModuleInterface } if (rtc.second->isTerminated(uuid_map_.at(rtc.first))) { - return true; + return false; } return rtc.second->isActivated(uuid_map_.at(rtc.first)); @@ -640,7 +640,7 @@ class SceneModuleInterface std::unordered_map> objects_of_interest_marker_interface_ptr_map_; - std::unique_ptr steering_factor_interface_ptr_; + std::shared_ptr steering_factor_interface_ptr_; mutable std::optional stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 82a364a9c37c7..ceecb7b02408a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -315,6 +315,12 @@ class SceneModuleManagerInterface "~/processing_time/" + name_, 20); } + // init steering factor + { + steering_factor_interface_ptr_ = + std::make_shared(node, utils::convertToSnakeCase(name_)); + } + // misc { node_ = node; @@ -339,6 +345,8 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; + std::shared_ptr steering_factor_interface_ptr_; + std::vector observers_; std::unique_ptr idle_module_ptr_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp index 28c310ae20ec2..778afd1698ff2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp @@ -38,7 +38,7 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 2b6f52998cb23..8dc04947923e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -86,7 +86,8 @@ class SamplingPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 526a761e6957a..b820e8d03322f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -33,8 +33,9 @@ SamplingPlannerModule::SamplingPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { internal_params_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp index b495e6619a1c7..d1c9c8e2535ec 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp @@ -39,7 +39,7 @@ class SideShiftModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index 543b17aca9352..74953b7927b5d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -45,7 +45,8 @@ class SideShiftModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index bcb972bf5dc8b..cd4298c56929a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -40,8 +40,9 @@ SideShiftModule::SideShiftModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{parameters} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp index 5d3d224673124..a26c48ad065c9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp @@ -38,7 +38,7 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index bb70ae2638056..e0bb5d95f565a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -88,7 +88,8 @@ class StartPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); ~StartPlannerModule() override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index e318fbedf3200..dd9129216fbba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -59,8 +59,9 @@ StartPlannerModule::StartPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index bfeb942c82be3..0f3536eeb4e7c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -305,7 +305,7 @@ class AvoidanceHelper { const auto & p = parameters_; return prepare_distance > - std::max(getEgoSpeed() * p->min_prepare_distance, p->min_prepare_distance); + std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance); } bool isComfortable(const AvoidLineArray & shift_lines) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp index 895390f91cc16..fa54ec52203f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -41,7 +41,7 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index ea12c66859a36..a2dbdcc1dadb9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -46,7 +46,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index a542107e890aa..9840743867e78 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -658,6 +658,7 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, ObjectInfo::DEVIATING_FROM_EGO_LANE); addObjects(data.other_objects, ObjectInfo::UNSTABLE_OBJECT); addObjects(data.other_objects, ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE); + addObjects(data.other_objects, ObjectInfo::INVALID_SHIFT_LINE); } // shift line pre-process diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 9e09a6ba0428e..8b2e0a6bd1077 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -76,8 +76,9 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT helper_{std::make_shared(parameters)}, parameters_{parameters}, generator_{parameters} diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 57a6e340bf853..e54a00b1cd2c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -248,6 +248,11 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; }; + const auto is_approved = [this](const auto & object) { + return (helper_->getShift(object.getPosition()) > 0.0 && isOnRight(object)) || + (helper_->getShift(object.getPosition()) < 0.0 && !isOnRight(object)); + }; + ObjectDataArray unavoidable_objects; // target objects are sorted by longitudinal distance. @@ -284,6 +289,11 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); if (!feasible_shift_profile.has_value()) { + if (is_approved(o)) { + // the avoidance path for this object has already approved + o.is_avoidable = true; + continue; + } if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { @@ -394,7 +404,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( outlines.emplace_back(al_avoid, std::nullopt); } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { outlines.emplace_back(al_avoid, al_return); - } else { + } else if (!is_approved(o)) { o.info = ObjectInfo::INVALID_SHIFT_LINE; continue; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md index 99396eb0edf22..0b6aa697fcbef 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -1,161 +1,138 @@ -## Out of Lane +# Out of Lane -### Role +## Role -`out_of_lane` is the module that decelerates and stops to prevent the ego vehicle from entering another lane with incoming dynamic objects. +There are cases where the ego vehicle footprint goes out of the driving lane, +for example when taking a narrow turn with a large vehicle. +The `out_of_lane` module adds deceleration and stop points to the ego trajectory in order to prevent collisions from occurring in these out of lane cases. -### Activation Timing +## Activation -This module is activated if `launch_out_of_lane` is set to true. +This module is activated if the launch parameter `launch_out_of_lane_module` is set to true. -### Inner-workings / Algorithms +## Inner-workings / Algorithms -The algorithm is made of the following steps. +This module calculates if out of lane collisions occur and insert stop point before the collisions if necessary. -1. Calculate the ego path footprints (red). -2. Calculate the other lanes (magenta). -3. Calculate the overlapping ranges between the ego path footprints and the other lanes (green). -4. For each overlapping range, decide if a stop or slow down action must be taken. -5. For each action, insert the corresponding stop or slow down point in the path. +The algorithm assumes the input ego trajectory contains accurate `time_from_start` +values in order to calculate accurate time to collisions with the predicted objects. -![overview](./docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png) +Next we explain the inner-workings of the module in more details. -#### 1. Ego Path Footprints +### 1. Ego trajectory footprints -In this first step, the ego footprint is projected at each path point and are eventually inflated based on the `extra_..._offset` parameters. +In this first step, the ego footprint is projected at each trajectory point and its size is modified based on the `ego.extra_..._offset` parameters. -#### 2. Other lanes +The length of the trajectory used for generating the footprints is limited by the `max_arc_length` parameter. -In the second step, the set of lanes to consider for overlaps is generated. -This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevant lanelets. -The selection distance is chosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`. +![ego_footprints](./docs/footprints.png) -A lanelet is deemed non-relevant if it meets one of the following conditions. +### 2. Ego lanelets -- It is part of the lanelets followed by the ego path. -- It contains the rear point of the ego footprint. -- It follows one of the ego path lanelets. +In the second step, we calculate the lanelets followed by the ego trajectory. +We select all lanelets crossed by the trajectory linestring (sequence of trajectory points), as well as their preceding lanelets. -#### 3. Overlapping ranges +![ego_lane](./docs/ego_lane.png) -In the third step, overlaps between the ego path footprints and the other lanes are calculated. -For each pair of other lane $l$ and ego path footprint $f$, we calculate the overlapping polygons using `boost::geometry::intersection`. -For each overlapping polygon found, if the distance inside the other lane $l$ is above the `overlap.minimum_distance` threshold, then the overlap is ignored. -Otherwise, the arc length range (relative to the ego path) and corresponding points of the overlapping polygons are stored. -Ultimately, for each other lane $l$, overlapping ranges of successive overlaps are built with the following information: +In the debug visualization the combination of all ego lanelets is shown as a blue polygon. -- overlapped other lane $l$. -- start and end ego path indexes. -- start and end ego path arc lengths. -- start and end overlap points. +### 3. Out of lane areas -#### 4. Decisions +Next, for each trajectory point, we create the corresponding out of lane areas by subtracting the ego lanelets (from step 2) from the trajectory point footprint (from step 1). +Each area is associated with the lanelets overlapped by the area and with the corresponding ego trajectory point. -In the fourth step, a decision to either slow down or stop before each overlapping range is taken based on the dynamic objects. -The conditions for the decision depend on the value of the `mode` parameter. +![out_of_lane_areas](./docs/out_of_lane_areas.png) -Whether it is decided to slow down or stop is determined by the distance between the ego vehicle and the start of the overlapping range (in arc length along the ego path). -If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used. -If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used. +In the debug visualization, the out of lane area polygon is connected to the corresponding trajectory point by a line. + +### 4. Predicted objects filtering + +We filter objects and their predicted paths with the following conditions: - - - - - -
- - - -
+- ignore objects with a speed bellow the `minimum_velocity` parameter; +- ignore objects coming from behind the ego vehicle if parameter `ignore_behind_ego` is set to true; +- ignore predicted paths whose confidence value is bellow the `predicted_path_min_confidence` parameter; +- cut the points of predicted paths going beyond the stop line of a red traffic light if parameter `cut_predicted_paths_beyond_red_lights` is set to `true`. -##### Threshold +| `cut_predicted_paths_beyond_red_lights = false` | `cut_predicted_paths_beyond_red_lights = true` | +| :---------------------------------------------: | :--------------------------------------------: | +| ![](./docs/path_green_light.png) | ![](./docs/path_red_light.png) | -With the `mode` set to `"threshold"`, -a decision to stop or slow down before a range is made if -an incoming dynamic object is estimated to reach the overlap within `threshold.time_threshold`. +In the debug visualization, the filtered predicted paths are shown in green and the stop lines of red traffic lights are shown in red. -##### TTC (time to collision) +### 5. Time to collisions -With the `mode` set to `"ttc"`, -estimates for the times when ego and the dynamic objects reach the start and end of the overlapping range are calculated. -This is then used to calculate the time to collision over the period where ego crosses the overlap. -If the time to collision is predicted to go bellow the `ttc.threshold`, the decision to stop or slow down is made. +For each out of lane area, we calculate the times when a dynamic object will overlap the area based on its filtered predicted paths. -##### Intervals +In the case where parameter `mode` is set to `threshold` and the calculated time is less than `threshold.time_threshold` parameter, then we decide to avoid the out of lane area. -With the `mode` set to `"intervals"`, -the estimated times when ego and the dynamic objects reach the start and end points of -the overlapping range are used to create time intervals. -These intervals can be made shorter or longer using the -`intervals.ego_time_buffer` and `intervals.objects_time_buffer` parameters. -If the time interval of ego overlaps with the time interval of an object, the decision to stop or slow down is made. +In the case where parameter `mode` is set to `ttc`, +we calculate the time to collision by comparing the predicted time of the object with the `time_from_start` field contained in the trajectory point. +If the time to collision is bellow the `ttc.threshold` parameter value, we decide to avoid the out of lane area. -##### Time estimates +![ttcs](./docs/ttcs.png) -###### Ego +In the debug visualization, the ttc (in seconds) is displayed on top of its corresponding trajectory point. +The color of the text is red if the collision should be avoided and green otherwise. -To estimate the times when ego will reach an overlap, it is assumed that ego travels along its path -at its current velocity or at half the velocity of the path points, whichever is higher. +### 6. Calculate the stop or slowdown point -###### Dynamic objects +First, the minimum stopping distance of the ego vehicle is calculated based on the jerk and deceleration constraints set by the velocity smoother parameters. -Two methods are used to estimate the time when a dynamic objects with reach some point. -If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. -Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. +We then search for the furthest pose along the trajectory where the ego footprint stays inside of the ego lane (calculate in step 2) and constraint the search to be between the minimum stopping distance and the 1st trajectory point with a collision to avoid (as determined in the previous step). +The search is done by moving backward along the trajectory with a distance step set by the `action.precision` parameter. -#### 5. Path update +We first do this search for a footprint expanded with the `ego.extra_..._offset`, `action.longitudinal_distance_buffer` and `action.lateral_distance_buffer` parameters. +If no valid pose is found, we search again while only considering the extra offsets but without considering the distance buffers. +If still no valid pose is found, we use the base ego footprint without any offset. +In case no pose is found, we fallback to using the pose before the detected collision without caring if it is out of lane or not. -Finally, for each decision to stop or slow down before an overlapping range, -a point is inserted in the path. -For a decision taken for an overlapping range with a lane $l$ starting at ego path point index $i$, -a point is inserted in the path between index $i$ and $i-1$ such that the ego footprint projected at the inserted point does not overlap $l$. -Such point with no overlap must exist since, by definition of the overlapping range, -we know that there is no overlap at $i-1$. +Whether it is decided to slow down or stop is determined by the distance between the ego vehicle and the trajectory point to avoid. +If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used. +If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used. -If the point would cause a higher deceleration than allowed by the `max_accel` parameter (node parameter), -it is skipped. +![avoid_collision](./docs/ttcs_avoid.png) -Moreover, parameter `action.distance_buffer` adds an extra distance between the ego footprint and the overlap when possible. +### About stability of the stop/slowdown pose -### Module Parameters +As the input trajectory can change significantly between iterations, +it is expected that the decisions of this module will also change. +To make the decision more stable, a stop or slowdown pose is used for a minimum duration set by the `action.min_duration` parameter. +If during that time a new pose closer to the ego vehicle is generated, then it replaces the previous one. +Otherwise, the stop or slowdown pose will only be discarded after no out of lane collision is detection for the set duration. + +## Module Parameters | Parameter | Type | Description | | ----------------------------- | ------ | --------------------------------------------------------------------------------- | | `mode` | string | [-] mode used to consider a dynamic object. Candidates: threshold, intervals, ttc | | `skip_if_already_overlapping` | bool | [-] if true, do not run this module when ego already overlaps another lane | +| `max_arc_length` | double | [m] maximum trajectory arc length that is checked for out_of_lane collisions | | Parameter /threshold | Type | Description | | -------------------- | ------ | ---------------------------------------------------------------- | | `time_threshold` | double | [s] consider objects that will reach an overlap within this time | -| Parameter /intervals | Type | Description | -| --------------------- | ------ | ------------------------------------------------------- | -| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer | -| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer | - | Parameter /ttc | Type | Description | | -------------- | ------ | ------------------------------------------------------------------------------------------------------ | | `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | -| Parameter /objects | Type | Description | -| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | -| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | - -| Parameter /overlap | Type | Description | -| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | -| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | -| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | - -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | +| Parameter /objects | Type | Description | +| --------------------------------------- | ------ | ------------------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | +| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | +| `cut_predicted_paths_beyond_red_lights` | bool | [-] if true, predicted paths are cut beyond the stop line of red traffic lights | +| `ignore_behind_ego` | bool | [-] if true, objects behind the ego vehicle are ignored | + +| Parameter /action | Type | Description | +| ------------------------------ | ------ | --------------------------------------------------------------------- | +| `precision` | double | [m] precision when inserting a stop pose in the trajectory | +| `longitudinal_distance_buffer` | double | [m] safety distance buffer to keep in front of the ego vehicle | +| `lateral_distance_buffer` | double | [m] safety distance buffer to keep on the side of the ego vehicle | +| `min_duration` | double | [s] minimum duration needed before a decision can be canceled | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | | -------------------- | ------ | ---------------------------------------------------- | diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml index a0cf69ee71027..5d36aa8d3f018 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -1,32 +1,22 @@ /**: ros__parameters: out_of_lane: # module to stop or slowdown before overlapping another lane with other objects - mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" + mode: ttc # mode used to consider a conflict with an object. "threshold", or "ttc" skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + max_arc_length: 100.0 # [m] maximum trajectory arc length that is checked for out_of_lane collisions threshold: time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time - intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego - ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer - objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer ttc: threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap objects: minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored - use_predicted_paths: true # if true, use the predicted paths to estimate future positions. - # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. - distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored - overlap: - minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered - extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) - action: # action to insert in the trajectory if an object causes a conflict at an overlap - skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed precision: 0.1 # [m] precision when inserting a stop pose in the trajectory longitudinal_distance_buffer: 1.5 # [m] safety distance buffer to keep in front of the ego vehicle lateral_distance_buffer: 1.0 # [m] safety distance buffer to keep on the side of the ego vehicle @@ -38,8 +28,8 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: - min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego - extra_front_offset: 0.0 # [m] extra front distance - extra_rear_offset: 0.0 # [m] extra rear distance - extra_right_offset: 0.0 # [m] extra right distance - extra_left_offset: 0.0 # [m] extra left distance + # extra footprint offsets to calculate out of lane collisions + extra_front_offset: 0.0 # [m] extra footprint front distance + extra_rear_offset: 0.0 # [m] extra footprint rear distance + extra_right_offset: 0.0 # [m] extra footprint right distance + extra_left_offset: 0.0 # [m] extra footprint left distance diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png new file mode 100644 index 0000000000000..65cb73226465a Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/footprints.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/footprints.png new file mode 100644 index 0000000000000..a04727eb5b718 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/footprints.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/object_paths.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/object_paths.png new file mode 100644 index 0000000000000..a68df681964f1 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/object_paths.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png deleted file mode 100644 index f095467b3b0ac..0000000000000 Binary files a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png and /dev/null differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_areas.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_areas.png new file mode 100644 index 0000000000000..b4d55a7cc4eea Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_areas.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png deleted file mode 100644 index 2f358975b9491..0000000000000 Binary files a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png and /dev/null differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png deleted file mode 100644 index fdb75ac0c6eb8..0000000000000 Binary files a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png and /dev/null differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/path_green_light.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/path_green_light.png new file mode 100644 index 0000000000000..920d913d98c17 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/path_green_light.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/path_red_light.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/path_red_light.png new file mode 100644 index 0000000000000..18b5682626185 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/path_red_light.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ttcs.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ttcs.png new file mode 100644 index 0000000000000..3bc4465d74b0d Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ttcs.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ttcs_avoid.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ttcs_avoid.png new file mode 100644 index 0000000000000..7338c7f575b1e Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ttcs_avoid.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 8b03fa66eab55..b7cd55ce2e642 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -15,74 +15,117 @@ #include "calculate_slowdown_points.hpp" #include "footprint.hpp" +#include "types.hpp" #include #include +#include -#include +#include +#include +#include +#include + +#include #include +#include +#include +#include + namespace autoware::motion_velocity_planner::out_of_lane { -bool can_decelerate( - const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel) +std::optional calculate_last_avoiding_pose( + const std::vector & trajectory, + const universe_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid, + const double min_arc_length, const double max_arc_length, const double precision) { - // TODO(Maxime): use the library function - const auto dist_ahead_of_ego = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, ego_data.pose.position, point.pose.position); - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); + geometry_msgs::msg::Pose interpolated_pose{}; + bool is_avoiding_pose = false; + + auto from = min_arc_length; + auto to = max_arc_length; + while (to - from > precision) { + auto l = from + 0.5 * (to - from); + interpolated_pose = motion_utils::calcInterpolatedPose(trajectory, l); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); + is_avoiding_pose = + std::all_of(polygons_to_avoid.begin(), polygons_to_avoid.end(), [&](const auto & polygon) { + return boost::geometry::disjoint(interpolated_footprint, polygon); + }); + if (is_avoiding_pose) { + from = l; + } else { + to = l; + } + } + if (is_avoiding_pose) { + return interpolated_pose; + } + return std::nullopt; } -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, - const autoware::universe_utils::Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params) +std::optional calculate_pose_ahead_of_collision( + const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, + const universe_utils::Polygon2d & footprint, const double precision) { - const auto from_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); - const auto to_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, 0, decision.target_trajectory_idx); - TrajectoryPoint interpolated_point; - for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { - // TODO(Maxime): binary search - interpolated_point.pose = - autoware::motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); - const auto respect_decel_limit = - !params.skip_if_over_max_decel || prev_slowdown_point || - can_decelerate(ego_data, interpolated_point, decision.velocity); - const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.pose); - const auto is_overlap_lane = boost::geometry::overlaps( - interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); - const auto is_overlap_extra_lane = - prev_slowdown_point && - boost::geometry::overlaps( - interpolated_footprint, - prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); - if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) - return interpolated_point; + const auto first_avoid_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0UL, point_to_avoid.trajectory_index); + for (auto l = first_avoid_arc_length - precision; l >= ego_data.min_stop_arc_length; + l -= precision) { + const auto interpolated_pose = + motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); + if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) { + return interpolated_pose; + } } return std::nullopt; } -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional & prev_slowdown_point, PlannerParam params) +std::optional calculate_slowdown_point( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params) { + const auto point_to_avoid_it = std::find_if( + out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(), + [&](const auto & p) { return p.to_avoid; }); + if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) { + return std::nullopt; + } + const auto raw_footprint = make_base_footprint(params, true); // ignore extra footprint offsets + const auto base_footprint = make_base_footprint(params); params.extra_front_offset += params.lon_dist_buffer; params.extra_right_offset += params.lat_dist_buffer; params.extra_left_offset += params.lat_dist_buffer; - const auto base_footprint = make_base_footprint(params); + const auto expanded_footprint = make_base_footprint(params); // with added distance buffers + lanelet::BasicPolygons2d polygons_to_avoid; + for (const auto & ll : point_to_avoid_it->overlapped_lanelets) { + polygons_to_avoid.push_back(ll.polygon2d().basicPolygon()); + } + // points are ordered by trajectory index so the first one has the smallest index and arc length + const auto first_outside_idx = out_of_lane_data.outside_points.front().trajectory_index; + const auto first_outside_arc_length = + motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, first_outside_idx); + std::optional slowdown_point; // search for the first slowdown decision for which a stop point can be inserted - for (const auto & decision : decisions) { - const auto last_in_lane_pose = - calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); - if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + // we first try to use the expanded footprint (distance buffers + extra footprint offsets) + for (const auto & footprint : {expanded_footprint, base_footprint, raw_footprint}) { + slowdown_point = calculate_last_avoiding_pose( + ego_data.trajectory_points, footprint, polygons_to_avoid, ego_data.min_stop_arc_length, + first_outside_arc_length, params.precision); + if (slowdown_point) { + break; + } } - return std::nullopt; + // fallback to simply stopping ahead of the collision to avoid (regardless of being out of lane or + // not) + if (!slowdown_point) { + slowdown_point = calculate_pose_ahead_of_collision( + ego_data, *point_to_avoid_it, expanded_footprint, params.precision); + } + return slowdown_point; } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index 145f21c94c0d0..394334d434558 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -19,40 +19,39 @@ #include +#include + #include -#include namespace autoware::motion_velocity_planner::out_of_lane { - -/// @brief estimate whether ego can decelerate without breaking the deceleration limit -/// @details assume ego wants to reach the target point at the target velocity -/// @param [in] ego_data ego data -/// @param [in] point target point -/// @param [in] target_vel target_velocity -bool can_decelerate( - const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel); - -/// @brief calculate the last pose along the trajectory where ego does not overlap the lane to avoid +/// @brief calculate the last pose along the trajectory where ego does not go out of lane /// @param [in] ego_data ego data -/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) /// @param [in] footprint the ego footprint -/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits -/// @param [in] params parameters +/// @param [in] min_arc_length minimum arc length for the search +/// @param [in] max_arc_length maximum arc length for the search +/// @param [in] precision [m] search precision /// @return the last pose that is not out of lane (if found) -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, - const autoware::universe_utils::Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params); +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const autoware::universe_utils::Polygon2d & footprint, + const double min_arc_length, const double max_arc_length, const double precision); + +/// @brief calculate the slowdown pose just ahead of a point to avoid +/// @param ego_data ego data (trajectory, velocity, etc) +/// @param point_to_avoid the point to avoid +/// @param footprint the ego footprint +/// @param params parameters +/// @return optional slowdown point to insert in the trajectory +std::optional calculate_pose_ahead_of_collision( + const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, + const universe_utils::Polygon2d & footprint, const double precision); /// @brief calculate the slowdown point to insert in the trajectory /// @param ego_data ego data (trajectory, velocity, etc) -/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) -/// @param prev_slowdown_point previously calculated slowdown point +/// @param out_of_lane_data data about out of lane areas /// @param params parameters /// @return optional slowdown point to insert in the trajectory -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional & prev_slowdown_point, PlannerParam params); +std::optional calculate_slowdown_point( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index d9a85e266f790..6febd15ef8052 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -16,12 +16,26 @@ #include "types.hpp" +#include +#include +#include #include +#include +#include +#include +#include #include +#include +#include + +#include +#include +#include +#include + #include -#include namespace autoware::motion_velocity_planner::out_of_lane::debug { @@ -36,209 +50,186 @@ visualization_msgs::msg::Marker get_base_marker() base_marker.id = 0; base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.pose.position = autoware::universe_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = autoware::universe_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + base_marker.pose.position = universe_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = universe_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); return base_marker; } -void add_footprint_markers( +void add_polygons_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) + const visualization_msgs::msg::Marker & base_marker, const lanelet::BasicPolygons2d & polygons) { - auto debug_marker = get_base_marker(); - debug_marker.ns = "footprints"; - for (const auto & f : footprints) { - debug_marker.points.clear(); - for (const auto & p : f) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - debug_marker.points.clear(); + auto debug_marker = base_marker; + debug_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + for (const auto & f : polygons) { + boost::geometry::for_each_segment(f, [&](const auto & s) { + const auto & [p1, p2] = s; + debug_marker.points.push_back(universe_utils::createMarkerPosition(p1.x(), p1.y(), 0.0)); + debug_marker.points.push_back(universe_utils::createMarkerPosition(p2.x(), p2.y(), 0.0)); + }); } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); + debug_marker_array.markers.push_back(debug_marker); } void add_current_overlap_marker( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) + const lanelet::BasicPolygon2d & current_footprint, const double z) { auto debug_marker = get_base_marker(); debug_marker.ns = "current_overlap"; debug_marker.points.clear(); for (const auto & p : current_footprint) - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z)); + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z)); if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); - if (current_overlapped_lanelets.empty()) - debug_marker.color = autoware::universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); - else - debug_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; - for (const auto & ll : current_overlapped_lanelets) { - debug_marker.points.clear(); - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_lanelet_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = ns; - debug_marker.color = color; - for (const auto & ll : lanelets) { - debug_marker.points.clear(); - - // add a small z offset to draw above the lanelet map - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); } -void add_range_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const std::vector & trajectory_points, - const size_t first_ego_idx, const double z, const size_t prev_nb) +void add_ttc_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const EgoData & ego_data, + const OutOfLaneData & out_of_lane_data, const size_t prev_nb) { auto debug_marker = get_base_marker(); - debug_marker.ns = "ranges"; - debug_marker.color = autoware::universe_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); - for (const auto & range : ranges) { - debug_marker.points.clear(); - debug_marker.points.push_back( - trajectory_points[first_ego_idx + range.entering_trajectory_idx].pose.position); - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - for (const auto & overlap : range.debug.overlaps) { - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + debug_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + debug_marker.scale.z = 0.5; + debug_marker.ns = "ttcs"; + for (const auto & p : out_of_lane_data.outside_points) { + if (p.to_avoid) { + debug_marker.color.r = 1.0; + debug_marker.color.g = 0.0; + } else { + debug_marker.color.r = 0.0; + debug_marker.color.g = 1.0; + } + if (p.ttc) { + debug_marker.pose = ego_data.trajectory_points[p.trajectory_index].pose; + debug_marker.text = std::to_string(*p.ttc); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; } - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.exiting_point.x(), range.exiting_point.y(), z)); - debug_marker.points.push_back( - trajectory_points[first_ego_idx + range.exiting_trajectory_idx].pose.position); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker.action = visualization_msgs::msg::Marker::DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { debug_marker_array.markers.push_back(debug_marker); + } } - -void add_decision_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, +size_t add_stop_line_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const StopLinesRtree & rtree, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); - debug_marker.action = debug_marker.ADD; - debug_marker.id = 0; - debug_marker.ns = "decisions"; - debug_marker.color = autoware::universe_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); - debug_marker.points.clear(); - for (const auto & range : ranges) { - debug_marker.type = debug_marker.LINE_STRIP; - if (range.debug.decision) { - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - debug_marker.points.push_back( - range.debug.object->kinematics.initial_pose_with_covariance.pose.position); + debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + debug_marker.ns = "stop_lines"; + const auto & add_lanelets_markers = [&](const auto & lanelets) { + for (const auto & ll : lanelets) { + debug_marker.points.clear(); + for (const auto & p : ll.polygon2d().basicPolygon()) { + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + } + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + ++debug_marker.id; } - debug_marker_array.markers.push_back(debug_marker); + }; + for (const auto & [_, stop_line] : rtree) { debug_marker.points.clear(); - debug_marker.id++; - - debug_marker.type = debug_marker.TEXT_VIEW_FACING; - debug_marker.pose.position.x = range.entering_point.x(); - debug_marker.pose.position.y = range.entering_point.y(); - debug_marker.pose.position.z = z; - std::stringstream ss; - ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time - << "\n"; - if (range.debug.object) { - debug_marker.pose.position.x += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; - debug_marker.pose.position.y += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; - debug_marker.pose.position.x /= 2; - debug_marker.pose.position.y /= 2; - ss << "Obj: " << range.debug.times.object.enter_time << " - " - << range.debug.times.object.exit_time << "\n"; + debug_marker.color.r = 1.0; + for (const auto & p : stop_line.stop_line) { + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); } - debug_marker.scale.z = 1.0; - debug_marker.text = ss.str(); debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; + ++debug_marker.id; + debug_marker.color.r = 0.0; + add_lanelets_markers(stop_line.lanelets); } - debug_marker.action = debug_marker.DELETE; + const auto max_id = debug_marker.id; + debug_marker.action = visualization_msgs::msg::Marker::DELETE; for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { debug_marker_array.markers.push_back(debug_marker); } + return max_id; } } // namespace -visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data) + +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) { - constexpr auto z = 0.0; + const auto z = ego_data.pose.position.z; visualization_msgs::msg::MarkerArray debug_marker_array; - debug::add_footprint_markers( - debug_marker_array, debug_data.footprints, z, debug_data.prev_footprints); - debug::add_current_overlap_marker( - debug_marker_array, debug_data.current_footprint, debug_data.current_overlapped_lanelets, z, - debug_data.prev_current_overlapped_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.trajectory_lanelets, "trajectory_lanelets", - autoware::universe_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), - debug_data.prev_trajectory_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.ignored_lanelets, "ignored_lanelets", - autoware::universe_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), - debug_data.prev_ignored_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.other_lanelets, "other_lanelets", - autoware::universe_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), - debug_data.prev_other_lanelets); - debug::add_range_markers( - debug_marker_array, debug_data.ranges, debug_data.trajectory_points, - debug_data.first_trajectory_idx, z, debug_data.prev_ranges); - debug::add_decision_markers(debug_marker_array, debug_data.ranges, z, debug_data.prev_ranges); + auto base_marker = get_base_marker(); + base_marker.pose.position.z = z + 0.5; + base_marker.ns = "footprints"; + base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); + // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation + // disabled to prevent performance issues when publishing the debug markers + // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); + + lanelet::BasicPolygons2d drivable_lane_polygons; + for (const auto & poly : ego_data.drivable_lane_polygons) { + drivable_lane_polygons.push_back(poly.outer); + } + base_marker.ns = "ego_lane"; + base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0); + add_polygons_markers(debug_marker_array, base_marker, drivable_lane_polygons); + + lanelet::BasicPolygons2d out_of_lane_areas; + for (const auto & p : out_of_lane_data.outside_points) { + out_of_lane_areas.push_back(p.outside_ring); + } + base_marker.ns = "out_of_lane_areas"; + base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0); + add_polygons_markers(debug_marker_array, base_marker, out_of_lane_areas); + for (const auto & [bbox, i] : out_of_lane_data.outside_areas_rtree) { + const auto & a = out_of_lane_data.outside_points[i]; + debug_marker_array.markers.back().points.push_back( + ego_data.trajectory_points[a.trajectory_index].pose.position); + const auto centroid = boost::geometry::return_centroid(a.outside_ring); + debug_marker_array.markers.back().points.push_back( + geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + } + + lanelet::BasicPolygons2d object_polygons; + for (const auto & o : objects.objects) { + for (const auto & path : o.kinematics.predicted_paths) { + for (const auto & pose : path.path) { + // limit the draw distance to improve performance + if (universe_utils::calcDistance2d(pose, ego_data.pose) < 50.0) { + const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); + lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); + object_polygons.push_back(ll_poly); + } + } + } + } + base_marker.ns = "objects"; + base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); + add_polygons_markers(debug_marker_array, base_marker, object_polygons); + + add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z); + + add_ttc_markers(debug_marker_array, ego_data, out_of_lane_data, debug_data.prev_ttcs); + debug_data.prev_ttcs = out_of_lane_data.outside_points.size(); + + debug_data.prev_stop_line = add_stop_line_markers( + debug_marker_array, ego_data.stop_lines_rtree, z, debug_data.prev_stop_line); + return debug_marker_array; } -autoware::motion_utils::VirtualWalls create_virtual_walls( - const DebugData & debug_data, const PlannerParam & params) +motion_utils::VirtualWalls create_virtual_walls( + const geometry_msgs::msg::Pose & pose, const bool stop, const PlannerParam & params) { - autoware::motion_utils::VirtualWalls virtual_walls; - autoware::motion_utils::VirtualWall wall; + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; wall.text = "out_of_lane"; wall.longitudinal_offset = params.front_offset; - wall.style = autoware::motion_utils::VirtualWallType::slowdown; - for (const auto & slowdown : debug_data.slowdowns) { - wall.pose = slowdown.point.pose; - virtual_walls.push_back(wall); - } + wall.style = stop ? motion_utils::VirtualWallType::stop : motion_utils::VirtualWallType::slowdown; + wall.pose = pose; + virtual_walls.push_back(wall); return virtual_walls; } } // namespace autoware::motion_velocity_planner::out_of_lane::debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp index ea225442443c8..dc1f942655612 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp @@ -23,9 +23,11 @@ namespace autoware::motion_velocity_planner::out_of_lane::debug { -visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); -autoware::motion_utils::VirtualWalls create_virtual_walls( - const DebugData & debug_data, const PlannerParam & params); +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data); +motion_utils::VirtualWalls create_virtual_walls( + const geometry_msgs::msg::Pose & pose, const bool stop, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane::debug #endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp deleted file mode 100644 index fc487a2626b88..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp +++ /dev/null @@ -1,389 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "decisions.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -namespace autoware::motion_velocity_planner::out_of_lane -{ -double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx) -{ - return autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); -} - -double time_along_trajectory( - const EgoData & ego_data, const size_t target_idx, const double min_velocity) -{ - const auto dist = distance_along_trajectory(ego_data, target_idx); - const auto v = std::max( - std::max(ego_data.velocity, min_velocity), - ego_data.trajectory_points[ego_data.first_trajectory_idx + target_idx] - .longitudinal_velocity_mps * - 0.5); - return dist / v; -} - -bool object_is_incoming( - const lanelet::BasicPoint2d & object_position, - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lane) -{ - const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); - if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; - for (const auto & lls : lanelets) - for (const auto & ll : lls) - if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; - return false; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger) -{ - // skip the dynamic object if it is not in a lane preceding the overlapped lane - // lane changes are intentionally not considered - const auto object_point = lanelet::BasicPoint2d( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - - const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; - const auto max_lon_deviation = object.shape.dimensions.x / 2.0; - auto worst_enter_time = std::optional(); - auto worst_exit_time = std::optional(); - - for (const auto & predicted_path : object.kinematics.predicted_paths) { - const auto unique_path_points = - autoware::motion_utils::removeOverlapPoints(predicted_path.path); - if (unique_path_points.size() < 2) continue; - const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); - const auto enter_point = - geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); - const auto enter_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, enter_segment_idx, enter_point); - const auto enter_lat_dist = std::abs(autoware::motion_utils::calcLateralOffset( - unique_path_points, enter_point, enter_segment_idx)); - const auto enter_segment_length = autoware::universe_utils::calcDistance2d( - unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); - const auto enter_offset_ratio = enter_offset / enter_segment_length; - const auto enter_time = - static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; - - const auto exit_point = - geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); - const auto exit_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, exit_segment_idx, exit_point); - const auto exit_lat_dist = std::abs( - autoware::motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); - const auto exit_segment_length = autoware::universe_utils::calcDistance2d( - unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); - const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); - const auto exit_time = - static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; - - RCLCPP_DEBUG( - logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, - enter_time, exit_time); - // predicted path is too far from the overlapping range to be relevant - const auto is_far_from_entering_point = enter_lat_dist > max_deviation; - const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; - if (is_far_from_entering_point && is_far_from_exiting_point) { - RCLCPP_DEBUG( - logger, - " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", - is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, - max_deviation); - continue; - } - const auto is_last_predicted_path_point = - (exit_segment_idx + 2 == unique_path_points.size() || - enter_segment_idx + 2 == unique_path_points.size()); - const auto does_not_reach_overlap = - is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); - if (does_not_reach_overlap) { - RCLCPP_DEBUG( - logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", - std::min(exit_offset, enter_offset), max_lon_deviation); - continue; - } - - const auto same_driving_direction_as_ego = enter_time < exit_time; - if (same_driving_direction_as_ego) { - worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; - } else { - worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; - } - } - if (worst_enter_time && worst_exit_time) { - RCLCPP_DEBUG( - logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, - *worst_exit_time); - return std::make_pair(*worst_enter_time, *worst_exit_time); - } - RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); - return {}; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger) -{ - const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; - const auto object_point = lanelet::BasicPoint2d(p.x, p.y); - const auto half_size = object.shape.dimensions.x / 2.0; - lanelet::ConstLanelets object_lanelets; - for (const auto & ll : inputs.lanelets) - if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) - object_lanelets.push_back(ll); - - geometry_msgs::msg::Pose pose; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - const auto range_size = std::abs(range_enter_length - range_exit_length); - auto worst_enter_dist = std::optional(); - auto worst_exit_dist = std::optional(); - for (const auto & lane : object_lanelets) { - const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); - RCLCPP_DEBUG( - logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); - if (path) { - lanelet::ConstLanelets lls; - for (const auto & ll : *path) lls.push_back(ll); - pose.position.set__x(object_point.x()).set__y(object_point.y()); - const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - RCLCPP_DEBUG( - logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, - enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, - range.exiting_point.x(), range.exiting_point.y()); - const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; - if (already_entered_range) continue; - // multiple paths to the overlap -> be conservative and use the "worst" case - // "worst" = min/max arc length depending on if the lane is running opposite to the ego - // trajectory - const auto is_opposite = enter_dist > exit_dist; - if (!worst_enter_dist) - worst_enter_dist = enter_dist; - else if (is_opposite) - worst_enter_dist = std::max(*worst_enter_dist, enter_dist); - else - worst_enter_dist = std::min(*worst_enter_dist, enter_dist); - if (!worst_exit_dist) - worst_exit_dist = exit_dist; - else if (is_opposite) - worst_exit_dist = std::max(*worst_exit_dist, exit_dist); - else - worst_exit_dist = std::min(*worst_exit_dist, exit_dist); - } - } - if (worst_enter_dist && worst_exit_dist) { - const auto v = object.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::make_pair((*worst_enter_dist - half_size) / v, (*worst_exit_dist + half_size) / v); - } - return {}; -} - -bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) -{ - const auto enter_within_threshold = - range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; - const auto exit_within_threshold = - range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; - return enter_within_threshold || exit_within_threshold; -} - -bool intervals_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto opposite_way_condition = [&]() -> bool { - const auto ego_exits_before_object_enters = - range_times.ego.exit_time + params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] (opposite way) ego exit %2.2fs < obj enter %2.2fs ? -> should not " - "enter = %d\n", - range_times.ego.exit_time + params.intervals_ego_buffer, - range_times.object.enter_time + params.intervals_obj_buffer, ego_exits_before_object_enters); - return ego_exits_before_object_enters; - }; - const auto same_way_condition = [&]() -> bool { - const auto object_enters_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer && - range_times.object.enter_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - const auto object_exits_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.exit_time + params.intervals_obj_buffer && - range_times.object.exit_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] obj enters during overlap ? %d OR obj exits during overlap %d ? -> should " - "not " - "enter = %d\n", - object_enters_during_overlap, object_exits_during_overlap, - object_enters_during_overlap || object_exits_during_overlap); - return object_enters_during_overlap || object_exits_during_overlap; - }; - - const auto object_is_going_same_way = - range_times.object.enter_time < range_times.object.exit_time; - return (object_is_going_same_way && same_way_condition()) || - (!object_is_going_same_way && opposite_way_condition()); -} - -bool ttc_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto ttc_at_enter = range_times.ego.enter_time - range_times.object.enter_time; - const auto ttc_at_exit = range_times.ego.exit_time - range_times.object.exit_time; - const auto collision_during_overlap = (ttc_at_enter < 0.0) != (ttc_at_exit < 0.0); - const auto ttc_is_bellow_threshold = - std::min(std::abs(ttc_at_enter), std::abs(ttc_at_exit)) <= params.ttc_threshold; - RCLCPP_DEBUG( - logger, "\t\t\t[TTC] (%2.2fs - %2.2fs) -> %d\n", ttc_at_enter, ttc_at_exit, - (collision_during_overlap || ttc_is_bellow_threshold)); - return collision_during_overlap || ttc_is_bellow_threshold; -} - -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - RCLCPP_DEBUG( - logger, " enter at %2.2fs, exits at %2.2fs\n", range_times.object.enter_time, - range_times.object.exit_time); - return (params.mode == "threshold" && threshold_condition(range_times, params)) || - (params.mode == "intervals" && intervals_condition(range_times, params, logger)) || - (params.mode == "ttc" && ttc_condition(range_times, params, logger)); -} - -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - RangeTimes range_times{}; - range_times.ego.enter_time = - time_along_trajectory(inputs.ego_data, range.entering_trajectory_idx, params.ego_min_velocity); - range_times.ego.exit_time = - time_along_trajectory(inputs.ego_data, range.exiting_trajectory_idx, params.ego_min_velocity); - RCLCPP_DEBUG( - logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", - range.entering_trajectory_idx, range.exiting_trajectory_idx, range.lane.id(), - range_times.ego.enter_time, range_times.ego.exit_time); - for (const auto & object : inputs.objects.objects) { - RCLCPP_DEBUG( - logger, "\t\t[%s] going at %2.2fm/s", - autoware::universe_utils::toHexString(object.object_id).c_str(), - object.kinematics.initial_twist_with_covariance.twist.linear.x); - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { - RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); - continue; // skip objects with velocity bellow a threshold - } - // skip objects that are already on the interval - const auto enter_exit_time = - params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_dist_buffer, logger) - : object_time_to_range(object, range, inputs, logger); - if (!enter_exit_time) { - RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); - continue; // skip objects that are not driving towards the overlapping range - } - - range_times.object.enter_time = enter_exit_time->first; - range_times.object.exit_time = enter_exit_time->second; - if (will_collide_on_range(range_times, params, logger)) { - range.debug.times = range_times; - range.debug.object = object; - return true; - } - } - range.debug.times = range_times; - return false; -} - -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params) -{ - if (distance < params.stop_dist_threshold) { - decision->velocity = 0.0; - } else if (distance < params.slow_dist_threshold) { - decision->velocity = params.slow_velocity; - } else { - decision.reset(); - } -} - -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - std::optional decision; - if (should_not_enter(range, inputs, params, logger)) { - decision.emplace(); - decision->target_trajectory_idx = inputs.ego_data.first_trajectory_idx + - range.entering_trajectory_idx; // add offset from curr pose - decision->lane_to_avoid = range.lane; - const auto ego_dist_to_range = - distance_along_trajectory(inputs.ego_data, range.entering_trajectory_idx); - set_decision_velocity(decision, ego_dist_to_range, params); - } - return decision; -} - -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger) -{ - std::vector decisions; - for (const auto & range : inputs.ranges) { - if (range.entering_trajectory_idx == 0UL) continue; // skip if we already entered the range - const auto optional_decision = calculate_decision(range, inputs, params, logger); - range.debug.decision = optional_decision; - if (optional_decision) decisions.push_back(*optional_decision); - } - return decisions; -} - -} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp deleted file mode 100644 index 455cee272f7be..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef DECISIONS_HPP_ -#define DECISIONS_HPP_ - -#include "types.hpp" - -#include -#include - -#include - -#include - -#include -#include -#include -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ -/// @brief calculate the distance along the ego trajectory between ego and some target trajectory -/// index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego trajectory index -/// @return distance between ego and the target [m] -double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx); -/// @brief estimate the time when ego will reach some target trajectory index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego trajectory index -/// @param [in] min_velocity minimum ego velocity used to estimate the time -/// @return time taken by ego to reach the target [s] -double time_along_trajectory(const EgoData & ego_data, const size_t target_idx); -/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit -/// points of an overlapping range -/// @details times when the predicted paths of the object enters/exits the range are calculated -/// but may not exist (e.g,, predicted path ends before reaching the end of the range) -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger); -/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit -/// points of an overlapping range -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit. -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger); -/// @brief decide whether an object is coming in the range at the same time as ego -/// @details the condition depends on the mode (threshold, intervals, ttc) -/// @param [in] range_times times when ego and the object enter/exit the range -/// @param [in] params parameters -/// @param [in] logger ros logger -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); -/// @brief check whether we should avoid entering the given range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return true if we should avoid entering the range -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief set the velocity of a decision (or unset it) based on the distance away from the range -/// @param [out] decision decision to update (either set its velocity or unset the decision) -/// @param [in] distance distance between ego and the range corresponding to the decision -/// @param [in] params parameters -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params); -/// @brief calculate the decision to slowdown or stop before an overlapping range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return an optional decision to slowdown or stop -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief calculate decisions to slowdown or stop before some overlapping ranges -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return the calculated decisions to slowdown or stop -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -} // namespace autoware::motion_velocity_planner::out_of_lane - -#endif // DECISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index a53129f372e3d..79bdefee4c4c7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -53,7 +54,7 @@ void cut_predicted_path_beyond_line( auto cut_idx = stop_line_idx; double arc_length = 0; while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += autoware::universe_utils::calcDistance2d( + arc_length += universe_utils::calcDistance2d( predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); --cut_idx; } @@ -102,12 +103,11 @@ void cut_predicted_path_beyond_red_lights( } autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const std::shared_ptr planner_data, const EgoData & ego_data, - const PlannerParam & params) + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data->predicted_objects.header; - for (const auto & object : planner_data->predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects.header; + for (const auto & object : planner_data.predicted_objects.objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; @@ -125,10 +125,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = autoware::motion_utils::removeOverlapPoints(predicted_path.path); + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); if (no_overlap_path.size() <= 1) return true; - const auto lat_offset_to_current_ego = std::abs( - autoware::motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); const auto is_crossing_ego = lat_offset_to_current_ego <= object.shape.dimensions.y / 2.0 + std::max( @@ -136,23 +136,21 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( params.right_offset + params.extra_right_offset); return is_low_confidence || is_crossing_ego; }; - if (params.objects_use_predicted_paths) { - auto & predicted_paths = filtered_object.kinematics.predicted_paths; - const auto new_end = - std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); - predicted_paths.erase(new_end, predicted_paths.end()); - if (params.objects_cut_predicted_paths_beyond_red_lights) - for (auto & predicted_path : predicted_paths) - cut_predicted_path_beyond_red_lights( - predicted_path, ego_data, filtered_object.shape.dimensions.x); - predicted_paths.erase( - std::remove_if( - predicted_paths.begin(), predicted_paths.end(), - [](const auto & p) { return p.path.empty(); }), - predicted_paths.end()); - } + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, ego_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); - if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + if (!filtered_object.kinematics.predicted_paths.empty()) filtered_objects.objects.push_back(filtered_object); } return filtered_objects; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index baf07e4b06ea5..3b2ae11718810 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -19,7 +19,6 @@ #include -#include #include namespace autoware::motion_velocity_planner::out_of_lane @@ -52,8 +51,7 @@ void cut_predicted_path_beyond_red_lights( /// @param [in] params parameters /// @return filtered predicted objects autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const std::shared_ptr planner_data, const EgoData & ego_data, - const PlannerParam & params); + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index f5d68c4fa9bba..564bf5de7ef2e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -21,15 +21,13 @@ #include #include -#include #include namespace autoware::motion_velocity_planner::out_of_lane { -autoware::universe_utils::Polygon2d make_base_footprint( - const PlannerParam & p, const bool ignore_offset) +universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool ignore_offset) { - autoware::universe_utils::Polygon2d base_footprint; + universe_utils::Polygon2d base_footprint; const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; @@ -43,10 +41,10 @@ autoware::universe_utils::Polygon2d make_base_footprint( } lanelet::BasicPolygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) + const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); @@ -59,22 +57,15 @@ std::vector calculate_trajectory_footprints( const auto base_footprint = make_base_footprint(params); std::vector trajectory_footprints; trajectory_footprints.reserve(ego_data.trajectory_points.size()); - double length = 0.0; - const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + - params.front_offset + params.extra_front_offset; - for (auto i = ego_data.first_trajectory_idx; - i < ego_data.trajectory_points.size() && length < range; ++i) { + for (auto i = ego_data.first_trajectory_idx; i < ego_data.trajectory_points.size(); ++i) { const auto & trajectory_pose = ego_data.trajectory_points[i].pose; const auto angle = tf2::getYaw(trajectory_pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back( p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); trajectory_footprints.push_back(footprint); - if (i + 1 < ego_data.trajectory_points.size()) - length += autoware::universe_utils::calcDistance2d( - trajectory_pose, ego_data.trajectory_points[i + 1].pose); } return trajectory_footprints; } @@ -84,7 +75,7 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( { const auto base_footprint = make_base_footprint(params, ignore_offset); const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp index ebe7ab0fa9d7f..88baeefba6f40 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -21,24 +21,21 @@ #include -namespace autoware::motion_velocity_planner -{ -namespace out_of_lane +namespace autoware::motion_velocity_planner::out_of_lane { /// @brief create the base footprint of ego /// @param [in] p parameters used to create the footprint /// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the /// footprint /// @return base ego footprint -autoware::universe_utils::Polygon2d make_base_footprint( +universe_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset = false); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose lanelet::BasicPolygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, - const geometry_msgs::msg::Pose & pose); + const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); /// @brief calculate the trajectory footprints /// @details the resulting polygon follows the format used by the lanelet library: clockwise order /// and implicit closing edge @@ -54,7 +51,6 @@ std::vector calculate_trajectory_footprints( /// footprint lanelet::BasicPolygon2d calculate_current_ego_footprint( const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); -} // namespace out_of_lane -} // namespace autoware::motion_velocity_planner +} // namespace autoware::motion_velocity_planner::out_of_lane #endif // FOOTPRINT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index bca849f806f63..f520a564519f0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -14,36 +14,46 @@ #include "lanelets_selection.hpp" +#include "types.hpp" + #include #include +#include +#include #include -#include #include #include -#include namespace autoware::motion_velocity_planner::out_of_lane { +namespace +{ +bool is_road_lanelet(const lanelet::ConstLanelet & lanelet) +{ + return lanelet.hasAttribute(lanelet::AttributeName::Subtype) && + lanelet.attribute(lanelet::AttributeName::Subtype) == lanelet::AttributeValueString::Road; +} +} // namespace + lanelet::ConstLanelets consecutive_lanelets( - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lanelet) + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) { - lanelet::ConstLanelets consecutives = route_handler->getRoutingGraphPtr()->following(lanelet); - const auto previous = route_handler->getRoutingGraphPtr()->previous(lanelet); + lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet); consecutives.insert(consecutives.end(), previous.begin(), previous.end()); return consecutives; } lanelet::ConstLanelets get_missing_lane_change_lanelets( - lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler) + const lanelet::ConstLanelets & trajectory_lanelets, + const route_handler::RouteHandler & route_handler) { lanelet::ConstLanelets missing_lane_change_lanelets; - const auto & routing_graph = *route_handler->getRoutingGraphPtr(); + const auto & routing_graph = *route_handler.getRoutingGraphPtr(); lanelet::ConstLanelets adjacents; lanelet::ConstLanelets consecutives; for (const auto & ll : trajectory_lanelets) { @@ -67,9 +77,9 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const std::shared_ptr route_handler) + const EgoData & ego_data, const route_handler::RouteHandler & route_handler) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); lanelet::ConstLanelets trajectory_lanelets; lanelet::BasicLineString2d trajectory_ls; for (const auto & p : ego_data.trajectory_points) @@ -77,7 +87,9 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( const auto candidates = lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls)); for (const auto & ll : candidates) { - if (!boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) { + if ( + is_road_lanelet(ll) && + !boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) { trajectory_lanelets.push_back(ll); } } @@ -89,43 +101,65 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( } lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params) + const lanelet::ConstLanelets & trajectory_lanelets, + const route_handler::RouteHandler & route_handler) { lanelet::ConstLanelets ignored_lanelets; - // ignore lanelets directly behind ego - const auto behind = - autoware::universe_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); - const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.y); - const auto behind_lanelets = lanelet::geometry::findWithin2d( - route_handler->getLaneletMapPtr()->laneletLayer, behind_point, 0.0); - for (const auto & l : behind_lanelets) { - const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, l.second.id()); - if (!is_trajectory_lanelet) ignored_lanelets.push_back(l.second); + // ignore lanelets directly preceding a trajectory lanelet + for (const auto & trajectory_lanelet : trajectory_lanelets) { + for (const auto & ll : route_handler.getPreviousLanelets(trajectory_lanelet)) { + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.id()); + if (!is_trajectory_lanelet) ignored_lanelets.push_back(ll); + } } return ignored_lanelets; } -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params) +void calculate_drivable_lane_polygons( + EgoData & ego_data, const route_handler::RouteHandler & route_handler) +{ + const auto route_lanelets = calculate_trajectory_lanelets(ego_data, route_handler); + const auto ignored_lanelets = + out_of_lane::calculate_ignored_lanelets(route_lanelets, route_handler); + for (const auto & ll : route_lanelets) { + out_of_lane::Polygons tmp; + boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); + ego_data.drivable_lane_polygons = tmp; + } + for (const auto & ll : ignored_lanelets) { + out_of_lane::Polygons tmp; + boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); + ego_data.drivable_lane_polygons = tmp; + } +} + +void calculate_overlapped_lanelets( + OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler) { - lanelet::ConstLanelets other_lanelets; - const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); - const auto lanelets_within_range = lanelet::geometry::findWithin2d( - route_handler->getLaneletMapPtr()->laneletLayer, ego_point, - std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + - params.extra_front_offset); - for (const auto & ll : lanelets_within_range) { - if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") - continue; - const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.second.id()); - const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - if (!is_trajectory_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); + out_of_lane_point.overlapped_lanelets = lanelet::ConstLanelets(); + const auto candidates = route_handler.getLaneletMapPtr()->laneletLayer.search( + lanelet::geometry::boundingBox2d(out_of_lane_point.outside_ring)); + for (const auto & ll : candidates) { + if ( + is_road_lanelet(ll) && !contains_lanelet(out_of_lane_point.overlapped_lanelets, ll.id()) && + boost::geometry::within(out_of_lane_point.outside_ring, ll.polygon2d().basicPolygon())) { + out_of_lane_point.overlapped_lanelets.push_back(ll); + } + } +} + +void calculate_overlapped_lanelets( + OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler) +{ + for (auto it = out_of_lane_data.outside_points.begin(); + it != out_of_lane_data.outside_points.end();) { + calculate_overlapped_lanelets(*it, route_handler); + if (it->overlapped_lanelets.empty()) { + // do not keep out of lane points that do not overlap any lanelet + out_of_lane_data.outside_points.erase(it); + } else { + ++it; + } } - return other_lanelets; } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp index 8618d808d7d40..a7729deb539b6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -44,6 +44,7 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_trajectory_lanelets( const EgoData & ego_data, const std::shared_ptr route_handler); + /// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during /// a lane change /// @param [in] trajectory_lanelets lanelets driven by the ego vehicle @@ -51,30 +52,28 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( /// @return lanelets that may be overlapped by a lane change (and are not already in /// trajectory_lanelets) lanelet::ConstLanelets get_missing_lane_change_lanelets( - lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler); + const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr & route_handler); + /// @brief calculate lanelets that should be ignored -/// @param [in] ego_data data about the ego vehicle -/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] trajectory_lanelets lanelets followed by the ego vehicle /// @param [in] route_handler route handler -/// @param [in] params parameters /// @return lanelets to ignore lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params); -/// @brief calculate lanelets that should be checked by the module -/// @param [in] ego_data data about the ego vehicle -/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle -/// @param [in] ignored_lanelets lanelets to ignore -/// @param [in] route_handler route handler -/// @param [in] params parameters -/// @return lanelets to check for overlaps -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params); + const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr & route_handler); + +/// @brief calculate the polygons representing the ego lane and add it to the ego data +/// @param [inout] ego_data ego data +/// @param [in] route_handler route handler with map information +void calculate_drivable_lane_polygons( + EgoData & ego_data, const route_handler::RouteHandler & route_handler); + +/// @brief calculate the lanelets overlapped at each out of lane point +/// @param [out] out_of_lane_data data with the out of lane points +/// @param [in] route_handler route handler with the lanelet map +void calculate_overlapped_lanelets( + OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp new file mode 100644 index 0000000000000..aef035200b6cc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -0,0 +1,164 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "out_of_lane_collisions.hpp" + +#include "types.hpp" + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +void update_collision_times( + OutOfLaneData & out_of_lane_data, const std::unordered_set & potential_collision_indexes, + const universe_utils::Polygon2d & object_footprint, const double time) +{ + for (const auto index : potential_collision_indexes) { + auto & out_of_lane_point = out_of_lane_data.outside_points[index]; + if (out_of_lane_point.collision_times.count(time) == 0UL) { + const auto has_collision = + !boost::geometry::disjoint(out_of_lane_point.outside_ring, object_footprint.outer()); + if (has_collision) { + out_of_lane_point.collision_times.insert(time); + } + } + } +} + +void calculate_object_path_time_collisions( + OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedPath & object_path, + const autoware_perception_msgs::msg::Shape & object_shape) +{ + const auto time_step = rclcpp::Duration(object_path.time_step).seconds(); + auto time = time_step; + for (const auto & object_pose : object_path.path) { + time += time_step; + const auto object_footprint = universe_utils::toPolygon2d(object_pose, object_shape); + std::vector query_results; + out_of_lane_data.outside_areas_rtree.query( + boost::geometry::index::intersects(object_footprint.outer()), + std::back_inserter(query_results)); + std::unordered_set potential_collision_indexes; + for (const auto & [_, index] : query_results) { + potential_collision_indexes.insert(index); + } + update_collision_times(out_of_lane_data, potential_collision_indexes, object_footprint, time); + } +} + +void calculate_objects_time_collisions( + OutOfLaneData & out_of_lane_data, + const std::vector & objects) +{ + for (const auto & object : objects) { + for (const auto & path : object.kinematics.predicted_paths) { + calculate_object_path_time_collisions(out_of_lane_data, path, object.shape); + } + } +} + +void calculate_min_max_arrival_times( + OutOfLanePoint & out_of_lane_point, + const std::vector & trajectory) +{ + auto min_time = std::numeric_limits::infinity(); + auto max_time = -std::numeric_limits::infinity(); + for (const auto & t : out_of_lane_point.collision_times) { + min_time = std::min(t, min_time); + max_time = std::max(t, max_time); + } + if (min_time <= max_time) { + out_of_lane_point.min_object_arrival_time = min_time; + out_of_lane_point.max_object_arrival_time = max_time; + const auto & ego_time = + rclcpp::Duration(trajectory[out_of_lane_point.trajectory_index].time_from_start).seconds(); + if (ego_time >= min_time && ego_time <= max_time) { + out_of_lane_point.ttc = 0.0; + } else { + out_of_lane_point.ttc = + std::min(std::abs(ego_time - min_time), std::abs(ego_time - max_time)); + } + } +}; + +void calculate_collisions_to_avoid( + OutOfLaneData & out_of_lane_data, + const std::vector & trajectory, + const PlannerParam & params) +{ + for (auto & out_of_lane_point : out_of_lane_data.outside_points) { + calculate_min_max_arrival_times(out_of_lane_point, trajectory); + } + for (auto & p : out_of_lane_data.outside_points) { + if (params.mode == "ttc") { + p.to_avoid = p.ttc && p.ttc <= params.ttc_threshold; + } else { + p.to_avoid = p.min_object_arrival_time && p.min_object_arrival_time <= params.time_threshold; + } + } +} + +std::vector calculate_out_of_lane_points(const EgoData & ego_data) +{ + std::vector out_of_lane_points; + OutOfLanePoint p; + for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { + p.trajectory_index = i + ego_data.first_trajectory_idx; + const auto & footprint = ego_data.trajectory_footprints[i]; + Polygons out_of_lane_polygons; + boost::geometry::difference(footprint, ego_data.drivable_lane_polygons, out_of_lane_polygons); + for (const auto & area : out_of_lane_polygons) { + if (!area.outer.empty()) { + p.outside_ring = area.outer; + out_of_lane_points.push_back(p); + } + } + } + return out_of_lane_points; +} + +void prepare_out_of_lane_areas_rtree(OutOfLaneData & out_of_lane_data) +{ + std::vector rtree_nodes; + for (auto i = 0UL; i < out_of_lane_data.outside_points.size(); ++i) { + OutAreaNode n; + n.first = boost::geometry::return_envelope( + out_of_lane_data.outside_points[i].outside_ring); + n.second = i; + rtree_nodes.push_back(n); + } + out_of_lane_data.outside_areas_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp new file mode 100644 index 0000000000000..33f0842c56d36 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OUT_OF_LANE_COLLISIONS_HPP_ +#define OUT_OF_LANE_COLLISIONS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief calculate the times and points where ego collides with an object's path outside of its +/// lane +void calculate_object_path_time_collisions( + OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedPath & object_path, + const autoware_perception_msgs::msg::Shape & object_shape); + +/// @brief calculate the times and points where ego collides with an object outside of its lane +void calculate_objects_time_collisions( + OutOfLaneData & out_of_lane_data, + const std::vector & objects); + +/// @brief calculate the collisions to avoid +/// @details either uses the time to collision or just the time when the object will arrive at the +/// point +void calculate_collisions_to_avoid( + OutOfLaneData & out_of_lane_data, + const std::vector & trajectory, + const PlannerParam & params); + +/// @brief calculate the out of lane points +std::vector calculate_out_of_lane_points(const EgoData & ego_data); + +/// @brief prepare the rtree of out of lane points for the given data +void prepare_out_of_lane_areas_rtree(OutOfLaneData & out_of_lane_data); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // OUT_OF_LANE_COLLISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index a6b34a1566e19..c97e10cc8706e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -16,16 +16,16 @@ #include "calculate_slowdown_points.hpp" #include "debug.hpp" -#include "decisions.hpp" #include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" -#include "overlapping_range.hpp" +#include "out_of_lane_collisions.hpp" #include "types.hpp" #include #include #include +#include #include #include #include @@ -34,11 +34,13 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -56,65 +58,52 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_diag_publisher_ = std::make_shared( + processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); processing_time_publisher_ = node.create_publisher( "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { - using autoware::universe_utils::getOrDeclareParameter; + using universe_utils::getOrDeclareParameter; auto & pp = params_; pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); pp.skip_if_already_overlapping = getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); + pp.max_arc_length = getOrDeclareParameter(node, ns_ + ".max_arc_length"); pp.time_threshold = getOrDeclareParameter(node, ns_ + ".threshold.time_threshold"); - pp.intervals_ego_buffer = getOrDeclareParameter(node, ns_ + ".intervals.ego_time_buffer"); - pp.intervals_obj_buffer = - getOrDeclareParameter(node, ns_ + ".intervals.objects_time_buffer"); pp.ttc_threshold = getOrDeclareParameter(node, ns_ + ".ttc.threshold"); pp.objects_min_vel = getOrDeclareParameter(node, ns_ + ".objects.minimum_velocity"); - pp.objects_use_predicted_paths = - getOrDeclareParameter(node, ns_ + ".objects.use_predicted_paths"); pp.objects_min_confidence = getOrDeclareParameter(node, ns_ + ".objects.predicted_path_min_confidence"); - pp.objects_dist_buffer = getOrDeclareParameter(node, ns_ + ".objects.distance_buffer"); pp.objects_cut_predicted_paths_beyond_red_lights = getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); pp.objects_ignore_behind_ego = getOrDeclareParameter(node, ns_ + ".objects.ignore_behind_ego"); - pp.overlap_min_dist = getOrDeclareParameter(node, ns_ + ".overlap.minimum_distance"); - pp.overlap_extra_length = getOrDeclareParameter(node, ns_ + ".overlap.extra_length"); - - pp.skip_if_over_max_decel = - getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); pp.lon_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.longitudinal_distance_buffer"); pp.lat_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.lateral_distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); - pp.slow_dist_threshold = - getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); pp.stop_dist_threshold = getOrDeclareParameter(node, ns_ + ".action.stop.distance_threshold"); - pp.ego_min_velocity = getOrDeclareParameter(node, ns_ + ".ego.min_assumed_velocity"); pp.extra_front_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_front_offset"); pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.front_offset = vehicle_info.max_longitudinal_offset_m; pp.rear_offset = vehicle_info.min_longitudinal_offset_m; pp.left_offset = vehicle_info.max_lateral_offset_m; @@ -123,44 +112,79 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) void OutOfLaneModule::update_parameters(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using universe_utils::updateParam; auto & pp = params_; updateParam(parameters, ns_ + ".mode", pp.mode); updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); + updateParam(parameters, ns_ + ".max_arc_length", pp.max_arc_length); updateParam(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); - updateParam(parameters, ns_ + ".intervals.ego_time_buffer", pp.intervals_ego_buffer); - updateParam(parameters, ns_ + ".intervals.objects_time_buffer", pp.intervals_obj_buffer); updateParam(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); updateParam(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); - updateParam(parameters, ns_ + ".objects.use_predicted_paths", pp.objects_use_predicted_paths); updateParam( parameters, ns_ + ".objects.predicted_path_min_confidence", pp.objects_min_confidence); - updateParam(parameters, ns_ + ".objects.distance_buffer", pp.objects_dist_buffer); updateParam( parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", pp.objects_cut_predicted_paths_beyond_red_lights); updateParam(parameters, ns_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego); - updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); - updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); - updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); updateParam(parameters, ns_ + ".action.precision", pp.precision); updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer); updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer); updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); - updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); - updateParam(parameters, ns_ + ".ego.min_assumed_velocity", pp.ego_min_velocity); updateParam(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); updateParam(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); updateParam(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); } +void OutOfLaneModule::limit_trajectory_size( + out_of_lane::EgoData & ego_data, + const std::vector & ego_trajectory_points, + const double max_arc_length) +{ + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_trajectory_index = + motion_utils::calcLongitudinalOffsetToSegment( + ego_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position); + auto l = -ego_data.longitudinal_offset_to_first_trajectory_index; + ego_data.trajectory_points.push_back(ego_trajectory_points[ego_data.first_trajectory_idx]); + for (auto i = ego_data.first_trajectory_idx + 1; i < ego_trajectory_points.size(); ++i) { + l += universe_utils::calcDistance2d(ego_trajectory_points[i - 1], ego_trajectory_points[i]); + if (l >= max_arc_length) { + break; + } + ego_data.trajectory_points.push_back(ego_trajectory_points[i]); + } +} + +void OutOfLaneModule::calculate_min_stop_and_slowdown_distances( + out_of_lane::EgoData & ego_data, const PlannerData & planner_data, + std::optional & previous_slowdown_pose_, const double slow_velocity) +{ + ego_data.min_stop_distance = planner_data.calculate_min_deceleration_distance(0.0).value_or(0.0); + ego_data.min_slowdown_distance = + planner_data.calculate_min_deceleration_distance(slow_velocity).value_or(0.0); + if (previous_slowdown_pose_) { + // Ensure we do not remove the previous slowdown point due to the min distance limit + const auto previous_slowdown_pose_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.first_trajectory_idx, previous_slowdown_pose_->position); + ego_data.min_stop_distance = + std::min(previous_slowdown_pose_arc_length, ego_data.min_stop_distance); + ego_data.min_slowdown_distance = + std::min(previous_slowdown_pose_arc_length, ego_data.min_slowdown_distance); + } + ego_data.min_stop_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0UL, ego_data.first_trajectory_idx) + + ego_data.longitudinal_offset_to_first_trajectory_index + + ego_data.min_stop_distance; +} + void prepare_stop_lines_rtree( out_of_lane::EgoData & ego_data, const PlannerData & planner_data, const double search_distance) { @@ -197,168 +221,154 @@ void prepare_stop_lines_rtree( ego_data.stop_lines_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } +out_of_lane::OutOfLaneData prepare_out_of_lane_data( + const out_of_lane::EgoData & ego_data, const route_handler::RouteHandler & route_handler) +{ + out_of_lane::OutOfLaneData out_of_lane_data; + out_of_lane_data.outside_points = out_of_lane::calculate_out_of_lane_points(ego_data); + out_of_lane::calculate_overlapped_lanelets(out_of_lane_data, route_handler); + out_of_lane::prepare_out_of_lane_areas_rtree(out_of_lane_data); + return out_of_lane_data; +} + VelocityPlanningResult OutOfLaneModule::plan( const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { VelocityPlanningResult result; - autoware::universe_utils::StopWatch stopwatch; + universe_utils::StopWatch stopwatch; stopwatch.tic(); + + stopwatch.tic("preprocessing"); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - ego_data.trajectory_points = ego_trajectory_points; - ego_data.first_trajectory_idx = - autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); - ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x; - ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); - stopwatch.tic("preprocessing"); - prepare_stop_lines_rtree(ego_data, *planner_data, 100.0); + limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length); + calculate_min_stop_and_slowdown_distances( + ego_data, *planner_data, previous_slowdown_pose_, params_.slow_velocity); + prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length); const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("calculate_trajectory_footprints"); - const auto current_ego_footprint = + ego_data.current_footprint = out_of_lane::calculate_current_ego_footprint(ego_data, params_, true); - const auto trajectory_footprints = - out_of_lane::calculate_trajectory_footprints(ego_data, params_); + ego_data.trajectory_footprints = out_of_lane::calculate_trajectory_footprints(ego_data, params_); const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); - // Calculate lanelets to ignore and consider + stopwatch.tic("calculate_lanelets"); - const auto trajectory_lanelets = - out_of_lane::calculate_trajectory_lanelets(ego_data, planner_data->route_handler); - const auto ignored_lanelets = out_of_lane::calculate_ignored_lanelets( - ego_data, trajectory_lanelets, planner_data->route_handler, params_); - const auto other_lanelets = out_of_lane::calculate_other_lanelets( - ego_data, trajectory_lanelets, ignored_lanelets, planner_data->route_handler, params_); + out_of_lane::calculate_drivable_lane_polygons(ego_data, *planner_data->route_handler); const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); - debug_data_.reset_data(); - debug_data_.trajectory_points = ego_trajectory_points; - debug_data_.footprints = trajectory_footprints; - debug_data_.trajectory_lanelets = trajectory_lanelets; - debug_data_.ignored_lanelets = ignored_lanelets; - debug_data_.other_lanelets = other_lanelets; - debug_data_.first_trajectory_idx = ego_data.first_trajectory_idx; - - if (params_.skip_if_already_overlapping) { - debug_data_.current_footprint = current_ego_footprint; - const auto overlapped_lanelet_it = - std::find_if(other_lanelets.begin(), other_lanelets.end(), [&](const auto & ll) { - return boost::geometry::intersects(ll.polygon2d().basicPolygon(), current_ego_footprint); - }); - if (overlapped_lanelet_it != other_lanelets.end()) { - debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); - RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module\n"); - return result; - } - } - // Calculate overlapping ranges - stopwatch.tic("calculate_overlapping_ranges"); - const auto ranges = out_of_lane::calculate_overlapping_ranges( - trajectory_footprints, trajectory_lanelets, other_lanelets, params_); - const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); - // Calculate stop and slowdown points - out_of_lane::DecisionInputs inputs; - inputs.ranges = ranges; - inputs.ego_data = ego_data; + stopwatch.tic("calculate_out_of_lane_areas"); + auto out_of_lane_data = prepare_out_of_lane_data(ego_data, *planner_data->route_handler); + const auto calculate_out_of_lane_areas_us = stopwatch.toc("calculate_out_of_lane_areas"); + stopwatch.tic("filter_predicted_objects"); - inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); + const auto objects = out_of_lane::filter_predicted_objects(*planner_data, ego_data, params_); const auto filter_predicted_objects_us = stopwatch.toc("filter_predicted_objects"); - inputs.route_handler = planner_data->route_handler; - inputs.lanelets = other_lanelets; - stopwatch.tic("calculate_decisions"); - const auto decisions = out_of_lane::calculate_decisions(inputs, params_, logger_); - const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); - stopwatch.tic("calc_slowdown_points"); + + stopwatch.tic("calculate_time_collisions"); + out_of_lane::calculate_objects_time_collisions(out_of_lane_data, objects.objects); + const auto calculate_time_collisions_us = stopwatch.toc("calculate_time_collisions"); + + stopwatch.tic("calculate_times"); + // calculate times + out_of_lane::calculate_collisions_to_avoid(out_of_lane_data, ego_data.trajectory_points, params_); + const auto calculate_times_us = stopwatch.toc("calculate_times"); + + if ( + params_.skip_if_already_overlapping && !ego_data.drivable_lane_polygons.empty() && + !lanelet::geometry::within(ego_data.current_footprint, ego_data.drivable_lane_polygons)) { + RCLCPP_WARN(logger_, "Ego is already out of lane, skipping the module\n"); + debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array( + ego_data, out_of_lane_data, objects, debug_data_)); + return result; + } + if ( // reset the previous inserted point if the timer expired - prev_inserted_point_ && - (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) - prev_inserted_point_.reset(); - auto point_to_insert = - out_of_lane::calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); - const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); - stopwatch.tic("insert_slowdown_points"); - debug_data_.slowdowns.clear(); - if ( // reset the timer if there is no previous inserted point or if we avoid the same lane - point_to_insert && - (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == - point_to_insert->slowdown.lane_to_avoid.id())) - prev_inserted_point_time_ = clock_->now(); - // reuse previous stop point if there is no new one or if its velocity is not higher than the new + previous_slowdown_pose_ && + (clock_->now() - previous_slowdown_time_).seconds() > params_.min_decision_duration) { + previous_slowdown_pose_.reset(); + } + + stopwatch.tic("calculate_slowdown_point"); + auto slowdown_pose = out_of_lane::calculate_slowdown_point(ego_data, out_of_lane_data, params_); + const auto calculate_slowdown_point_us = stopwatch.toc("calculate_slowdown_point"); + + if ( // reset the timer if there is no previous inserted point + slowdown_pose && (!previous_slowdown_pose_)) { + previous_slowdown_time_ = clock_->now(); + } + // reuse previous stop pose if there is no new one or if its velocity is not higher than the new // one and its arc length is lower - const auto should_use_prev_inserted_point = [&]() { - if ( - point_to_insert && prev_inserted_point_ && - prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { - const auto arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, point_to_insert->point.pose.position); - const auto prev_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + const auto should_use_previous_pose = [&]() { + if (slowdown_pose && previous_slowdown_pose_) { + const auto arc_length = + motion_utils::calcSignedArcLength(ego_trajectory_points, 0LU, slowdown_pose->position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, previous_slowdown_pose_->position); return prev_arc_length < arc_length; } - return !point_to_insert && prev_inserted_point_; + return !slowdown_pose && previous_slowdown_pose_; }(); - if (should_use_prev_inserted_point) { + if (should_use_previous_pose) { // if the trajectory changed the prev point is no longer on the trajectory so we project it - const auto insert_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); - prev_inserted_point_->point.pose = - autoware::motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); - point_to_insert = prev_inserted_point_; + const auto new_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, previous_slowdown_pose_->position); + slowdown_pose = motion_utils::calcInterpolatedPose(ego_trajectory_points, new_arc_length); } - if (point_to_insert) { - prev_inserted_point_ = point_to_insert; - RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); - debug_data_.slowdowns = {*point_to_insert}; - if (point_to_insert->slowdown.velocity == 0.0) - result.stop_points.push_back(point_to_insert->point.pose.position); - else + if (slowdown_pose) { + const auto arc_length = + motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.first_trajectory_idx, slowdown_pose->position) - + ego_data.longitudinal_offset_to_first_trajectory_index; + const auto slowdown_velocity = + arc_length <= params_.stop_dist_threshold ? 0.0 : params_.slow_velocity; + previous_slowdown_pose_ = slowdown_pose; + if (slowdown_velocity == 0.0) { + result.stop_points.push_back(slowdown_pose->position); + } else { result.slowdown_intervals.emplace_back( - point_to_insert->point.pose.position, point_to_insert->point.pose.position, - point_to_insert->slowdown.velocity); - - const auto is_approaching = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, ego_data.pose.position, - point_to_insert->point.pose.position) > 0.1 && - ego_data.velocity > 0.1; - const auto status = is_approaching ? autoware::motion_utils::VelocityFactor::APPROACHING - : autoware::motion_utils::VelocityFactor::STOPPED; + slowdown_pose->position, slowdown_pose->position, slowdown_velocity); + } + + const auto is_approaching = + motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 && + planner_data->current_odometry.twist.twist.linear.x > 0.1; + const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING + : motion_utils::VelocityFactor::STOPPED; velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); + ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane"); result.velocity_factor = velocity_factor_interface_.get(); - } else if (!decisions.empty()) { - RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); + virtual_wall_marker_creator.add_virtual_walls( + out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + } else if (std::any_of( + out_of_lane_data.outside_points.begin(), out_of_lane_data.outside_points.end(), + [](const auto & p) { return p.to_avoid; })) { + RCLCPP_WARN( + logger_, "[out_of_lane] Could not insert slowdown point because of deceleration limits"); } - const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); - debug_data_.ranges = inputs.ranges; + stopwatch.tic("gen_debug"); + const auto markers = + out_of_lane::debug::create_debug_marker_array(ego_data, out_of_lane_data, objects, debug_data_); + const auto markers_us = stopwatch.toc("gen_debug"); + stopwatch.tic("pub"); + debug_publisher_->publish(markers); + const auto pub_markers_us = stopwatch.toc("pub"); const auto total_time_us = stopwatch.toc(); - RCLCPP_DEBUG( - logger_, - "Total time = %2.2fus\n" - "\tpreprocessing = %2.0fus\n" - "\tcalculate_lanelets = %2.0fus\n" - "\tcalculate_trajectory_footprints = %2.0fus\n" - "\tcalculate_overlapping_ranges = %2.0fus\n" - "\tfilter_pred_objects = %2.0fus\n" - "\tcalculate_decisions = %2.0fus\n" - "\tcalc_slowdown_points = %2.0fus\n" - "\tinsert_slowdown_points = %2.0fus\n", - preprocessing_us, total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, - calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us, - calc_slowdown_points_us, insert_slowdown_points_us); - debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); - virtual_wall_marker_creator.add_virtual_walls( - out_of_lane::debug::create_virtual_walls(debug_data_, params_)); - virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); std::map processing_times; processing_times["preprocessing"] = preprocessing_us / 1000; processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000; processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000; - processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000; + processing_times["calculate_out_of_lane_areas"] = calculate_out_of_lane_areas_us / 1000; processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000; - processing_times["calculate_decision"] = calculate_decisions_us / 1000; - processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000; - processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000; + processing_times["calculate_time_collisions"] = calculate_time_collisions_us / 1000; + processing_times["calculate_times"] = calculate_times_us / 1000; + processing_times["calculate_slowdown_point"] = calculate_slowdown_point_us / 1000; + processing_times["generate_markers"] = markers_us / 1000; + processing_times["publish_markers"] = pub_markers_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); tier4_debug_msgs::msg::Float64Stamped processing_time_msg; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index 310a73c04d643..fc0cd4ac539ca 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -23,12 +23,14 @@ #include #include +#include #include #include #include #include +#include #include #include @@ -46,17 +48,28 @@ class OutOfLaneModule : public PluginModuleInterface private: void init_parameters(rclcpp::Node & node); - out_of_lane::PlannerParam params_; + /// @brief resize the trajectory to start from the segment closest to ego and to have at most the + /// given length + static void limit_trajectory_size( + out_of_lane::EgoData & ego_data, + const std::vector & ego_trajectory_points, + const double max_arc_length); + /// @brief calculate the minimum stop and slowdown distances of ego + static void calculate_min_stop_and_slowdown_distances( + out_of_lane::EgoData & ego_data, const PlannerData & planner_data, + std::optional & previous_slowdown_pose_, const double slow_velocity); + + out_of_lane::PlannerParam params_{}; inline static const std::string ns_ = "out_of_lane"; - std::string module_name_; - std::optional prev_inserted_point_{}; - rclcpp::Clock::SharedPtr clock_{}; - rclcpp::Time prev_inserted_point_time_{}; + std::string module_name_{"uninitialized"}; + rclcpp::Clock::SharedPtr clock_{nullptr}; + std::optional previous_slowdown_pose_{std::nullopt}; + rclcpp::Time previous_slowdown_time_{0}; protected: // Debug - mutable out_of_lane::DebugData debug_data_; + mutable out_of_lane::DebugData debug_data_{}; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp deleted file mode 100644 index a1a722bb07f14..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "overlapping_range.hpp" - -#include -#include - -#include - -#include -#include - -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ - -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & trajectory_footprint, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet) -{ - Overlap overlap; - const auto & left_bound = lanelet.leftBound2d().basicLineString(); - const auto & right_bound = lanelet.rightBound2d().basicLineString(); - // TODO(Maxime): these intersects (for each trajectory footprint, for each lanelet) are too - // expensive - const auto overlap_left = boost::geometry::intersects(trajectory_footprint, left_bound); - const auto overlap_right = boost::geometry::intersects(trajectory_footprint, right_bound); - - lanelet::BasicPolygons2d overlapping_polygons; - if (overlap_left || overlap_right) - boost::geometry::intersection( - trajectory_footprint, lanelet.polygon2d().basicPolygon(), overlapping_polygons); - for (const auto & overlapping_polygon : overlapping_polygons) { - for (const auto & point : overlapping_polygon) { - if (overlap_left && overlap_right) - overlap.inside_distance = boost::geometry::distance(left_bound, right_bound); - else if (overlap_left) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, left_bound)); - else if (overlap_right) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, right_bound)); - geometry_msgs::msg::Pose p; - p.position.x = point.x(); - p.position.y = point.y(); - const auto length = lanelet::utils::getArcCoordinates(trajectory_lanelets, p).length; - if (length > overlap.max_arc_length) { - overlap.max_arc_length = length; - overlap.max_overlap_point = point; - } - if (length < overlap.min_arc_length) { - overlap.min_arc_length = length; - overlap.min_overlap_point = point; - } - } - } - return overlap; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params) -{ - OverlapRanges ranges; - OtherLane other_lane(lanelet); - std::vector overlaps; - for (auto i = 0UL; i < trajectory_footprints.size(); ++i) { - const auto overlap = calculate_overlap(trajectory_footprints[i], trajectory_lanelets, lanelet); - const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; - if (has_overlap) { // open/update the range - overlaps.push_back(overlap); - if (!other_lane.range_is_open) { - other_lane.first_range_bound.index = i; - other_lane.first_range_bound.point = overlap.min_overlap_point; - other_lane.first_range_bound.arc_length = - overlap.min_arc_length - params.overlap_extra_length; - other_lane.first_range_bound.inside_distance = overlap.inside_distance; - other_lane.range_is_open = true; - } - other_lane.last_range_bound.index = i; - other_lane.last_range_bound.point = overlap.max_overlap_point; - other_lane.last_range_bound.arc_length = overlap.max_arc_length + params.overlap_extra_length; - other_lane.last_range_bound.inside_distance = overlap.inside_distance; - } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - } - // close the range if it is still open - if (other_lane.range_is_open) { - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - return ranges; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params) -{ - OverlapRanges ranges; - for (auto & lanelet : lanelets) { - const auto lanelet_ranges = - calculate_overlapping_ranges(trajectory_footprints, trajectory_lanelets, lanelet, params); - ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); - } - return ranges; -} - -} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp deleted file mode 100644 index bc5872db16e03..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef OVERLAPPING_RANGE_HPP_ -#define OVERLAPPING_RANGE_HPP_ - -#include "types.hpp" - -#include - -#include - -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ - -/// @brief calculate the overlap between the given footprint and lanelet -/// @param [in] path_footprint footprint used to calculate the overlap -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelet lanelet used to calculate the overlap -/// @return the found overlap between the footprint and the lanelet -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & trajectory_footprint, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet); -/// @brief calculate the overlapping ranges between the trajectory footprints and a lanelet -/// @param [in] trajectory_footprints footprints used to calculate the overlaps -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelet lanelet used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelet -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params); -/// @brief calculate the overlapping ranges between the trajectory footprints and some lanelets -/// @param [in] trajectory_footprints footprints used to calculate the overlaps -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelets lanelets used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelets, sorted by -/// increasing arc length along the trajectory -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params); -} // namespace autoware::motion_velocity_planner::out_of_lane - -#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index 96c54064e296c..c3714c5609135 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -28,52 +28,45 @@ #include #include +#include -#include -#include -#include #include +#include #include #include #include namespace autoware::motion_velocity_planner::out_of_lane { -using autoware_planning_msgs::msg::TrajectoryPoint; +using Polygons = boost::geometry::model::multi_polygon; -/// @brief parameters for the "out of lane" module +/// @brief parameters for the out_of_lane module struct PlannerParam { std::string mode; // mode used to consider a conflict with an object bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps // another lane + double max_arc_length; // [m] maximum arc length along the trajectory to check for collision - double time_threshold; // [s](mode="threshold") objects time threshold - double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range - double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range + double time_threshold; // [s](mode="threshold") objects time threshold double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object - double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - bool objects_use_predicted_paths; // whether to use the objects' predicted paths bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red // lights' stop lines - double objects_min_vel; // [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // minimum confidence to consider a predicted path - double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in - // the other lane + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path bool objects_ignore_behind_ego; // if true, objects behind the ego vehicle are ignored - double overlap_extra_length; // [m] extra length to add around an overlap range - double overlap_min_dist; // [m] min distance inside another lane to consider an overlap - // action to insert in the trajectory if an object causes a conflict at an overlap - bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle - double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle - double slow_velocity; - double slow_dist_threshold; - double stop_dist_threshold; - double precision; // [m] precision when inserting a stop pose in the trajectory - double min_decision_duration; // [s] minimum duration needed a decision can be canceled + // action to insert in the trajectory if an object causes a collision at an overlap + double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle + double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle + double slow_velocity; // [m/s] slowdown velocity + double stop_dist_threshold; // [m] if a collision is detected bellow this distance ahead of ego, + // try to insert a stop point + double precision; // [m] precision when inserting a stop pose in the trajectory + double + min_decision_duration; // [s] duration needed before a stop or slowdown point can be removed + // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -85,98 +78,6 @@ struct PlannerParam double extra_left_offset; // [m] extra left distance }; -struct EnterExitTimes -{ - double enter_time{}; - double exit_time{}; -}; -struct RangeTimes -{ - EnterExitTimes ego{}; - EnterExitTimes object{}; -}; - -/// @brief action taken by the "out of lane" module -struct Slowdown -{ - size_t target_trajectory_idx{}; // we want to slowdown before this trajectory index - double velocity{}; // desired slow down velocity - lanelet::ConstLanelet lane_to_avoid{}; // we want to slowdown before entering this lane -}; -/// @brief slowdown to insert in a trajectory -struct SlowdownToInsert -{ - Slowdown slowdown{}; - autoware_planning_msgs::msg::TrajectoryPoint point{}; -}; - -/// @brief bound of an overlap range (either the first, or last bound) -struct RangeBound -{ - size_t index{}; - lanelet::BasicPoint2d point{}; - double arc_length{}; - double inside_distance{}; -}; - -/// @brief representation of an overlap between the ego footprint and some other lane -struct Overlap -{ - double inside_distance = 0.0; ///!< distance inside the overlap - double min_arc_length = std::numeric_limits::infinity(); - double max_arc_length = 0.0; - lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length - lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length -}; - -/// @brief range along the trajectory where ego overlaps another lane -struct OverlapRange -{ - lanelet::ConstLanelet lane{}; - size_t entering_trajectory_idx{}; - size_t exiting_trajectory_idx{}; - lanelet::BasicPoint2d entering_point{}; // pose of the overlapping point closest along the lane - lanelet::BasicPoint2d exiting_point{}; // pose of the overlapping point furthest along the lane - double inside_distance{}; // [m] how much ego footprint enters the lane - mutable struct - { - std::vector overlaps{}; - std::optional decision{}; - RangeTimes times{}; - std::optional object{}; - } debug; -}; -using OverlapRanges = std::vector; -/// @brief representation of a lane and its current overlap range -struct OtherLane -{ - bool range_is_open = false; - RangeBound first_range_bound{}; - RangeBound last_range_bound{}; - lanelet::ConstLanelet lanelet{}; - lanelet::BasicPolygon2d polygon{}; - - explicit OtherLane(lanelet::ConstLanelet ll) : lanelet(std::move(ll)) - { - polygon = lanelet.polygon2d().basicPolygon(); - } - - [[nodiscard]] OverlapRange close_range() - { - OverlapRange range; - range.lane = lanelet; - range.entering_trajectory_idx = first_range_bound.index; - range.entering_point = first_range_bound.point; - range.exiting_trajectory_idx = last_range_bound.index; - range.exiting_point = last_range_bound.point; - range.inside_distance = - std::max(first_range_bound.inside_distance, last_range_bound.inside_distance); - range_is_open = false; - last_range_bound = {}; - return range; - } -}; - namespace bgi = boost::geometry::index; struct StopLine { @@ -185,68 +86,52 @@ struct StopLine }; using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; -using OutAreaRtree = bgi::rtree, bgi::rstar<16>>; +using OutAreaNode = std::pair; +using OutAreaRtree = bgi::rtree>; /// @brief data related to the ego vehicle struct EgoData { - std::vector trajectory_points{}; + std::vector trajectory_points; + geometry_msgs::msg::Pose pose; size_t first_trajectory_idx{}; - double velocity{}; // [m/s] - double max_decel{}; // [m/s²] - geometry_msgs::msg::Pose pose{}; + double longitudinal_offset_to_first_trajectory_index{}; + double min_stop_distance{}; + double min_slowdown_distance{}; + double min_stop_arc_length{}; + + Polygons drivable_lane_polygons; + + lanelet::BasicPolygon2d current_footprint; + std::vector trajectory_footprints; + StopLinesRtree stop_lines_rtree; }; -/// @brief data needed to make decisions -struct DecisionInputs +struct OutOfLanePoint { - OverlapRanges ranges; - EgoData ego_data; - autoware_perception_msgs::msg::PredictedObjects objects; - std::shared_ptr route_handler; - lanelet::ConstLanelets lanelets; + size_t trajectory_index; + lanelet::BasicPolygon2d outside_ring; + std::set collision_times; + std::optional min_object_arrival_time; + std::optional max_object_arrival_time; + std::optional ttc; + lanelet::ConstLanelets overlapped_lanelets; + bool to_avoid = false; +}; +struct OutOfLaneData +{ + std::vector outside_points; + OutAreaRtree outside_areas_rtree; }; /// @brief debug data struct DebugData { - std::vector footprints; - std::vector slowdowns; - geometry_msgs::msg::Pose ego_pose; - OverlapRanges ranges; - lanelet::BasicPolygon2d current_footprint; - lanelet::ConstLanelets current_overlapped_lanelets; - lanelet::ConstLanelets trajectory_lanelets; - lanelet::ConstLanelets ignored_lanelets; - lanelet::ConstLanelets other_lanelets; - std::vector trajectory_points; - size_t first_trajectory_idx; - - size_t prev_footprints = 0; - size_t prev_slowdowns = 0; - size_t prev_ranges = 0; - size_t prev_current_overlapped_lanelets = 0; - size_t prev_ignored_lanelets = 0; - size_t prev_trajectory_lanelets = 0; - size_t prev_other_lanelets = 0; - void reset_data() - { - prev_footprints = footprints.size(); - footprints.clear(); - prev_slowdowns = slowdowns.size(); - slowdowns.clear(); - prev_ranges = ranges.size(); - ranges.clear(); - prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); - current_overlapped_lanelets.clear(); - prev_ignored_lanelets = ignored_lanelets.size(); - ignored_lanelets.clear(); - prev_trajectory_lanelets = trajectory_lanelets.size(); - trajectory_lanelets.clear(); - prev_other_lanelets = other_lanelets.size(); - other_lanelets.clear(); - } + size_t prev_out_of_lane_areas = 0; + size_t prev_ttcs = 0; + size_t prev_objects = 0; + size_t prev_stop_line = 0; }; } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt index f1eb7ff047fdc..ffc06aa8cc2f8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt @@ -5,9 +5,20 @@ find_package(autoware_cmake REQUIRED) autoware_package() -# ament_auto_add_library(${PROJECT_NAME}_lib SHARED -# DIRECTORY src -# ) +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_collision_checker.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME}_lib + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() ament_auto_package(INSTALL_TO_SHARE include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp new file mode 100644 index 0000000000000..bf471c62af969 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp @@ -0,0 +1,69 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +namespace bgi = boost::geometry::index; +using RtreeNode = std::pair; +using Rtree = bgi::rtree>; + +/// @brief collision as a trajectory index and corresponding collision points +struct Collision +{ + size_t trajectory_index{}; + autoware::universe_utils::MultiPoint2d collision_points; +}; + +/// @brief collision checker for a trajectory as a sequence of 2D footprints +class CollisionChecker +{ + const autoware::universe_utils::MultiPolygon2d trajectory_footprints_; + std::shared_ptr rtree_; + +public: + /// @brief construct the collisions checker + /// @param trajectory_footprints footprints of the trajectory to be used for collision checking + /// @details internally, the rtree is built with the packing algorithm + explicit CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints); + + /// @brief efficiently calculate collisions with a geometry + /// @tparam Geometry boost geometry type + /// @param geometry geometry to check for collisions + /// @return collisions between the trajectory footprints and the input geometry + template + [[nodiscard]] std::vector get_collisions(const Geometry & geometry) const; + + /// @brief direct access to the rtree storing the polygon footprints + /// @return rtree of the polygon footprints + [[nodiscard]] std::shared_ptr get_rtree() const { return rtree_; } + + /// @brief get the size of the trajectory used by this collision checker + [[nodiscard]] size_t trajectory_size() const { return trajectory_footprints_.size(); } +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp index 5dbb872d99ca9..a96587c4465d6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp @@ -15,7 +15,10 @@ #ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#include +#include #include +#include #include #include @@ -37,12 +40,9 @@ #include #include -#include -#include #include #include #include -#include namespace autoware::motion_velocity_planner { @@ -59,11 +59,11 @@ struct PlannerData } // msgs from callbacks that are used for data-ready - nav_msgs::msg::Odometry current_odometry{}; - geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration{}; - autoware_perception_msgs::msg::PredictedObjects predicted_objects{}; - pcl::PointCloud no_ground_pointcloud{}; - nav_msgs::msg::OccupancyGrid occupancy_grid{}; + nav_msgs::msg::Odometry current_odometry; + geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; + autoware_perception_msgs::msg::PredictedObjects predicted_objects; + pcl::PointCloud no_ground_pointcloud; + nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; // nearest search @@ -79,7 +79,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_; // parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; @@ -88,7 +88,7 @@ struct PlannerData *@brief queries the traffic signal information of given Id. if keep_last_observation = true, *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation */ - std::optional get_traffic_signal( + [[nodiscard]] std::optional get_traffic_signal( const lanelet::Id id, const bool keep_last_observation = false) const { const auto & traffic_light_id_map = @@ -98,6 +98,15 @@ struct PlannerData } return std::make_optional(traffic_light_id_map.at(id)); } + + [[nodiscard]] std::optional calculate_min_deceleration_distance( + const double target_velocity) const + { + return motion_utils::calcDecelDistWithJerkAndAccConstraints( + current_odometry.twist.twist.linear.x, target_velocity, + current_acceleration.accel.accel.linear.x, velocity_smoother_->getMinDecel(), + std::abs(velocity_smoother_->getMinJerk()), velocity_smoother_->getMinJerk()); + } }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp new file mode 100644 index 0000000000000..6e84b63a3c7fc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp @@ -0,0 +1,70 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_velocity_planner_common/collision_checker.hpp" + +namespace autoware::motion_velocity_planner +{ +CollisionChecker::CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints) +: trajectory_footprints_(std::move(trajectory_footprints)) +{ + std::vector nodes; + nodes.reserve(trajectory_footprints_.size()); + for (auto i = 0UL; i < trajectory_footprints_.size(); ++i) { + nodes.emplace_back( + boost::geometry::return_envelope(trajectory_footprints_[i]), + i); + } + rtree_ = std::make_shared(nodes.begin(), nodes.end()); +} + +template +std::vector CollisionChecker::get_collisions(const Geometry & geometry) const +{ + std::vector collisions; + std::vector approximate_results; + autoware::universe_utils::MultiPoint2d intersections; + ; + rtree_->query(bgi::intersects(geometry), std::back_inserter(approximate_results)); + for (const auto & result : approximate_results) { + intersections.clear(); + boost::geometry::intersection(trajectory_footprints_[result.second], geometry, intersections); + if (!intersections.empty()) { + Collision c; + c.trajectory_index = result.second; + c.collision_points = intersections; + collisions.push_back(c); + } + } + return collisions; +} + +template std::vector CollisionChecker::get_collisions( + const autoware::universe_utils::Point2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware::universe_utils::Line2d & geometry) const; +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiPolygon2d & geometry) const; + +// @warn Multi geometries usually lead to very inefficient queries +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiPoint2d & geometry) const; +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiLineString2d & geometry) const; +template std::vector CollisionChecker::get_collisions< + autoware::universe_utils::Polygon2d>(const autoware::universe_utils::Polygon2d & geometry) const; +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp new file mode 100644 index 0000000000000..df813db336a9d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp @@ -0,0 +1,176 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_velocity_planner_common/collision_checker.hpp" + +#include + +#include + +#include +#include + +using autoware::motion_velocity_planner::CollisionChecker; +using autoware::universe_utils::Line2d; +using autoware::universe_utils::MultiLineString2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +Point2d random_point() +{ + static std::random_device r; + static std::default_random_engine e1(r()); + static std::uniform_real_distribution uniform_dist(-100, 100); + return {uniform_dist(e1), uniform_dist(e1)}; +} + +Line2d random_line() +{ + const auto point = random_point(); + const auto point2 = Point2d{point.x() + 1, point.y() + 1}; + const auto point3 = Point2d{point2.x() - 1, point2.y() + 1}; + const auto point4 = Point2d{point3.x() + 1, point3.y() + 1}; + return {point, point2, point3, point4}; +} + +Polygon2d random_polygon() +{ + Polygon2d polygon; + const auto point = random_point(); + const auto point2 = Point2d{point.x() + 1, point.y() + 4}; + const auto point3 = Point2d{point.x() + 4, point.y() + 4}; + const auto point4 = Point2d{point.x() + 3, point.y() + 1}; + polygon.outer() = {point, point2, point3, point4, point}; + return polygon; +} + +bool all_within(const MultiPoint2d & pts1, const MultiPoint2d & pts2) +{ + // results from the collision checker and the direct checks can have some small precision errors + constexpr auto eps = 1e-3; + for (const auto & p1 : pts1) { + bool found = false; + for (const auto & p2 : pts2) { + if (boost::geometry::comparable_distance(p1, p2) < eps) { + found = true; + break; + } + } + if (!found) return false; + } + return true; +} + +TEST(TestCollisionChecker, Benchmark) +{ + constexpr auto nb_ego_footprints = 1000; + constexpr auto nb_obstacles = 1000; + MultiPolygon2d ego_footprints; + ego_footprints.reserve(nb_ego_footprints); + for (auto i = 0; i < nb_ego_footprints; ++i) { + ego_footprints.push_back(random_polygon()); + } + const auto cc_constr_start = std::chrono::system_clock::now(); + CollisionChecker collision_checker(ego_footprints); + const auto cc_constr_end = std::chrono::system_clock::now(); + const auto cc_constr_ns = + std::chrono::duration_cast(cc_constr_end - cc_constr_start).count(); + std::printf( + "Collision checker construction (with %d footprints): %ld ns\n", nb_ego_footprints, + cc_constr_ns); + MultiPolygon2d poly_obstacles; + MultiPoint2d point_obstacles; + MultiLineString2d line_obstacles; + for (auto i = 0; i < nb_obstacles; ++i) { + poly_obstacles.push_back(random_polygon()); + point_obstacles.push_back(random_point()); + line_obstacles.push_back(random_line()); + } + const auto check_obstacles_one_by_one = [&](const auto & obstacles) { + std::chrono::nanoseconds collision_checker_ns{}; + std::chrono::nanoseconds naive_ns{}; + for (const auto & obs : obstacles) { + const auto cc_start = std::chrono::system_clock::now(); + const auto collisions = collision_checker.get_collisions(obs); + MultiPoint2d cc_collision_points; + for (const auto & c : collisions) + cc_collision_points.insert( + cc_collision_points.end(), c.collision_points.begin(), c.collision_points.end()); + const auto cc_end = std::chrono::system_clock::now(); + const auto naive_start = std::chrono::system_clock::now(); + MultiPoint2d naive_collision_points; + for (const auto & ego_footprint : ego_footprints) { + MultiPoint2d points; + boost::geometry::intersection(ego_footprint, obs, points); + naive_collision_points.insert(naive_collision_points.end(), points.begin(), points.end()); + } + const auto naive_end = std::chrono::system_clock::now(); + const auto equal = all_within(cc_collision_points, naive_collision_points) && + all_within(naive_collision_points, cc_collision_points); + EXPECT_TRUE(equal); + if (!equal) { + std::cout << "cc: " << boost::geometry::wkt(cc_collision_points) << std::endl; + std::cout << "naive: " << boost::geometry::wkt(naive_collision_points) << std::endl; + } + collision_checker_ns += + std::chrono::duration_cast(cc_end - cc_start); + naive_ns += std::chrono::duration_cast(naive_end - naive_start); + } + std::printf("%20s%10ld ns\n", "collision checker : ", collision_checker_ns.count()); + std::printf("%20s%10ld ns\n", "naive : ", naive_ns.count()); + }; + const auto check_obstacles = [&](const auto & obstacles) { + std::chrono::nanoseconds collision_checker_ns{}; + std::chrono::nanoseconds naive_ns{}; + const auto cc_start = std::chrono::system_clock::now(); + const auto collisions = collision_checker.get_collisions(obstacles); + MultiPoint2d cc_collision_points; + for (const auto & c : collisions) + cc_collision_points.insert( + cc_collision_points.end(), c.collision_points.begin(), c.collision_points.end()); + const auto cc_end = std::chrono::system_clock::now(); + const auto naive_start = std::chrono::system_clock::now(); + MultiPoint2d naive_collision_points; + boost::geometry::intersection(ego_footprints, obstacles, naive_collision_points); + const auto naive_end = std::chrono::system_clock::now(); + const auto equal = all_within(cc_collision_points, naive_collision_points) && + all_within(naive_collision_points, cc_collision_points); + EXPECT_TRUE(equal); + if (!equal) { + std::cout << "cc: " << boost::geometry::wkt(cc_collision_points) << std::endl; + std::cout << "naive: " << boost::geometry::wkt(naive_collision_points) << std::endl; + } + collision_checker_ns += std::chrono::duration_cast(cc_end - cc_start); + naive_ns += std::chrono::duration_cast(naive_end - naive_start); + std::printf("%20s%10ld ns\n", "collision checker : ", collision_checker_ns.count()); + std::printf("%20s%10ld ns\n", "naive : ", naive_ns.count()); + }; + + std::cout << "* check one by one\n"; + std::printf("%d Polygons:\n", nb_obstacles); + check_obstacles_one_by_one(poly_obstacles); + std::printf("%d Lines:\n", nb_obstacles); + check_obstacles_one_by_one(line_obstacles); + std::printf("%d Points:\n", nb_obstacles); + check_obstacles_one_by_one(point_obstacles); + std::cout << "* check all at once\n"; + std::printf("%d Polygons:\n", nb_obstacles); + check_obstacles(poly_obstacles); + std::printf("%d Lines:\n", nb_obstacles); + check_obstacles(line_obstacles); + std::printf("%d Points:\n", nb_obstacles); + check_obstacles(point_obstacles); +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 76b64c0a4d5e2..9a08616b8fe7b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -83,8 +82,11 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); - processing_time_publisher_ = this->create_publisher( - "~/debug/total_time/processing_time_ms", 1); + processing_time_publisher_ = + this->create_publisher("~/debug/processing_time_ms", 1); + debug_viz_pub_ = + this->create_publisher("~/debug/markers", 1); + diagnostics_pub_ = this->create_publisher("/diagnostics", 10); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); @@ -98,7 +100,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. - if (name == "") { + if (name.empty()) { break; } planner_manager_.load_module_plugin(*this, name); @@ -134,7 +136,7 @@ bool MotionVelocityPlannerNode::update_planner_data() const auto check_with_log = [&](const auto ptr, const auto & log) { constexpr auto throttle_duration = 3000; // [ms] if (!ptr) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log); + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "%s", log); is_ready = false; return false; } @@ -277,7 +279,6 @@ void MotionVelocityPlannerNode::on_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()}; - auto output_trajectory_msg = generate_trajectory(input_trajectory_points); output_trajectory_msg.header = input_trajectory_msg->header; processing_times["generate_trajectory"] = stop_watch.toc(true); @@ -293,6 +294,13 @@ void MotionVelocityPlannerNode::on_trajectory( processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); + + std::shared_ptr diagnostics = + planner_manager_.get_diagnostics(get_clock()->now()); + if (!diagnostics->status.empty()) { + diagnostics_pub_->publish(*diagnostics); + } + planner_manager_.clear_diagnostics(); } void MotionVelocityPlannerNode::insert_stop( @@ -325,7 +333,8 @@ void MotionVelocityPlannerNode::insert_slowdown( autoware::motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); if (from_insert_idx && to_insert_idx) { for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) - trajectory.points[idx].longitudinal_velocity_mps = slowdown_interval.velocity; + trajectory.points[idx].longitudinal_velocity_mps = + static_cast(slowdown_interval.velocity); } else { RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); } @@ -360,16 +369,14 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware::motion_velocity_planner::TrajectoryPoints clipped; autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; clipped.insert( - clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + clipped.end(), std::next(traj_resampled.begin(), static_cast(traj_resampled_closest)), + traj_resampled.end()); if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } - traj_smoothed.insert( - traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); - if (external_v_limit) { autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + 0LU, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; } @@ -377,13 +384,21 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) { + universe_utils::StopWatch stop_watch; autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; - if (smooth_velocity_before_planning_) + if (smooth_velocity_before_planning_) { + stop_watch.tic("smooth"); input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); - const auto resampled_trajectory = - autoware::motion_utils::resampleTrajectory(output_trajectory_msg, 0.5); - + RCLCPP_DEBUG(get_logger(), "smooth: %2.2f us", stop_watch.toc("smooth")); + } + autoware_planning_msgs::msg::Trajectory smooth_velocity_trajectory; + smooth_velocity_trajectory.points = { + input_trajectory_points.begin(), input_trajectory_points.end()}; + auto resampled_trajectory = + autoware::motion_utils::resampleTrajectory(smooth_velocity_trajectory, 0.5); + motion_utils::calculate_time_from_start( + resampled_trajectory.points, planner_data_.current_odometry.pose.pose.position); const auto planning_results = planner_manager_.plan_velocities( resampled_trajectory.points, std::make_shared(planner_data_)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 8debb9cbedf00..7a7fb5790ace1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -43,6 +43,8 @@ #include #include +using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + namespace autoware::motion_velocity_planner { using autoware_map_msgs::msg::LaneletMapBin; @@ -97,9 +99,11 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; - autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; + rclcpp::Publisher::SharedPtr diagnostics_pub_; // parameters rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index 66063fcbaebca..04641e0cea6bb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -71,13 +71,60 @@ void MotionVelocityPlannerManager::update_module_parameters( for (auto & plugin : loaded_plugins_) plugin->update_parameters(parameters); } +std::shared_ptr MotionVelocityPlannerManager::make_diagnostic( + const std::string & module_name, const std::string & reason, const bool is_decided) +{ + auto status = std::make_shared(); + status->level = status->OK; + status->name = module_name + '.' + reason; + diagnostic_msgs::msg::KeyValue key_value; + { + // Decision + key_value.key = "decision"; + if (is_decided) + key_value.value = reason; + else + key_value.value = "none"; + status->values.push_back(key_value); + } + // Add other information to the status if necessary in the future. + return status; +} + +std::shared_ptr MotionVelocityPlannerManager::get_diagnostics( + const rclcpp::Time & current_time) const +{ + auto diagnostics = std::make_shared(); + + for (const auto & ds_ptr : diagnostics_) { + if ( + !ds_ptr->values.empty() && ds_ptr->values[0].key == "decision" && + ds_ptr->values[0].value != "none") { + diagnostics->status.push_back(*ds_ptr); + } + } + diagnostics->header.stamp = current_time; + diagnostics->header.frame_id = "map"; + return diagnostics; +} + std::vector MotionVelocityPlannerManager::plan_velocities( const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { std::vector results; - for (auto & plugin : loaded_plugins_) - results.push_back(plugin->plan(ego_trajectory_points, planner_data)); + for (auto & plugin : loaded_plugins_) { + VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); + results.push_back(res); + + const auto stop_reason_diag = + make_diagnostic(plugin->get_module_name(), "stop", res.stop_points.size() > 0); + diagnostics_.push_back(stop_reason_diag); + + const auto slow_down_reason_diag = + make_diagnostic(plugin->get_module_name(), "slow_down", res.slowdown_intervals.size() > 0); + diagnostics_.push_back(slow_down_reason_diag); + } return results; } } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp index 20165d359f669..ac2e421d30cb6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -35,6 +36,9 @@ #include #include +using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; +using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + namespace autoware::motion_velocity_planner { class MotionVelocityPlannerManager @@ -48,7 +52,14 @@ class MotionVelocityPlannerManager const std::vector & ego_trajectory_points, const std::shared_ptr planner_data); + // Diagnostic + std::shared_ptr make_diagnostic( + const std::string & module_name, const std::string & reason, const bool is_decided = true); + std::shared_ptr get_diagnostics(const rclcpp::Time & current_time) const; + void clear_diagnostics() { diagnostics_.clear(); } + private: + std::vector> diagnostics_; pluginlib::ClassLoader plugin_loader_; std::vector> loaded_plugins_; };