Merge branch 'main' into feat/check_collision_outside_of_plan

Signed-off-by: kyoichi-sugahara <[email protected]>

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -38,6 +38,14 @@ Planning:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance
 
+  behavior_path_planner_goal_planner:
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.goal_planner
+
+  behavior_path_planner_start_planner:
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.start_planner
+
   behavior_path_avoidance_by_lane_change:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE

control/lane_departure_checker/schema/lane_departure_checker.json

@@ -0,0 +1,79 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for lane_departure_checker",
+  "type": "object",
+  "definitions": {
+    "lane_departure_checker": {
+      "type": "object",
+      "properties": {
+        "footprint_margin_scale": {
+          "type": "number",
+          "default": 1.0,
+          "description": "Coefficient for expanding footprint margin. Multiplied by 1 standard deviation."
+        },
+        "resample_interval": {
+          "type": "number",
+          "default": 0.3,
+          "description": "Minimum Euclidean distance between points when resample trajectory.[m]."
+        },
+        "max_deceleration": {
+          "type": "number",
+          "default": 2.8,
+          "description": "Maximum deceleration when calculating braking distance."
+        },
+        "delay_time": {
+          "type": "number",
+          "default": 1.3,
+          "description": "Delay time which took to actuate brake when calculating braking distance. [second]."
+        },
+        "max_lateral_deviation": {
+          "type": "number",
+          "default": 2.0,
+          "description": "Maximum lateral deviation in vehicle coordinate. [m]."
+        },
+        "max_longitudinal_deviation": {
+          "type": "number",
+          "default": 2.0,
+          "description": "Maximum longitudinal deviation in vehicle coordinate. [m]."
+        },
+        "max_yaw_deviation_deg": {
+          "type": "number",
+          "default": 60.0,
+          "description": "Maximum ego yaw deviation from trajectory. [deg]."
+        },
+        "ego_nearest_dist_threshold": {
+          "type": "number"
+        },
+        "ego_nearest_yaw_threshold": {
+          "type": "number"
+        },
+        "min_braking_distance": {
+          "type": "number"
+        }
+      },
+      "required": [
+        "footprint_margin_scale",
+        "resample_interval",
+        "max_deceleration",
+        "max_lateral_deviation",
+        "max_longitudinal_deviation",
+        "max_yaw_deviation_deg",
+        "ego_nearest_dist_threshold",
+        "ego_nearest_yaw_threshold",
+        "min_braking_distance"
+      ]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/lane_departure_checker"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

control/mpc_lateral_controller/src/lowpass_filter.cpp

@@ -120,10 +120,8 @@ bool filt_vector(const int num, std::vector<double> & u)
     double tmp = 0.0;
     int num_tmp = 0;
     double count = 0;
-    if (i - num < 0) {
-      num_tmp = i;
-    } else if (i + num > static_cast<int>(u.size()) - 1) {
-      num_tmp = static_cast<int>(u.size()) - i - 1;
+    if ((i - num < 0) || (i + num > static_cast<int>(u.size()) - 1)) {
+      num_tmp = std::min(i, static_cast<int>(u.size()) - i - 1);
     } else {
       num_tmp = num;
     }

control/mpc_lateral_controller/test/test_lowpass_filter.cpp

@@ -61,6 +61,30 @@ TEST(TestLowpassFilter, MoveAverageFilter)
     EXPECT_EQ(filtered_vec[4], 23.0 / 3);
     EXPECT_EQ(filtered_vec[5], original_vec[5]);
   }
+  {
+    const int window_size = 3;
+    const std::vector<double> original_vec = {1.0, 1.0, 1.0, 1.0};
+    std::vector<double> filtered_vec = original_vec;
+    EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec));
+    ASSERT_EQ(filtered_vec.size(), original_vec.size());
+    EXPECT_EQ(filtered_vec[0], original_vec[0]);
+    EXPECT_EQ(filtered_vec[1], 1.0);
+    EXPECT_EQ(filtered_vec[2], 1.0);
+    EXPECT_EQ(filtered_vec[3], original_vec[3]);
+  }
+  {
+    const int window_size = 4;
+    const std::vector<double> original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0};
+    std::vector<double> filtered_vec = original_vec;
+    EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec));
+    ASSERT_EQ(filtered_vec.size(), original_vec.size());
+    EXPECT_EQ(filtered_vec[0], original_vec[0]);
+    EXPECT_EQ(filtered_vec[1], 8.0 / 3);
+    EXPECT_EQ(filtered_vec[2], 21.0 / 5);
+    EXPECT_EQ(filtered_vec[3], 30.0 / 5);
+    EXPECT_EQ(filtered_vec[4], 23.0 / 3);
+    EXPECT_EQ(filtered_vec[5], original_vec[5]);
+  }
 }
 TEST(TestLowpassFilter, Butterworth2dFilter)
 {

launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py

@@ -42,7 +42,8 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw")
     add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois")
     add_launch_arg(
-        "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals"
+        "output/traffic_signals",
+        "/perception/traffic_light_recognition/traffic_signals",
     )
 
     # traffic_light_fine_detector
@@ -64,14 +65,20 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("classifier_type", "1")
     add_launch_arg(
         "classifier_model_path",
-        os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"),
+        os.path.join(
+            classifier_share_dir,
+            "data",
+            "traffic_light_classifier_efficientNet_b1.onnx",
+        ),
     )
     add_launch_arg(
-        "classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt")
+        "classifier_label_path",
+        os.path.join(classifier_share_dir, "data", "lamp_labels.txt"),
     )
     add_launch_arg("classifier_precision", "fp16")
     add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]")
     add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]")
