Merge branch 'main' into fix/ignore_spell_check

Author: h-ohta

.github/workflows/build-and-test.yaml

@@ -9,7 +9,7 @@ on:
 jobs:
   build-and-test:
     if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }}
-    runs-on: ubuntu-latest
+    runs-on: [self-hosted, linux, X64]
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
@@ -27,14 +27,6 @@ jobs:
       - name: Check out repository
         uses: actions/checkout@v3
 
-      - name: Free disk space (Ubuntu)
-        uses: jlumbroso/free-disk-space@v1.2.0
-        with:
-          tool-cache: false
-          dotnet: false
-          swap-storage: false
-          large-packages: false
-
       - name: Remove exec_depend
         uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
 

control/control_validator/src/utils.cpp

@@ -104,10 +104,10 @@ Trajectory alignTrajectoryWithReferenceTrajectory(
 
   // If first point of predicted_trajectory is in front of start of trajectory, erase points which
   // are in front of trajectory start point and insert pNew along the predicted_trajectory
-  // predicted_trajectory:   　　　　p1-----p2-----p3----//------pN
+  // predicted_trajectory:           p1-----p2-----p3----//------pN
   // trajectory:                               t1--------//------tN
   // ↓
-  // predicted_trajectory:   　　　　        pNew--p3----//------pN
+  // predicted_trajectory:                   pNew--p3----//------pN
   // trajectory:                               t1--------//------tN
   auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint(
     trajectory_points, modified_trajectory_points, predicted_trajectory_points);
@@ -119,7 +119,7 @@ Trajectory alignTrajectoryWithReferenceTrajectory(
 
   // If last point of predicted_trajectory is behind of end of trajectory, erase points which are
   // behind trajectory last point and insert pNew along the predicted_trajectory
-  // predicted_trajectory:   　　　　p1-----//------pN-2-----pN-1-----pN
+  // predicted_trajectory:           p1-----//------pN-2-----pN-1-----pN
   // trajectory:                     t1-----//-----tN-1--tN
   // ↓
   // predicted_trajectory:           p1-----//------pN-2-pNew

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -71,12 +71,14 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
   if (need_rebuild_) {
     ndt_ptr_mutex_->lock();
     auto param = ndt_ptr_->getParams();
+    auto input_source = ndt_ptr_->getInputSource();
 
     ndt_ptr_.reset(new NdtType);
 
     ndt_ptr_->setParams(param);
 
     update_ndt(position, *ndt_ptr_);
+    ndt_ptr_->setInputSource(input_source);
     ndt_ptr_mutex_->unlock();
     need_rebuild_ = false;
   } else {

perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json

@@ -15,9 +15,36 @@
           "type": "boolean",
           "default": "true",
           "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud"
+        },
+        "timer_interval_ms": {
+          "type": "number",
+          "default": "100",
+          "description": "Timer interval to load map points [ms]"
+        },
+        "map_update_distance_threshold": {
+          "type": "number",
+          "default": "10.0",
+          "description": "Threshold distance to update map points with input points [m]"
+        },
+        "map_loader_radius": {
+          "type": "number",
+          "default": "150.0",
+          "description": "Radius to load map points [m]"
+        },
+        "publish_debug_pcd": {
+          "type": "boolean",
+          "default": "false",
+          "description": "Publish a downsampled map pointcloud for debugging"
         }
       },
-      "required": ["distance_threshold", "use_dynamic_map_loading"],
+      "required": [
+        "distance_threshold",
+        "use_dynamic_map_loading",
+        "timer_interval_ms",
+        "map_update_distance_threshold",
+        "map_loader_radius",
+        "publish_debug_pcd"
+      ],
       "additionalProperties": false
     }
   },

perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json

@@ -20,9 +20,37 @@
           "type": "number",
           "default": "0.5",
           "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis"
