Merge pull request #755 from tier4/sync-upstream

chore: sync upstream

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -1622,7 +1622,8 @@ std::pair<PredictedObjects, PredictedObjects> 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);
   }
 

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)

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<double> & curvatures);
 
-// cspell: ignore steerings
 void calcSteeringAngles(
-  const Trajectory & trajectory, const double wheelbase, std::vector<double> & steerings);
+  const Trajectory & trajectory, const double wheelbase, std::vector<double> & steering_array);
 
 std::pair<double, size_t> calcMaxCurvature(const Trajectory & trajectory);
 

planning/planning_validator/src/utils.cpp

@@ -216,7 +216,7 @@ std::pair<double, size_t> calcMaxRelativeAngles(const Trajectory & trajectory)
 }
 
 void calcSteeringAngles(
-  const Trajectory & trajectory, const double wheelbase, std::vector<double> & steerings)
+  const Trajectory & trajectory, const double wheelbase, std::vector<double> & steering_array)
 {
   const auto curvatureToSteering = [](const auto k, const auto wheelbase) {
     return std::atan(k * wheelbase);
@@ -225,19 +225,19 @@ void calcSteeringAngles(
   std::vector<double> 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<double, size_t> calcMaxSteeringAngles(
   const Trajectory & trajectory, const double wheelbase)
 {
-  std::vector<double> steerings;
-  calcSteeringAngles(trajectory, wheelbase, steerings);
+  std::vector<double> steering_array;
+  calcSteeringAngles(trajectory, wheelbase, steering_array);
 
-  return getAbsMaxValAndIdx(steerings);
+  return getAbsMaxValAndIdx(steering_array);
 }
 
 std::pair<double, size_t> calcMaxSteeringRates(
@@ -247,8 +247,8 @@ std::pair<double, size_t> calcMaxSteeringRates(
     return {0.0, 0};
   }
 
-  std::vector<double> steerings;
-  calcSteeringAngles(trajectory, wheelbase, steerings);
+  std::vector<double> steering_array;
+  calcSteeringAngles(trajectory, wheelbase, steering_array);
 
   double max_steering_rate = 0.0;
   size_t max_index = 0;
@@ -259,8 +259,8 @@ std::pair<double, size_t> 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);

planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp

@@ -50,11 +50,11 @@ class Bezier
   explicit Bezier(const std::vector<Eigen::Vector2d> & control_points);
   /// @brief return the control points
   [[nodiscard]] const Eigen::Matrix<double, 6, 2> & 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<Eigen::Vector2d> 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<Eigen::Vector2d> 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<Eigen::Vector3d> 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;

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<Bezier> sample(

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;
   };
 };
 

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<double> dyaws;
-    dyaws.reserve(trajectory.yaws.size());
+    std::vector<double> 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;

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.
 

planning/sampling_based_planner/path_sampler/src/node.cpp

@@ -458,7 +458,7 @@ std::vector<TrajectoryPoint> 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;

planning/sampling_based_planner/path_sampler/src/path_generation.cpp

@@ -68,11 +68,11 @@ std::vector<sampler_common::Path> 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<frenet_planner::Path> 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);
 }

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<double>(nb_knots - 2 * repeat_endknots);
+    constexpr auto repeat_end_knots = 3lu;
+    const auto knot_step = 1.0 / static_cast<double>(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<double, 2, spline_dim>(knots, control_points);
     x.reserve(path.size());

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