Merge branch 'main' into feat/insert_ahead_stop_point

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

perception/heatmap_visualizer/CMakeLists.txt

@@ -16,4 +16,5 @@ rclcpp_components_register_node(heatmap_visualizer_component
 
 ament_auto_package(INSTALL_TO_SHARE
     launch
+    config
 )

perception/heatmap_visualizer/README.md

@@ -44,14 +44,15 @@ When publishing, firstly these values are normalized to [0, 1] using maximum and
 
 ### Core Parameters
 
-| Name                          | Type   | Default Value | Description                                       |
-| ----------------------------- | ------ | ------------- | ------------------------------------------------- |
-| `frame_count`                 | int    | `50`          | The number of frames to publish heatmap           |
-| `map_frame`                   | string | `base_link`   | the frame of heatmap to be respected              |
-| `map_length`                  | float  | `200.0`       | the length of map in meter                        |
-| `map_resolution`              | float  | `0.8`         | the resolution of map                             |
-| `use_confidence`              | bool   | `false`       | the flag if use confidence score as heatmap value |
-| `rename_car_to_truck_and_bus` | bool   | `true`        | the flag if rename car to truck or bus            |
+| Name                  | Type          | Default Value                                                                         | Description                                     |
+| --------------------- | ------------- | ------------------------------------------------------------------------------------- | ----------------------------------------------- |
+| `publish_frame_count` | int           | `50`                                                                                  | The number of frames to publish heatmap         |
+| `heatmap_frame_id`    | string        | `base_link`                                                                           | The frame ID of heatmap to be respected         |
+| `heatmap_length`      | float         | `200.0`                                                                               | A length of map in meter                        |
+| `heatmap_resolution`  | float         | `0.8`                                                                                 | A resolution of map                             |
+| `use_confidence`      | bool          | `false`                                                                               | A flag if use confidence score as heatmap value |
+| `class_names`         | array<string> | `["UNKNOWN", "CAR", "TRUCK", "BUS", "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"]` | An array of class names to be published         |
+| `rename_to_car`       | bool          | `true`                                                                                | A flag if rename car like vehicle to car        |
 
 ## Assumptions / Known limits
 

perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml

@@ -0,0 +1,33 @@
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+/**:
+  ros__parameters:
+    publish_frame_count: 50
+    heatmap_frame_id: "base_link"
+    heatmap_length: 200.0
+    heatmap_resolution: 0.8
+    use_confidence: false
+    class_names:
+      [
+        "UNKNOWN",
+        "CAR",
+        "TRUCK",
+        "BUS",
+        "TRAILER",
+        "BICYCLE",
+        "MOTORBIKE",
+        "PEDESTRIAN",
+      ]
+    rename_to_car: false

perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml

@@ -2,20 +2,10 @@
 <launch>
   <!-- heatmap visualizer-->
   <arg name="input/objects" default="objects"/>
-  <arg name="frame_count" default="50"/>
-  <arg name="map_frame" default="base_link"/>
-  <arg name="map_length" default="200.0"/>
-  <arg name="map_resolution" default="0.5"/>
-  <arg name="use_confidence" default="false"/>
-  <arg name="rename_car_to_truck_and_bus" default="true"/>
+  <arg name="config_file" default="$(find-pkg-share heatmap_visualizer)/config/heatmap_visualizer.param.yaml"/>
 
   <node pkg="heatmap_visualizer" exec="heatmap_visualizer" output="screen">
     <remap from="~/input/objects" to="$(var input/objects)"/>
-    <param name="frame_count" value="$(var frame_count)"/>
-    <param name="map_frame" value="$(var map_frame)"/>
-    <param name="map_length" value="$(var map_length)"/>
-    <param name="map_resolution" value="$(var map_resolution)"/>
-    <param name="use_confidence" value="$(var use_confidence)"/>
-    <param name="rename_car_to_truck_and_bus" value="$(var rename_car_to_truck_and_bus)"/>
+    <param from="$(var config_file)"/>
   </node>
 </launch>

perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json

@@ -0,0 +1,81 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Heatmap Visualizer Node",
+  "type": "object",
+  "definitions": {
+    "heatmap_visualizer": {
+      "type": "object",
+      "properties": {
+        "publish_frame_count": {
+          "type": "integer",
+          "description": "The number of frames to be published at once.",
+          "default": 50,
+          "minimum": 1
+        },
+        "heatmap_frame_id": {
+          "type": "string",
+          "description": "A frame ID in which visualized heatmap is with respect to.",
+          "default": "base_link"
+        },
+        "heatmap_length": {
+          "type": "number",
+          "description": "A size length of heatmap [m].",
+          "default": 200.0,
+          "exclusiveMinimum": 0.0
+        },
+        "heatmap_resolution": {
+          "type": "number",
+          "description": "A cell resolution of heatmap [m].",
+          "default": 0.8,
+          "exclusiveMinimum": 0.0
+        },
+        "use_confidence": {
+          "type": "boolean",
+          "description": "Indicates whether to use object existence probability as score. It this parameter is false, 1.0 is set to score.",
+          "default": false
+        },
+        "class_names": {
+          "type": "array",
+          "description": "An array of object class names.",
+          "default": [
+            "UNKNOWN",
+            "CAR",
+            "TRUCK",
+            "BUS",
+            "TRAILER",
+            "BICYCLE",
+            "MOTORBIKE",
+            "PEDESTRIAN"
+          ],
+          "uniqueItems": true
+        },
+        "rename_to_car": {
+          "type": "boolean",
+          "description": "Indicates whether to rename car like vehicle into car.",
+          "default": false
+        }
+      },
+      "required": [
+        "publish_frame_count",
+        "heatmap_frame_id",
+        "heatmap_length",
+        "heatmap_resolution",
+        "use_confidence",
+        "class_names",
+        "rename_to_car"
+      ]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/heatmap_visualizer"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp

@@ -27,13 +27,13 @@ namespace heatmap_visualizer
 HeatmapVisualizerNode::HeatmapVisualizerNode(const rclcpp::NodeOptions & node_options)
 : Node("heatmap_visualizer", node_options), frame_count_(0)
 {
-  total_frame_ = declare_parameter("frame_count", 50);
-  map_frame_ = declare_parameter("map_frame", "base_link");
-  map_length_ = declare_parameter("map_length", 200.0);
-  map_resolution_ = declare_parameter("map_resolution", 0.8);
-  use_confidence_ = declare_parameter("use_confidence", false);
-  class_names_ = declare_parameter("class_names", class_names_);
-  rename_car_to_truck_and_bus_ = declare_parameter("rename_car_to_truck_and_bus", false);
+  total_frame_ = static_cast<uint32_t>(declare_parameter<int64_t>("publish_frame_count"));
+  map_frame_ = declare_parameter<std::string>("heatmap_frame_id");
+  map_length_ = static_cast<float>(declare_parameter<double>("heatmap_length"));
+  map_resolution_ = static_cast<float>(declare_parameter<double>("heatmap_resolution"));
+  use_confidence_ = declare_parameter<bool>("use_confidence");
+  class_names_ = declare_parameter<std::vector<std::string>>("class_names");
+  rename_car_to_truck_and_bus_ = declare_parameter<bool>("rename_to_car");
 
   width_ = static_cast<uint32_t>(map_length_ / map_resolution_);
   height_ = static_cast<uint32_t>(map_length_ / map_resolution_);

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -57,6 +57,8 @@
         absence_traffic_light:
           creep_velocity: 1.388 # [m/s]
           maximum_peeking_distance: 6.0 # [m]
+        attention_lane_crop_curvature_threshold: 0.25
+        attention_lane_curvature_calculation_ds: 0.5
 
       enable_rtc:
         intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -173,6 +173,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       &debug_marker_array);
   }
 
+  if (debug_data_.occlusion_attention_area) {
+    appendMarkerArray(
+      createLaneletPolygonsMarkerArray(
+        debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917,
+        0.568, 0.596),
+      &debug_marker_array);
+  }
+
   if (debug_data_.adjacent_area) {
     appendMarkerArray(
       createLaneletPolygonsMarkerArray(

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -140,6 +140,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".occlusion.absence_traffic_light.creep_velocity");
   ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter<double>(
     node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance");
+  ip.occlusion.attention_lane_crop_curvature_threshold =
+    getOrDeclareParameter<double>(node, ns + ".occlusion.attention_lane_crop_curvature_threshold");
+  ip.occlusion.attention_lane_curvature_calculation_ds =
+    getOrDeclareParameter<double>(node, ns + ".occlusion.attention_lane_curvature_calculation_ds");
 }
 
 void IntersectionModuleManager::launchNewModules(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -989,6 +989,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention();
   const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area();
   debug_data_.attention_area = intersection_lanelets.attention_area();
+  debug_data_.occlusion_attention_area = occlusion_attention_area;
   debug_data_.adjacent_area = intersection_lanelets.adjacent_area();
 
   // get intersection area
@@ -1027,7 +1028,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   if (!occlusion_attention_divisions_) {
     occlusion_attention_divisions_ = util::generateDetectionLaneDivisions(
       occlusion_attention_lanelets, routing_graph_ptr,
-      planner_data_->occupancy_grid->info.resolution);
+      planner_data_->occupancy_grid->info.resolution,
+      planner_param_.occlusion.attention_lane_crop_curvature_threshold,
+      planner_param_.occlusion.attention_lane_curvature_calculation_ds);
   }
   const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value();
 

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -119,6 +119,8 @@ class IntersectionModule : public SceneModuleInterface
         double creep_velocity;
         double maximum_peeking_distance;
       } absence_traffic_light;
+      double attention_lane_crop_curvature_threshold;
+      double attention_lane_curvature_calculation_ds;
     } occlusion;
   };
 

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -20,6 +20,7 @@
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <interpolation/spline_interpolation_points_2d.hpp>
 #include <lanelet2_extension/regulatory_elements/road_marking.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -42,6 +43,22 @@
 #include <tuple>
 #include <unordered_map>
 #include <vector>
+
+namespace tier4_autoware_utils
+{
+
+template <>
+inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p)
+{
+  geometry_msgs::msg::Point point;
+  point.x = p.x();
+  point.y = p.y();
+  point.z = p.z();
+  return point;
+}
+
+}  // namespace tier4_autoware_utils
+
 namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
@@ -809,10 +826,26 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel(
   return TrafficPrioritizedLevel::NOT_PRIORITIZED;
 }
 
+double getHighestCurvature(const lanelet::ConstLineString3d & centerline)
+{
+  std::vector<lanelet::ConstPoint3d> points;
+  for (auto point = centerline.begin(); point != centerline.end(); point++) {
+    points.push_back(*point);
+  }
+
+  SplineInterpolationPoints2d interpolation(points);
+  const std::vector<double> curvatures = interpolation.getSplineInterpolatedCurvatures();
+  std::vector<double> curvatures_positive;
+  for (const auto & curvature : curvatures) {
+    curvatures_positive.push_back(std::fabs(curvature));
+  }
+  return *std::max_element(curvatures_positive.begin(), curvatures_positive.end());
+}
+
 std::vector<DiscretizedLane> generateDetectionLaneDivisions(
   lanelet::ConstLanelets detection_lanelets_all,
-  [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const double resolution)
+  const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution,
+  const double curvature_threshold, const double curvature_calculation_ds)
 {
   using lanelet::utils::getCenterlineWithOffset;
   using lanelet::utils::to2D;
@@ -824,6 +857,12 @@ std::vector<DiscretizedLane> generateDetectionLaneDivisions(
     if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) {
       continue;
     }
+    const auto fine_centerline =
+      lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds);
+    const double highest_curvature = getHighestCurvature(fine_centerline);
+    if (highest_curvature > curvature_threshold) {
+      continue;
+    }
     detection_lanelets.push_back(detection_lanelet);
   }
 

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -109,7 +109,8 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel(
 
 std::vector<DiscretizedLane> generateDetectionLaneDivisions(
   lanelet::ConstLanelets detection_lanelets,
-  const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution);
+  const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution,
+  const double curvature_threshold, const double curvature_calculation_ds);
 
 std::optional<InterpolatedPathInfo> generateInterpolatedPath(
   const int lane_id, const std::set<int> & associative_lane_ids,

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -39,6 +39,7 @@ struct DebugData
   std::optional<geometry_msgs::msg::Pose> occlusion_first_stop_wall_pose{std::nullopt};
   std::optional<geometry_msgs::msg::Pose> pass_judge_wall_pose{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> attention_area{std::nullopt};
+  std::optional<std::vector<lanelet::CompoundPolygon3d>> occlusion_attention_area{std::nullopt};
   std::optional<geometry_msgs::msg::Polygon> intersection_area{std::nullopt};
   std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> adjacent_area{std::nullopt};