diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 3f47b97aa0adf..e6b946a130682 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1622,7 +1622,8 @@ std::pair separateObjectsByPath( double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; - const auto offset = p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + const auto offset = + 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; max_offset = std::max(max_offset, offset); } diff --git a/planning/freespace_planning_algorithms/test/debug_plot.py b/planning/freespace_planning_algorithms/test/debug_plot.py index bc96963f09dcb..a34fd745af047 100755 --- a/planning/freespace_planning_algorithms/test/debug_plot.py +++ b/planning/freespace_planning_algorithms/test/debug_plot.py @@ -223,6 +223,6 @@ def create_concat_png(src_list, dest, is_horizontal): plt.savefig(file_name) print("saved to {}".format(file_name)) - algowise_summary_file = os.path.join("/tmp", "summary-{}.png".format(algo_name)) + algo_summary_file = os.path.join("/tmp", "summary-{}.png".format(algo_name)) if concat: - create_concat_png(algo_png_images, algowise_summary_file, True) + create_concat_png(algo_png_images, algo_summary_file, True) diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 9b5775642424c..6c0d2e86943dd 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -38,9 +38,8 @@ Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_in void calcCurvature(const Trajectory & trajectory, std::vector & curvatures); -// cspell: ignore steerings void calcSteeringAngles( - const Trajectory & trajectory, const double wheelbase, std::vector & steerings); + const Trajectory & trajectory, const double wheelbase, std::vector & steering_array); std::pair calcMaxCurvature(const Trajectory & trajectory); diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index b896b3f6d8979..e4c882cfaedb9 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -216,7 +216,7 @@ std::pair calcMaxRelativeAngles(const Trajectory & trajectory) } void calcSteeringAngles( - const Trajectory & trajectory, const double wheelbase, std::vector & steerings) + const Trajectory & trajectory, const double wheelbase, std::vector & steering_array) { const auto curvatureToSteering = [](const auto k, const auto wheelbase) { return std::atan(k * wheelbase); @@ -225,19 +225,19 @@ void calcSteeringAngles( std::vector curvatures; calcCurvature(trajectory, curvatures); - steerings.clear(); + steering_array.clear(); for (const auto k : curvatures) { - steerings.push_back(curvatureToSteering(k, wheelbase)); + steering_array.push_back(curvatureToSteering(k, wheelbase)); } } std::pair calcMaxSteeringAngles( const Trajectory & trajectory, const double wheelbase) { - std::vector steerings; - calcSteeringAngles(trajectory, wheelbase, steerings); + std::vector steering_array; + calcSteeringAngles(trajectory, wheelbase, steering_array); - return getAbsMaxValAndIdx(steerings); + return getAbsMaxValAndIdx(steering_array); } std::pair calcMaxSteeringRates( @@ -247,8 +247,8 @@ std::pair calcMaxSteeringRates( return {0.0, 0}; } - std::vector steerings; - calcSteeringAngles(trajectory, wheelbase, steerings); + std::vector steering_array; + calcSteeringAngles(trajectory, wheelbase, steering_array); double max_steering_rate = 0.0; size_t max_index = 0; @@ -259,8 +259,8 @@ std::pair calcMaxSteeringRates( const auto v = 0.5 * (p_next.longitudinal_velocity_mps + p_prev.longitudinal_velocity_mps); const auto dt = delta_s / std::max(v, 1.0e-5); - const auto steer_prev = steerings.at(i); - const auto steer_next = steerings.at(i + 1); + const auto steer_prev = steering_array.at(i); + const auto steer_next = steering_array.at(i + 1); const auto steer_rate = (steer_next - steer_prev) / dt; takeBigger(max_steering_rate, max_index, steer_rate, i); diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp index 89d91bd3110f5..93c6ec6b87f36 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp +++ b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp @@ -50,11 +50,11 @@ class Bezier explicit Bezier(const std::vector & control_points); /// @brief return the control points [[nodiscard]] const Eigen::Matrix & getControlPoints() const; - /// @brief return the curve in cartersian frame with the desired resolution + /// @brief return the curve in cartesian frame with the desired resolution [[nodiscard]] std::vector cartesian(const double resolution) const; - /// @brief return the curve in cartersian frame with the desired number of points + /// @brief return the curve in cartesian frame with the desired number of points [[nodiscard]] std::vector cartesian(const int nb_points) const; - /// @brief return the curve in cartersian frame (including angle) with the desired number of + /// @brief return the curve in cartesian frame (including angle) with the desired number of /// points [[nodiscard]] std::vector cartesianWithHeading(const int nb_points) const; /// @brief calculate the curve value for the given parameter t @@ -65,7 +65,7 @@ class Bezier [[nodiscard]] Eigen::Vector2d velocity(const double t) const; /// @brief calculate the acceleration (2nd derivative) for the given parameter t [[nodiscard]] Eigen::Vector2d acceleration(const double t) const; - /// @breif return the heading (in radians) of the tangent for the given parameter t + /// @brief return the heading (in radians) of the tangent for the given parameter t [[nodiscard]] double heading(const double t) const; /// @brief calculate the curvature for the given parameter t [[nodiscard]] double curvature(const double t) const; diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp index 9dea7d34742dc..624cd585b0a72 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp +++ b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp @@ -33,6 +33,7 @@ struct SamplingParameters double mk_max; // Minimum normalized curvature vector magnitude }; /// @brief sample Bezier curves given an initial and final state and sampling parameters +// cspell: ignore Artuñedoet /// @details from Section IV of A. Artuñedoet al.: Real-Time Motion Planning Approach for Automated /// Driving in Urban Environments std::vector sample( diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp index 848d8f11938cf..c44cb5d814d71 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp +++ b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp @@ -126,15 +126,15 @@ struct Trajectory : sampler_common::Trajectory [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override { - auto * subtraj = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx)); + auto * sub_trajectory = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx)); assert(to_idx >= from_idx); - subtraj->reserve(to_idx - from_idx); + sub_trajectory->reserve(to_idx - from_idx); const auto copy_subset = [&](const auto & from, auto & to) { to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx)); }; - copy_subset(frenet_points, subtraj->frenet_points); - return subtraj; + copy_subset(frenet_points, sub_trajectory->frenet_points); + return sub_trajectory; }; }; diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp index ebd628793fa24..3368c49459a55 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -183,17 +183,17 @@ void calculateCartesian( for (const auto & fp : trajectory.frenet_points) trajectory.points.push_back(reference.cartesian(fp)); calculateLengthsAndYaws(trajectory); - std::vector dyaws; - dyaws.reserve(trajectory.yaws.size()); + std::vector d_yaws; + d_yaws.reserve(trajectory.yaws.size()); for (size_t i = 0; i + 1 < trajectory.yaws.size(); ++i) - dyaws.push_back(autoware::common::helper_functions::wrap_angle( + d_yaws.push_back(autoware::common::helper_functions::wrap_angle( trajectory.yaws[i + 1] - trajectory.yaws[i])); - dyaws.push_back(0.0); + d_yaws.push_back(0.0); // Calculate curvatures for (size_t i = 1; i < trajectory.yaws.size(); ++i) { const auto curvature = trajectory.lengths[i] == trajectory.lengths[i - 1] ? 0.0 - : dyaws[i] / (trajectory.lengths[i] - trajectory.lengths[i - 1]); + : d_yaws[i] / (trajectory.lengths[i] - trajectory.lengths[i - 1]); trajectory.curvatures.push_back(curvature); } const auto last_curvature = trajectory.curvatures.empty() ? 0.0 : trajectory.curvatures.back(); @@ -205,7 +205,7 @@ void calculateCartesian( const auto s_acc = trajectory.longitudinal_polynomial->acceleration(time); const auto d_vel = trajectory.lateral_polynomial->velocity(time); const auto d_acc = trajectory.lateral_polynomial->acceleration(time); - Eigen::Rotation2D rotation(dyaws[i]); + Eigen::Rotation2D rotation(d_yaws[i]); Eigen::Vector2d vel_vector{s_vel, d_vel}; Eigen::Vector2d acc_vector{s_acc, d_acc}; const auto vel = rotation * vel_vector; diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index 533c79252dfe4..2d7fe52e5dcad 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -2,7 +2,7 @@ ## Purpose -This package implements a node that uses sampling based planning to generate a drivable trajectry. +This package implements a node that uses sampling based planning to generate a drivable trajectory. ## Feature @@ -33,7 +33,7 @@ Note that the velocity is just taken over from the input path. ## Algorithm -Sampling based planning is decomposed into 3 successives steps: +Sampling based planning is decomposed into 3 successive steps: 1. Sampling: candidate trajectories are generated. 2. Pruning: invalid candidates are discarded. @@ -65,7 +65,7 @@ Each soft constraint is associated with a weight to allow tuning of the preferen ## Limitations The quality of the candidates generated with polynomials in frenet frame greatly depend on the reference path. -If the reference path is not smooth, the resulting candidates will probably be undrivable. +If the reference path is not smooth, the resulting candidates will probably be undriveable. Failure to find a valid trajectory current results in a suddenly stopping trajectory. diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index 32c7cebc39b5f..588f29b06c40d 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -458,7 +458,7 @@ std::vector PathSampler::generatePath(const PlannerData & plann prev_path_ = selected_path; } else { RCLCPP_WARN( - get_logger(), "No valid path found (out of %lu) outputing %s\n", candidate_paths.size(), + get_logger(), "No valid path found (out of %lu) outputting %s\n", candidate_paths.size(), prev_path_ ? "previous path" : "stopping path"); int coll = 0; int da = 0; diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp index b574f87e319fb..4ae6382b82383 100644 --- a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp +++ b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp @@ -68,11 +68,11 @@ std::vector generateBezierPaths( target_state.pose = path_spline.cartesian({target_s, 0}); target_state.curvature = path_spline.curvature(target_s); target_state.heading = path_spline.yaw(target_s); - const auto beziers = + const auto bezier_samples = bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); const auto step = std::min(0.1, params.sampling.resolution / target_length); - for (const auto & bezier : beziers) { + for (const auto & bezier : bezier_samples) { sampler_common::Path path; path.lengths.push_back(0.0); for (double t = 0.0; t <= 1.0; t += step) { @@ -107,19 +107,19 @@ std::vector generateFrenetPaths( // Calculate Velocity and acceleration parametrized over arc length // From appendix I of Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame const auto frenet_yaw = initial_state.heading - path_spline.yaw(s); - const auto path_curv = path_spline.curvature(s); + const auto path_curvature = path_spline.curvature(s); const auto delta_s = 0.001; - initial_frenet_state.lateral_velocity = (1 - path_curv * d) * std::tan(frenet_yaw); - const auto path_curv_deriv = (path_spline.curvature(s + delta_s) - path_curv) / delta_s; + initial_frenet_state.lateral_velocity = (1 - path_curvature * d) * std::tan(frenet_yaw); + const auto path_curvature_deriv = (path_spline.curvature(s + delta_s) - path_curvature) / delta_s; const auto cos_yaw = std::cos(frenet_yaw); if (cos_yaw == 0.0) { initial_frenet_state.lateral_acceleration = 0.0; } else { initial_frenet_state.lateral_acceleration = - -(path_curv_deriv * d + path_curv * initial_frenet_state.lateral_velocity) * + -(path_curvature_deriv * d + path_curvature * initial_frenet_state.lateral_velocity) * std::tan(frenet_yaw) + - ((1 - path_curv * d) / (cos_yaw * cos_yaw)) * - (initial_state.curvature * ((1 - path_curv * d) / cos_yaw) - path_curv); + ((1 - path_curvature * d) / (cos_yaw * cos_yaw)) * + (initial_state.curvature * ((1 - path_curvature * d) / cos_yaw) - path_curvature); } return frenet_planner::generatePaths(path_spline, initial_frenet_state, sampling_parameters); } diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp index 25677a5b2d29b..c17f3479932f3 100644 --- a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp @@ -125,11 +125,11 @@ sampler_common::transform::Spline2D preparePathSpline( control_points.transposeInPlace(); const auto nb_knots = path.size() + spline_dim + 3; Eigen::RowVectorXd knots(nb_knots); - constexpr auto repeat_endknots = 3lu; - const auto knot_step = 1.0 / static_cast(nb_knots - 2 * repeat_endknots); + constexpr auto repeat_end_knots = 3lu; + const auto knot_step = 1.0 / static_cast(nb_knots - 2 * repeat_end_knots); auto i = 0lu; - for (; i < repeat_endknots; ++i) knots[i] = 0.0; - for (; i < nb_knots - repeat_endknots; ++i) knots[i] = knots[i - 1] + knot_step; + for (; i < repeat_end_knots; ++i) knots[i] = 0.0; + for (; i < nb_knots - repeat_end_knots; ++i) knots[i] = knots[i - 1] + knot_step; for (; i < nb_knots; ++i) knots[i] = 1.0; const auto spline = Eigen::Spline(knots, control_points); x.reserve(path.size()); diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp index 54e8c457034a9..1cb14fdf56198 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp @@ -227,19 +227,19 @@ struct Trajectory : Path [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override { - auto * subtraj = new Trajectory(*Path::subset(from_idx, to_idx)); + auto * sub_trajectory = new Trajectory(*Path::subset(from_idx, to_idx)); const auto copy_subset = [&](const auto & from, auto & to) { to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx)); }; - copy_subset(longitudinal_velocities, subtraj->longitudinal_velocities); - copy_subset(longitudinal_accelerations, subtraj->longitudinal_accelerations); - copy_subset(lateral_velocities, subtraj->lateral_velocities); - copy_subset(lateral_accelerations, subtraj->lateral_accelerations); - copy_subset(jerks, subtraj->jerks); - copy_subset(times, subtraj->times); - return subtraj; + copy_subset(longitudinal_velocities, sub_trajectory->longitudinal_velocities); + copy_subset(longitudinal_accelerations, sub_trajectory->longitudinal_accelerations); + copy_subset(lateral_velocities, sub_trajectory->lateral_velocities); + copy_subset(lateral_accelerations, sub_trajectory->lateral_accelerations); + copy_subset(jerks, sub_trajectory->jerks); + copy_subset(times, sub_trajectory->times); + return sub_trajectory; } [[nodiscard]] Trajectory resample(const double fixed_interval) const