+        },
+        "timer_interval_ms": {
+          "type": "number",
+          "default": "100",
+          "description": "Timer interval to load map points [ms]"
+        },
+        "map_update_distance_threshold": {
+          "type": "number",
+          "default": "10.0",
+          "description": "Threshold distance to update map points with input points [m]"
+        },
+        "map_loader_radius": {
+          "type": "number",
+          "default": "150.0",
+          "description": "Radius to load map points [m]"
+        },
+        "publish_debug_pcd": {
+          "type": "boolean",
+          "default": "false",
+          "description": "Publish a downsampled map pointcloud for debugging"
         }
       },
-      "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"],
+      "required": [
+        "distance_threshold",
+        "use_dynamic_map_loading",
+        "downsize_ratio_z_axis",
+        "timer_interval_ms",
+        "map_update_distance_threshold",
+        "map_loader_radius",
+        "publish_debug_pcd"
+      ],
       "additionalProperties": false
     }
   },

perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json

@@ -21,6 +21,11 @@
           "default": "0.5",
           "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis"
         },
+        "timer_interval_ms": {
+          "type": "number",
+          "default": "100",
+          "description": "Timer interval to load map points [ms]"
+        },
         "map_update_distance_threshold": {
           "type": "number",
           "default": "10.0",
@@ -31,19 +36,20 @@
           "default": "150.0",
           "description": "Radius to load map points [m]"
         },
-        "timer_interval_ms": {
-          "type": "number",
-          "default": "100",
-          "description": "Timer interval to load map points [ms]"
+        "publish_debug_pcd": {
+          "type": "boolean",
+          "default": "false",
+          "description": "Publish a downsampled map pointcloud for debugging"
         }
       },
       "required": [
         "distance_threshold",
         "use_dynamic_map_loading",
         "downsize_ratio_z_axis",
+        "timer_interval_ms",
         "map_update_distance_threshold",
         "map_loader_radius",
-        "timer_interval_ms"
+        "publish_debug_pcd"
       ],
       "additionalProperties": false
     }

perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json

@@ -20,9 +20,37 @@
           "type": "number",
           "default": "0.5",
           "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis"
+        },
+        "timer_interval_ms": {
+          "type": "number",
+          "default": "100",
+          "description": "Timer interval to load map points [ms]"
+        },
+        "map_update_distance_threshold": {
+          "type": "number",
+          "default": "10.0",
+          "description": "Threshold distance to update map points with input points [m]"
+        },
+        "map_loader_radius": {
+          "type": "number",
+          "default": "150.0",
+          "description": "Radius to load map points [m]"
+        },
+        "publish_debug_pcd": {
+          "type": "boolean",
+          "default": "false",
+          "description": "Publish a downsampled map pointcloud for debugging"
         }
       },
-      "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"],
+      "required": [
+        "distance_threshold",
+        "use_dynamic_map_loading",
+        "downsize_ratio_z_axis",
+        "timer_interval_ms",
+        "map_update_distance_threshold",
+        "map_loader_radius",
+        "publish_debug_pcd"
+      ],
       "additionalProperties": false
     }
   },

perception/map_based_prediction/images/inside_road.svg

@@ -79,7 +79,7 @@ class Tracker
     const rclcpp::Time & time) const;
 
   /*
-   *　Pure virtual function
+   * Pure virtual function
    */
 
 protected:

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp

@@ -79,7 +79,7 @@ class Tracker
     const rclcpp::Time & time) const;
 
   /*
-   *　Pure virtual function
+   * Pure virtual function
    */
 
 protected:

perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp

@@ -54,6 +54,10 @@ class AvoidanceByLaneChange : public NormalLaneChange
     const AvoidancePlanningData & data, const PredictedObject & object) const;
 
   void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const;
+
+  double calcMinAvoidanceLength(const ObjectData & nearest_object) const;
+  double calcMinimumLaneChangeLength() const;
+  double calcLateralOffset() const;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp

@@ -16,13 +16,9 @@
 
 #include "behavior_path_planner_common/interface/scene_module_interface.hpp"
 #include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
-#include "behavior_path_planner_common/marker_utils/utils.hpp"
 
-#include <algorithm>
 #include <memory>
 #include <string>
