Review changes

Author: gmajrobotec

common/math/geometry/src/spline/catmull_rom_spline.cpp

@@ -27,7 +27,7 @@ namespace math
 namespace geometry
 {
 auto CatmullRomSpline::getPolygon(
-  const double width, const size_t num_points, const double z_offset)
+  const double width, const std::size_t num_points, const double z_offset)
   -> std::vector<geometry_msgs::msg::Point>
 {
   if (num_points == 0) {
@@ -36,7 +36,7 @@ auto CatmullRomSpline::getPolygon(
   std::vector<geometry_msgs::msg::Point> points;
   std::vector<geometry_msgs::msg::Point> left_bounds = getLeftBounds(width, num_points, z_offset);
   std::vector<geometry_msgs::msg::Point> right_bounds = getRightBounds(width, num_points, z_offset);
-  for (size_t i = 0; i < left_bounds.size() - 1; i++) {
+  for (std::size_t i = 0; i < left_bounds.size() - 1; i++) {
     geometry_msgs::msg::Point pr_0 = right_bounds[i];
     geometry_msgs::msg::Point pl_0 = left_bounds[i];
     geometry_msgs::msg::Point pr_1 = right_bounds[i + 1];
@@ -52,12 +52,12 @@ auto CatmullRomSpline::getPolygon(
 }
 
 auto CatmullRomSpline::getRightBounds(
-  const double width, const size_t num_points, const double z_offset) const
+  const double width, const std::size_t num_points, const double z_offset) const
   -> std::vector<geometry_msgs::msg::Point>
 {
   std::vector<geometry_msgs::msg::Point> points;
   double step_size = getLength() / static_cast<double>(num_points);
-  for (size_t i = 0; i < num_points + 1; i++) {
+  for (std::size_t i = 0; i < num_points + 1; i++) {
     double s = step_size * static_cast<double>(i);
     points.emplace_back(
       [this](const double local_width, const double local_s, const double local_z_offset) {
@@ -75,12 +75,12 @@ auto CatmullRomSpline::getRightBounds(
 }
 
 auto CatmullRomSpline::getLeftBounds(
-  const double width, const size_t num_points, const double z_offset) const
+  const double width, const std::size_t num_points, const double z_offset) const
   -> std::vector<geometry_msgs::msg::Point>
 {
   std::vector<geometry_msgs::msg::Point> points;
   double step_size = getLength() / static_cast<double>(num_points);
-  for (size_t i = 0; i < num_points + 1; i++) {
+  for (std::size_t i = 0; i < num_points + 1; i++) {
     double s = step_size * static_cast<double>(i);
     points.emplace_back(
       [this](const double local_width, const double local_s, const double local_z_offset) {
@@ -142,8 +142,8 @@ CatmullRomSpline::CatmullRomSpline(const std::vector<geometry_msgs::msg::Point>
     /// @note In this case, spline is interpreted as curve.
     default:
       [this](const auto & control_points) -> void {
-        size_t n = control_points.size() - 1;
-        for (size_t i = 0; i < n; i++) {
+        std::size_t n = control_points.size() - 1;
+        for (std::size_t i = 0; i < n; i++) {
           if (i == 0) {
             double ax = 0;
             double bx = control_points[0].x - 2 * control_points[1].x + control_points[2].x;
@@ -244,7 +244,7 @@ CatmullRomSpline::CatmullRomSpline(const std::vector<geometry_msgs::msg::Point>
   }
 }
 
-auto CatmullRomSpline::getCurveIndexAndS(const double s) const -> std::pair<size_t, double>
+auto CatmullRomSpline::getCurveIndexAndS(const double s) const -> std::pair<std::size_t, double>
 {
   if (s < 0) {
     return std::make_pair(0, s);
@@ -254,7 +254,7 @@ auto CatmullRomSpline::getCurveIndexAndS(const double s) const -> std::pair<size
       curves_.size() - 1, s - (total_length_ - curves_[curves_.size() - 1].getLength()));
   }
   double current_s = 0;
-  for (size_t i = 0; i < curves_.size(); i++) {
+  for (std::size_t i = 0; i < curves_.size(); i++) {
     double prev_s = current_s;
     current_s = current_s + length_list_[i];
     if (prev_s <= s && s < current_s) {
@@ -264,11 +264,12 @@ auto CatmullRomSpline::getCurveIndexAndS(const double s) const -> std::pair<size
   THROW_SIMULATION_ERROR("failed to calculate curve index");  // LCOV_EXCL_LINE
 }
 
-auto CatmullRomSpline::getSInSplineCurve(const size_t curve_index, const double s) const -> double
+auto CatmullRomSpline::getSInSplineCurve(const std::size_t curve_index, const double s) const
+  -> double
 {
-  size_t n = curves_.size();
+  std::size_t n = curves_.size();
   double ret = 0;
-  for (size_t i = 0; i < n; i++) {
+  for (std::size_t i = 0; i < n; i++) {
     if (i == curve_index) {
       return ret + s;
     } else {
@@ -295,7 +296,7 @@ auto CatmullRomSpline::getCollisionPointsIn2D(
                                                    const auto local_search_backward) {
     std::set<double> s_value_candidates;
     auto current_curve_start_s = 0.0;
-    for (size_t i = 0; i < curves_.size(); ++i) {
+    for (std::size_t i = 0; i < curves_.size(); ++i) {
       if (
         s_range == std::nullopt ||
         (current_curve_start_s >= s_range->first && current_curve_start_s <= s_range->second)) {
@@ -383,17 +384,17 @@ auto CatmullRomSpline::getCollisionPointIn2D(
   const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
   const bool search_backward) const -> std::optional<double>
 {
-  size_t n = curves_.size();
+  std::size_t n = curves_.size();
   if (search_backward) {
-    for (size_t i = 0; i < n; i++) {
+    for (std::size_t i = 0; i < n; i++) {
       auto s = curves_[n - 1 - i].getCollisionPointIn2D(point0, point1, search_backward, true);
       if (s) {
         return getSInSplineCurve(n - 1 - i, s.value());
       }
     }
     return std::nullopt;
   } else {
-    for (size_t i = 0; i < n; i++) {
+    for (std::size_t i = 0; i < n; i++) {
       auto s = curves_[i].getCollisionPointIn2D(point0, point1, search_backward, true);
       if (s) {
         return getSInSplineCurve(i, s.value());
@@ -435,7 +436,7 @@ auto CatmullRomSpline::getSValue(
       return line_segments_[0].getSValue(pose, threshold_distance, true);
     default:
       double s = 0;
-      for (size_t i = 0; i < curves_.size(); i++) {
+      for (std::size_t i = 0; i < curves_.size(); i++) {
         auto s_value = curves_[i].getSValue(pose, threshold_distance, true);
         if (s_value) {
           s = s + s_value.value();
@@ -692,7 +693,7 @@ auto CatmullRomSpline::checkConnection() const -> bool
     THROW_SIMULATION_ERROR(                                    // LCOV_EXCL_LINE
       "number of control points and curves does not match.");  // LCOV_EXCL_LINE
   }
-  for (size_t i = 0; i < curves_.size(); i++) {
+  for (std::size_t i = 0; i < curves_.size(); i++) {
     const auto control_point0 = control_points[i];
     const auto control_point1 = control_points[i + 1];
     const auto p0 = curves_[i].getPoint(0, false);

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -294,7 +294,7 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const
  * 1. Check if route to target entity from reference entity exists, if not try to transform pose to other 
  *    routable lanelet, within matching distance (findRoutableAlternativeLaneletPoseFrom).
  * 2. Calculate longitudinal distance between entities bounding boxes -> bounding_box_distance.
- * 3. Calculate longitudinal distance between entities poses -> position_distance.
+ * 3. Calculate longitudinal distance between entities poses -> longitudinal_distance.
  * 4. Calculate target entity bounding box distance to reference entity spline (minimal distance from all corners) 
  *    -> target_to_spline_distance.
  * 5. If target_to_spline_distance is less than half width of reference entity target entity is conflicting.
@@ -336,26 +336,26 @@ auto ActionNode::getDistanceToTargetEntity(
       math::geometry::getPointsFromBbox(target_bounding_box));
     const auto bounding_box_diagonal_length =
       math::geometry::getDistance(bounding_box_map_points[0], bounding_box_map_points[2]);
-    if (const auto position_distance = traffic_simulator::distance::longitudinalDistance(
+    if (const auto longitudinal_distance = traffic_simulator::distance::longitudinalDistance(
           *from_lanelet_pose, *target_lanelet_pose, include_adjacent_lanelet,
           include_opposite_direction, routing_configuration, hdmap_utils);
-        !position_distance) {
+        !longitudinal_distance) {
       return std::nullopt;
     } else if (const auto bounding_box_distance =
                  traffic_simulator::distance::boundingBoxLaneLongitudinalDistance(
-                   position_distance, from_bounding_box, target_bounding_box);
+                   longitudinal_distance, from_bounding_box, target_bounding_box);
                !bounding_box_distance || bounding_box_distance.value() < 0.0) {
       return std::nullopt;
     } else {
-      // TODO rotation of NPC is not taken into account, same as in boundingBoxLaneLongitudinalDistance
-      // this should be considered to be changed in separate task in the future
+      /// @todo rotation of NPC is not taken into account, same as in boundingBoxLaneLongitudinalDistance
+      /// this should be considered to be changed in separate task in the future
       const auto target_bounding_box_distance =
         bounding_box_distance.value() + from_bounding_box.dimensions.x / 2.0;
 
       /// @note if the distance of the target entity to the spline is smaller than the width of the reference entity
       if (const auto target_to_spline_distance = traffic_simulator::distance::distanceToSpline(
             static_cast<geometry_msgs::msg::Pose>(*target_lanelet_pose), target_bounding_box,
-            spline, position_distance.value());
+            spline, longitudinal_distance.value());
           target_to_spline_distance <= from_bounding_box.dimensions.y / 2.0) {
         return target_bounding_box_distance;
       }