+    add_launch_arg("backlight_threshold", "0.85")
 
     add_launch_arg("use_intra_process", "False")
     add_launch_arg("use_multithread", "False")
@@ -102,6 +109,7 @@ def create_parameter_dict(*args):
                         "classifier_precision",
                         "classifier_mean",
                         "classifier_std",
+                        "backlight_threshold",
                     )
                 ],
                 remappings=[
@@ -122,7 +130,10 @@ def create_parameter_dict(*args):
                     ("~/input/image", LaunchConfiguration("input/image")),
                     ("~/input/rois", LaunchConfiguration("output/rois")),
                     ("~/input/rough/rois", "detection/rough/rois"),
-                    ("~/input/traffic_signals", LaunchConfiguration("output/traffic_signals")),
+                    (
+                        "~/input/traffic_signals",
+                        LaunchConfiguration("output/traffic_signals"),
+                    ),
                     ("~/output/image", "debug/rois"),
                     ("~/output/image/compressed", "debug/rois/compressed"),
                     ("~/output/image/compressedDepth", "debug/rois/compressedDepth"),

localization/landmark_based_localizer/README.md

@@ -43,68 +43,7 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc
 
 ## Map specifications
 
-For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret.
-
-The four vertices of a landmark are defined counterclockwise.
-
-The order of the four vertices is defined as follows. In the coordinate system of a landmark,
-
-- the x-axis is parallel to the vector from the first vertex to the second vertex
-- the y-axis is parallel to the vector from the second vertex to the third vertex
-
-![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg)
-
-### Example of `lanelet2_map.osm`
-
-The values provided below are placeholders.
-Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`.
-
-For example, when using the AR tag, it would look like this.
-
-```xml
-...
-
-  <node id="1" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.2356"/>
-    <tag k="local_y" v="87.4506"/>
-    <tag k="ele" v="2.1725"/>
-  </node>
-  <node id="2" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.639"/>
-    <tag k="local_y" v="87.5886"/>
-    <tag k="ele" v="2.5947"/>
-  </node>
-  <node id="3" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.2331"/>
-    <tag k="local_y" v="87.4713"/>
-    <tag k="ele" v="3.0208"/>
-  </node>
-  <node id="4" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="21.8298"/>
-    <tag k="local_y" v="87.3332"/>
-    <tag k="ele" v="2.5985"/>
-  </node>
-
-...
-
-  <way id="5">
-    <nd ref="1"/>
-    <nd ref="2"/>
-    <nd ref="3"/>
-    <nd ref="4"/>
-    <tag k="type" v="pose_marker"/>
-    <tag k="subtype" v="apriltag_16h5"/>
-    <tag k="area" v="yes"/>
-    <tag k="marker_id" v="0"/>
-  </way>
-
-...
-
-```
+See <https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#localization-landmarks>
 
 ## About `consider_orientation`
 

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp

@@ -142,7 +142,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options)
 
 void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg)
 {
-  landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger());
+  landmark_manager_.parse_landmarks(msg, "apriltag_16h5");
   const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg();
   mapped_tag_pose_pub_->publish(marker_msg);
 }
@@ -173,7 +173,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
   const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose;
 
   // detect
-  const std::vector<landmark_manager::Landmark> landmarks = detect_landmarks(msg);
+  const std::vector<Landmark> landmarks = detect_landmarks(msg);
   if (landmarks.empty()) {
     return;
   }
@@ -183,7 +183,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
     PoseArray pose_array_msg;
     pose_array_msg.header.stamp = sensor_stamp;
     pose_array_msg.header.frame_id = "map";
-    for (const landmark_manager::Landmark & landmark : landmarks) {
+    for (const Landmark & landmark : landmarks) {
       const Pose detected_marker_on_map =
         tier4_autoware_utils::transformPose(landmark.pose, self_pose);
       pose_array_msg.poses.push_back(detected_marker_on_map);
@@ -293,7 +293,7 @@ std::vector<landmark_manager::Landmark> ArTagBasedLocalizer::detect_landmarks(
     cv_ptr->image.copyTo(in_image);
   } catch (cv_bridge::Exception & e) {
     RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
-    return std::vector<landmark_manager::Landmark>{};
+    return std::vector<Landmark>{};
   }
 
   // get transform from base_link to camera
@@ -303,15 +303,15 @@ std::vector<landmark_manager::Landmark> ArTagBasedLocalizer::detect_landmarks(
       tf_buffer_->lookupTransform("base_link", msg->header.frame_id, sensor_stamp);
   } catch (tf2::TransformException & ex) {
     RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what());
-    return std::vector<landmark_manager::Landmark>{};
+    return std::vector<Landmark>{};
   }
 
   // detect
   std::vector<aruco::Marker> markers;
   detector_.detect(in_image, markers, cam_param_, marker_size_, false);
 
   // parse
-  std::vector<landmark_manager::Landmark> landmarks;
+  std::vector<Landmark> landmarks;
 
   for (aruco::Marker & marker : markers) {
     // convert marker pose to tf
@@ -327,7 +327,7 @@ std::vector<landmark_manager::Landmark> ArTagBasedLocalizer::detect_landmarks(
     const double distance = std::hypot(pose.position.x, pose.position.y, pose.position.z);
     if (distance <= distance_threshold_) {
       tf2::doTransform(pose, pose, transform_sensor_to_base_link);
-      landmarks.push_back(landmark_manager::Landmark{std::to_string(marker.id), pose});
+      landmarks.push_back(Landmark{std::to_string(marker.id), pose});
     }
 
     // for debug, drawing the detected markers

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp

@@ -80,6 +80,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   using TransformStamped = geometry_msgs::msg::TransformStamped;
   using MarkerArray = visualization_msgs::msg::MarkerArray;
   using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
+  using Landmark = landmark_manager::Landmark;
 
 public:
   explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
@@ -89,7 +90,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   void image_callback(const Image::ConstSharedPtr & msg);
   void cam_info_callback(const CameraInfo::ConstSharedPtr & msg);
   void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg);
-  std::vector<landmark_manager::Landmark> detect_landmarks(const Image::ConstSharedPtr & msg);
+  std::vector<Landmark> detect_landmarks(const Image::ConstSharedPtr & msg);
 
   // Parameters
   float marker_size_{};

localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp

@@ -15,6 +15,8 @@
 #ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
 #define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
 
+#include "lanelet2_extension/localization/landmark.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
@@ -40,13 +42,13 @@ class LandmarkManager
 public:
   void parse_landmarks(
     const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
-    const std::string & target_subtype, const rclcpp::Logger & logger);
+    const std::string & target_subtype);
 
   [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const;
 
   [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose(
-    const std::vector<landmark_manager::Landmark> & detected_landmarks,
-    const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const;
+    const std::vector<Landmark> & detected_landmarks, const geometry_msgs::msg::Pose & self_pose,
+    const bool consider_orientation) const;
 
 private:
   // To allow multiple landmarks with the same id to be registered on a vector_map,