-#include <utility>
-#include <vector>
 
 namespace behavior_path_planner
 {
@@ -45,7 +41,8 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
 
 bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
 {
-  return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck();
+  return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() &&
+         module_type_->isValidPath();
 }
 void AvoidanceByLaneChangeInterface::processOnEntry()
 {

planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp

@@ -20,16 +20,21 @@
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
+#include <behavior_path_avoidance_module/data_structs.hpp>
+#include <behavior_path_lane_change_module/utils/utils.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/logging.hpp>
 
 #include <boost/geometry/algorithms/centroid.hpp>
 #include <boost/geometry/strategies/cartesian/centroid_bashein_detmer.hpp>
 
+#include <limits>
 #include <utility>
 
 namespace behavior_path_planner
 {
+using behavior_path_planner::utils::lane_change::debug::createExecutionArea;
+
 AvoidanceByLaneChange::AvoidanceByLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters,
   std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
@@ -68,44 +73,15 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
     return false;
   }
 
-  const auto current_lanes = getCurrentLanes();
-  if (current_lanes.empty()) {
-    RCLCPP_DEBUG(logger_, "no empty lanes");
-    return false;
-  }
-
   const auto & nearest_object = data.target_objects.front();
+  const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
+  const auto minimum_lane_change_length = calcMinimumLaneChangeLength();
 
-  // get minimum lane change distance
-  const auto shift_intervals =
-    getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
-  const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
-    *lane_change_parameters_, shift_intervals,
-    lane_change_parameters_->backward_length_buffer_for_end_of_lane);
-
-  // get minimum avoid distance
-
-  const auto ego_width = getCommonParam().vehicle_width;
-  const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
-  const auto nearest_object_parameter =
-    avoidance_parameters_->object_parameters.at(nearest_object_type);
-  const auto avoid_margin =
-    nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
-    nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
-
-  avoidance_helper_->setData(planner_data_);
-  const auto shift_length = avoidance_helper_->getShiftLength(
-    nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);
-
-  const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length);
-
-  RCLCPP_DEBUG(
-    logger_,
-    "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance "
-    "%.3f",
-    nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance);
+  lane_change_debug_.execution_area = createExecutionArea(
+    getCommonParam().vehicle_info, getEgoPose(),
+    std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset());
 
-  return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance);
+  return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length);
 }
 
 bool AvoidanceByLaneChange::specialExpiredCheck() const
@@ -274,4 +250,48 @@ ObjectData AvoidanceByLaneChange::createObjectData(
 
   return object_data;
 }
+
+double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_object) const
+{
+  const auto ego_width = getCommonParam().vehicle_width;
+  const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
+  const auto nearest_object_parameter =
+    avoidance_parameters_->object_parameters.at(nearest_object_type);
+  const auto avoid_margin =
+    nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
+    nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
+
+  avoidance_helper_->setData(planner_data_);
+  const auto shift_length = avoidance_helper_->getShiftLength(
+    nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);
+
+  return avoidance_helper_->getMinAvoidanceDistance(shift_length);
+}
+
+double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
+{
+  const auto current_lanes = getCurrentLanes();
+  if (current_lanes.empty()) {
+    RCLCPP_DEBUG(logger_, "no empty lanes");
+    return std::numeric_limits<double>::infinity();
+  }
+
+  // get minimum lane change distance
+  const auto shift_intervals =
+    getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
+  return utils::lane_change::calcMinimumLaneChangeLength(
+    *lane_change_parameters_, shift_intervals,
+    lane_change_parameters_->backward_length_buffer_for_end_of_lane);
+}
+
+double AvoidanceByLaneChange::calcLateralOffset() const
+{
+  auto additional_lat_offset{0.0};
+  for (const auto & [type, p] : avoidance_parameters_->object_parameters) {
+    const auto offset =
+      2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral;
+    additional_lat_offset = std::max(additional_lat_offset, offset);
+  }
+  return additional_lat_offset;
+}
 }  // namespace behavior_path_planner