diff --git a/perception/heatmap_visualizer/CMakeLists.txt b/perception/heatmap_visualizer/CMakeLists.txt index ed995491f2af6..75fa02a1cdcf7 100644 --- a/perception/heatmap_visualizer/CMakeLists.txt +++ b/perception/heatmap_visualizer/CMakeLists.txt @@ -16,4 +16,5 @@ rclcpp_components_register_node(heatmap_visualizer_component ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/heatmap_visualizer/README.md b/perception/heatmap_visualizer/README.md index dd8b6f2a7f2b2..747a158b3d487 100644 --- a/perception/heatmap_visualizer/README.md +++ b/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 | `["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 diff --git a/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml new file mode 100644 index 0000000000000..2da863ea5e37a --- /dev/null +++ b/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 diff --git a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml index e87ae4e63a4d1..7251d776b6110 100644 --- a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml +++ b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml @@ -2,20 +2,10 @@ - - - - - - + - - - - - - + diff --git a/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json new file mode 100644 index 0000000000000..cb67ae383c35a --- /dev/null +++ b/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": ["/**"] +} diff --git a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp index ad57d8638065f..d9337e0c9f61d 100644 --- a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp +++ b/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(declare_parameter("publish_frame_count")); + map_frame_ = declare_parameter("heatmap_frame_id"); + map_length_ = static_cast(declare_parameter("heatmap_length")); + map_resolution_ = static_cast(declare_parameter("heatmap_resolution")); + use_confidence_ = declare_parameter("use_confidence"); + class_names_ = declare_parameter>("class_names"); + rename_car_to_truck_and_bus_ = declare_parameter("rename_to_car"); width_ = static_cast(map_length_ / map_resolution_); height_ = static_cast(map_length_ / map_resolution_); diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 5e74e837bf186..ccd3612203af5 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/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 diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index c875611b16003..9ff638cb61876 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/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( diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index d42c9b3ca2aa2..6f47dfbbe31fb 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -140,6 +140,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.absence_traffic_light.creep_velocity"); ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter( node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance"); + ip.occlusion.attention_lane_crop_curvature_threshold = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 89cdeaae56723..10f1ed73bf435 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/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(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 7bd923ed6198c..e75c57ffb3fc7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/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; }; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index f381925b13208..2f3fcc6f6fde0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,22 @@ #include #include #include + +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 points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector 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 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 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); } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 6acc1d60443bf..a75545353fb7a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -109,7 +109,8 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel( std::vector 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 generateInterpolatedPath( const int lane_id, const std::set & associative_lane_ids, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 5c188f4aebebf..aed81d771e480 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -39,6 +39,7 @@ struct DebugData std::optional occlusion_first_stop_wall_pose{std::nullopt}; std::optional pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; std::optional intersection_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt};