Merge branch 'main' into feat/centerpoint_pillar_num_check

common/autoware_component_interface_tools/CMakeLists.txt

@@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "ServiceLogChecker"
+  PLUGIN "autoware::component_interface_tools::ServiceLogChecker"
   EXECUTABLE service_log_checker_node
 )
 

common/autoware_component_interface_tools/src/service_log_checker.cpp

@@ -22,6 +22,8 @@
 #define FMT_HEADER_ONLY
 #include <fmt/format.h>
 
+namespace autoware::component_interface_tools
+{
 ServiceLogChecker::ServiceLogChecker(const rclcpp::NodeOptions & options)
 : Node("service_log_checker", options), diagnostics_(this)
 {
@@ -98,6 +100,6 @@ void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusW
     stat.summary(DiagnosticStatus::ERROR, "ERROR");
   }
 }
-
+}  // namespace autoware::component_interface_tools
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(ServiceLogChecker)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::component_interface_tools::ServiceLogChecker)

common/autoware_component_interface_tools/src/service_log_checker.hpp

@@ -23,6 +23,8 @@
 #include <string>
 #include <unordered_map>
 
+namespace autoware::component_interface_tools
+{
 class ServiceLogChecker : public rclcpp::Node
 {
 public:
@@ -38,5 +40,5 @@ class ServiceLogChecker : public rclcpp::Node
   void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
   std::unordered_map<std::string, std::string> errors_;
 };
-
+}  // namespace autoware::component_interface_tools
 #endif  // SERVICE_LOG_CHECKER_HPP_

common/autoware_traffic_light_recognition_marker_publisher/CMakeLists.txt

@@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "TrafficLightRecognitionMarkerPublisher"
+  PLUGIN "autoware::traffic_light_recognition_marker_publisher::TrafficLightRecognitionMarkerPublisher"
   EXECUTABLE ${PROJECT_NAME}_node
 )
 

common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp

@@ -20,6 +20,8 @@
 #include <utility>
 #include <vector>
 
+namespace autoware::traffic_light_recognition_marker_publisher
+{
 TrafficLightRecognitionMarkerPublisher::TrafficLightRecognitionMarkerPublisher(
   const rclcpp::NodeOptions & options)
 : Node("traffic_light_recognition_marker_publisher", options)
@@ -159,6 +161,8 @@ std_msgs::msg::ColorRGBA TrafficLightRecognitionMarkerPublisher::getTrafficLight
 
   return c;  // white
 }
+}  // namespace autoware::traffic_light_recognition_marker_publisher
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightRecognitionMarkerPublisher)
+RCLCPP_COMPONENTS_REGISTER_NODE(
+  autoware::traffic_light_recognition_marker_publisher::TrafficLightRecognitionMarkerPublisher)

common/autoware_traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp

@@ -29,6 +29,8 @@
 #include <map>
 #include <string>
 
+namespace autoware::traffic_light_recognition_marker_publisher
+{
 class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node
 {
 public:
@@ -58,5 +60,6 @@ class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node
   bool is_map_ready_ = false;
   int32_t marker_id = 0;
 };
+}  // namespace autoware::traffic_light_recognition_marker_publisher
 
 #endif  // TRAFFIC_LIGHT_RECOGNITION_MARKER_PUBLISHER_HPP_

common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp

@@ -82,7 +82,7 @@ void BagTimeManagerPanel::onPauseClicked()
     pause_button_->setStyleSheet("background-color: #00FF00;");
     auto req = std::make_shared<Resume::Request>();
     client_resume_->async_send_request(
-      req, [this]([[maybe_unused]] rclcpp::Client<Resume>::SharedFuture result) {});
+      req, []([[maybe_unused]] rclcpp::Client<Resume>::SharedFuture result) {});
   } else {
     // do pause
     current_state_ = STATE::PAUSE;
@@ -91,7 +91,7 @@ void BagTimeManagerPanel::onPauseClicked()
     pause_button_->setStyleSheet("background-color: #FF0000;");
     auto req = std::make_shared<Pause::Request>();
     client_pause_->async_send_request(
-      req, [this]([[maybe_unused]] rclcpp::Client<Pause>::SharedFuture result) {});
+      req, []([[maybe_unused]] rclcpp::Client<Pause>::SharedFuture result) {});
   }
 }
 

evaluator/localization_evaluator/package.xml

@@ -6,6 +6,11 @@
 
   <maintainer email="[email protected]">Dominik Jargot</maintainer>
   <maintainer email="[email protected]">Koji Minoda</maintainer>
+  <maintainer email="[email protected]">Anh Nguyen</maintainer>
+  <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <maintainer email="[email protected]">Taiki Yamada</maintainer>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
 
   <license>Apache License 2.0</license>
 

perception/autoware_image_projection_based_fusion/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.14)
 project(autoware_image_projection_based_fusion)
 # add_compile_options(-Wno-unknown-pragmas)
 add_compile_options(-Wno-unknown-pragmas -fopenmp)
+add_compile_options(-Wno-error=attributes)
 
 find_package(autoware_cmake REQUIRED)
 find_package(OpenCV REQUIRED)

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/debugger.hpp

@@ -56,7 +56,7 @@ class Debugger
   void imageCallback(
     const sensor_msgs::msg::Image::ConstSharedPtr input_image_msg, const std::size_t image_id);
 
-  rclcpp::Node * node_ptr_;
+  [[maybe_unused]] rclcpp::Node * node_ptr_;
   std::shared_ptr<image_transport::ImageTransport> image_transport_;
   std::vector<std::string> input_camera_topics_;
   std::vector<image_transport::Subscriber> image_subs_;

perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp

@@ -55,7 +55,6 @@ class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
 
   DetectionClassRemapper detection_class_remapper_;
-  float score_threshold_{0.0};
   std::vector<std::string> class_names_;
 
   NonMaximumSuppression iou_bev_nms_;

perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp

@@ -77,7 +77,6 @@ class TrackerObjectDebugger
 
   std::vector<ObjectData> object_data_list_;
   std::list<int32_t> unused_marker_ids_;
-  int32_t marker_id_ = 0;
   std::vector<std::vector<ObjectData>> object_data_groups_;
 
   std::vector<std::string> channel_names_;

perception/autoware_multi_object_tracker/src/processor/input_manager.hpp

@@ -88,7 +88,6 @@ class InputStream
 
   // bool is_time_initialized_{false};
   int initial_count_{0};
-  double expected_interval_{};
   double latency_mean_{};
   double latency_var_{};
   double interval_mean_{};
@@ -130,8 +129,6 @@ class InputManager
   double target_stream_latency_std_{0.04};   // [s]
   double target_stream_interval_{0.1};       // [s]
   double target_stream_interval_std_{0.02};  // [s]
-  double target_latency_{0.2};               // [s]
-  double target_latency_band_{1.0};          // [s]
 
 private:
   void getObjectTimeInterval(

perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp

@@ -56,13 +56,8 @@ class LowIntensityClusterFilter : public rclcpp::Node
   double max_y_;
   double min_y_;
 
-  double max_x_transformed_;
-  double min_x_transformed_;
-  double max_y_transformed_;
-  double min_y_transformed_;
   // Eigen::Vector4f min_boundary_transformed_;
   // Eigen::Vector4f max_boundary_transformed_;
-  bool is_validation_range_transformed_ = false;
   const std::string base_link_frame_id_ = "base_link";
   autoware::detected_object_validation::utils::FilterTargetLabel filter_target_;
 

perception/autoware_tracking_object_merger/src/association/data_association.cpp

@@ -94,11 +94,11 @@ void DataAssociation::assign(
   const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
   std::unordered_map<int, int> & reverse_assignment)
 {
-  std::vector<std::vector<double>> score(src.rows());
+  std::vector<std::vector<double>> score(static_cast<size_t>(src.rows()));
   for (int row = 0; row < src.rows(); ++row) {
-    score.at(row).resize(src.cols());
+    score.at(static_cast<size_t>(row)).resize(static_cast<size_t>(src.cols()));
     for (int col = 0; col < src.cols(); ++col) {
-      score.at(row).at(col) = src(row, col);
+      score.at(static_cast<size_t>(row)).at(static_cast<size_t>(col)) = src(row, col);
     }
   }
   // Solve
@@ -134,15 +134,17 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
   const autoware_perception_msgs::msg::TrackedObjects & objects0,
   const autoware_perception_msgs::msg::TrackedObjects & objects1)
 {
-  Eigen::MatrixXd score_matrix =
-    Eigen::MatrixXd::Zero(objects1.objects.size(), objects0.objects.size());
+  Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(
+    static_cast<Eigen::Index>(objects1.objects.size()),
+    static_cast<Eigen::Index>(objects0.objects.size()));
   for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) {
     const auto & object1 = objects1.objects.at(objects1_idx);
     for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) {
       const auto & object0 = objects0.objects.at(objects0_idx);
       const double score = calcScoreBetweenObjects(object0, object1);
 
-      score_matrix(objects1_idx, objects0_idx) = score;
+      score_matrix(
+        static_cast<Eigen::Index>(objects1_idx), static_cast<Eigen::Index>(objects0_idx)) = score;
     }
   }
   return score_matrix;
@@ -159,15 +161,17 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
   const autoware_perception_msgs::msg::TrackedObjects & objects0,
   const std::vector<TrackerState> & trackers)
 {
-  Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size());
+  Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(
+    static_cast<Eigen::Index>(trackers.size()), static_cast<Eigen::Index>(objects0.objects.size()));
   for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) {
     const auto & object1 = trackers.at(trackers_idx).getObject();
 
     for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) {
       const auto & object0 = objects0.objects.at(objects0_idx);
       const double score = calcScoreBetweenObjects(object0, object1);
 
-      score_matrix(trackers_idx, objects0_idx) = score;
+      score_matrix(
+        static_cast<Eigen::Index>(trackers_idx), static_cast<Eigen::Index>(objects0_idx)) = score;
     }
   }
   return score_matrix;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -218,14 +218,23 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
     shifted_path.path.points.push_back(p);
   }
 
+  // combine road lanes and shoulder lanes to find closest lanelet id
+  const auto lanes = std::invoke([&]() -> lanelet::ConstLanelets {
+    auto lanes = road_lanes;
+    lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
+    return lanes;  // not copy the value (Return Value Optimization)
+  });
+
   // set goal pose with velocity 0
   {
     PathPointWithLaneId p{};
     p.point.longitudinal_velocity_mps = 0.0;
     p.point.pose = goal_pose;
-    p.lane_ids = shifted_path.path.points.back().lane_ids;
-    for (const auto & lane : pull_over_lanes) {
-      p.lane_ids.push_back(lane.id());
+    lanelet::Lanelet goal_lanelet{};
+    if (lanelet::utils::query::getClosestLanelet(lanes, goal_pose, &goal_lanelet)) {
+      p.lane_ids = {goal_lanelet.id()};
+    } else {
+      p.lane_ids = shifted_path.path.points.back().lane_ids;
     }
     shifted_path.path.points.push_back(p);
   }
@@ -234,24 +243,13 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   for (size_t i = path_shifter.getShiftLines().front().start_idx;
        i < shifted_path.path.points.size() - 1; ++i) {
     auto & point = shifted_path.path.points.at(i);
-    // set velocity
     point.point.longitudinal_velocity_mps =
       std::min(point.point.longitudinal_velocity_mps, static_cast<float>(pull_over_velocity));
-
-    // add target lanes to points after shift start
-    // add road lane_ids if not found
-    for (const auto id : shifted_path.path.points.back().lane_ids) {
-      if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) {
-        point.lane_ids.push_back(id);
-      }
-    }
-    // add shoulder lane_id if not found
-    for (const auto & lane : pull_over_lanes) {
-      if (
-        std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) ==
-        point.lane_ids.end()) {
-        point.lane_ids.push_back(lane.id());
-      }
+    lanelet::Lanelet lanelet{};
+    if (lanelet::utils::query::getClosestLanelet(lanes, point.point.pose, &lanelet)) {
+      point.lane_ids = {lanelet.id()};  // overwrite lane_ids
+    } else {
+      point.lane_ids = shifted_path.path.points.at(i - 1).lane_ids;
     }
   }
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -351,6 +351,71 @@ stop
 @enduml
 ```
 
+#### Delay Lane Change Check
+
+In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle.
+To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected.
+
+1. The distance from object to terminal end is sufficient to perform lane change
+2. The distance to object is less than the lane changing length
+3. The distance from object to next object is sufficient to perform lane change
+
+If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked.
+
+The following flow chart illustrates the delay lane change check.
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #White
+
+start
+if (Is target objects, candidate path, OR current lane path empty?) then (yes)
+  #LightPink:Return false;
+  stop
+else (no)
+endif
+
+:Start checking objects from closest to furthest;
+repeat
+  if (Is distance from object to terminal sufficient) then (yes)
+  else (no)
+    #LightPink:Return false;
+    stop
+  endif
+
+  if (Is distance to object less than lane changing length) then (yes)
+  else (no)
+    if (Is only check parked vehicles and vehicle is not parked) then (yes)
+    else (no)
+      if(Is last object OR distance to next object is sufficient) then (yes)
+        #LightGreen: Return true;
+        stop
+      else (no)
+      endif
+    endif
+  endif
+  repeat while (Is finished checking all objects) is (FALSE)
+
+#LightPink: Return false;
+stop
+
+@enduml
+```
+
+The following figures demonstrate different situations under which will or will not be triggered:
+
+1. Delay lane change will be triggered as there is sufficient distance ahead.
+   ![delay lane change 1](./images/delay_lane_change_1.drawio.svg)
+2. Delay lane change will NOT be triggered as there is no sufficient distance ahead
+   ![delay lane change 2](./images/delay_lane_change_2.drawio.svg)
+3. Delay lane change will be triggered by fist NPC as there is sufficient distance ahead.
+   ![delay lane change 3](./images/delay_lane_change_3.drawio.svg)
+4. Delay lane change will be triggered by second NPC as there is sufficient distance ahead
+   ![delay lane change 4](./images/delay_lane_change_4.drawio.svg)
+5. Delay lane change will NOT be triggered as there is no sufficient distance ahead.
+   ![delay lane change 5](./images/delay_lane_change_5.drawio.svg)
+
 #### Candidate Path's Safety check
 
 See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md)
@@ -828,8 +893,6 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `trajectory.lat_acc_sampling_num`            | [-]    | int    | Number of possible lane-changing trajectories that are being influenced by lateral acceleration                        | 3                  |
 | `trajectory.max_longitudinal_acc`            | [m/s2] | double | maximum longitudinal acceleration for lane change                                                                      | 1.0                |
 | `trajectory.min_longitudinal_acc`            | [m/s2] | double | maximum longitudinal deceleration for lane change                                                                      | -1.0               |
-| `object_check_min_road_shoulder_width`       | [m]    | double | Width considered as a road shoulder if the lane does not have a road shoulder                                          | 0.5                |
-| `object_shiftable_ratio_threshold`           | [-]    | double | Vehicles around the center line within this distance ratio will be excluded from parking objects                       | 0.6                |
 | `min_length_for_turn_signal_activation`      | [m]    | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length             | 10.0               |
 | `lateral_acceleration.velocity`              | [m/s]  | double | Reference velocity for lateral acceleration calculation (look up table)                                                | [0.0, 4.0, 10.0]   |
 | `lateral_acceleration.min_values`            | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table)                                              | [0.4, 0.4, 0.4]    |
@@ -860,6 +923,15 @@ The following parameters are used to judge lane change completion.
 | `stuck_detection.velocity`  | [m/s] | double | Velocity threshold for ego vehicle stuck detection  | 0.1           |
 | `stuck_detection.stop_time` | [s]   | double | Stop time threshold for ego vehicle stuck detection | 3.0           |
 
+### Delay Lane Change
+
+| Name                                              | Unit | Type   | Description                                                                                           | Default value |
+| :------------------------------------------------ | ---- | ------ | ----------------------------------------------------------------------------------------------------- | ------------- |
+| `delay_lane_change.enable`                        | [-]  | bool   | Flag to enable/disable lane change delay feature                                                      | true          |
+| `delay_lane_change.check_only_parked_vehicle`     | [-]  | bool   | Flag to limit delay feature for only parked vehicles                                                  | false         |
+| `delay_lane_change.min_road_shoulder_width`       | [m]  | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5           |
+| `delay_lane_change.th_parked_vehicle_shift_ratio` | [-]  | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked             | 0.6           |
+
 ### Collision checks
 
 #### Target Objects