feat(crosswalk): add option to slowdown at occluded crosswalks (#6122)

planning/behavior_velocity_crosswalk_module/CMakeLists.txt

@@ -6,10 +6,8 @@ autoware_package()
 pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/debug.cpp
-  src/manager.cpp
-  src/scene_crosswalk.cpp
-  src/util.cpp
+  DIRECTORY
+  src
 )
 
 ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -69,3 +69,20 @@
           bicycle: true # [-] whether to look and stop by BICYCLE objects
           motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.)
           pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects
+
+      # param for occlusions
+      occlusion:
+        enable: true  # if true, ego will slowdown around crosswalks that are occluded
+        occluded_object_velocity: 1.0 # [m/s] assumed velocity of objects that may come out of the occluded space
+        slow_down_velocity: 1.0  # [m/s]
+        time_buffer: 1.0  # [s] consecutive time with/without an occlusion to add/remove the slowdown
+        min_size: 0.5  # [m] minimum size of an occlusion (square side size)
+        free_space_max: 43  # [-] maximum value of a free space cell in the occupancy grid
+        occupied_min: 58    # [-] minimum value of an occupied cell in the occupancy grid
+        ignore_with_red_traffic_light: true  # [-] if true, occlusions at crosswalks with traffic lights are ignored
+        ignore_behind_predicted_objects: true  # [-] if true, occlusions behind predicted objects are ignored
+        ignore_velocity_thresholds:
+          default: 0.5  # [m/s] occlusions are only ignored behind objects with a higher or equal velocity
+          custom_labels: ["PEDESTRIAN"]  # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels)
+          custom_thresholds: [0.0]  # velocities of the custom labels
+        extra_predicted_objects_size: 0.5  # [m] extra size added to the objects for masking the occlusions

planning/behavior_velocity_crosswalk_module/package.xml

@@ -28,9 +28,14 @@
   <depend>behavior_velocity_planner_common</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>grid_map_core</depend>
+  <depend>grid_map_ros</depend>
+  <depend>grid_map_utils</depend>
+  <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>libboost-dev</depend>
   <depend>motion_utils</depend>
+  <depend>nav_msgs</depend>
   <depend>pcl_conversions</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -17,6 +17,7 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
+#include <algorithm>
 #include <limits>
 #include <memory>
 #include <set>
@@ -125,6 +126,46 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns + ".object_filtering.target_object.motorcycle");
   cp.look_pedestrian =
     getOrDeclareParameter<bool>(node, ns + ".object_filtering.target_object.pedestrian");
+
+  // param for occlusions
+  cp.occlusion_enable = getOrDeclareParameter<bool>(node, ns + ".occlusion.enable");
+  cp.occlusion_occluded_object_velocity =
+    getOrDeclareParameter<double>(node, ns + ".occlusion.occluded_object_velocity");
+  cp.occlusion_slow_down_velocity =
+    getOrDeclareParameter<float>(node, ns + ".occlusion.slow_down_velocity");
+  cp.occlusion_time_buffer = getOrDeclareParameter<double>(node, ns + ".occlusion.time_buffer");
+  cp.occlusion_min_size = getOrDeclareParameter<double>(node, ns + ".occlusion.min_size");
+  cp.occlusion_free_space_max = getOrDeclareParameter<int>(node, ns + ".occlusion.free_space_max");
+  cp.occlusion_occupied_min = getOrDeclareParameter<int>(node, ns + ".occlusion.occupied_min");
+  cp.occlusion_ignore_with_red_traffic_light =
+    getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_with_red_traffic_light");
+  cp.occlusion_ignore_behind_predicted_objects =
+    getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_behind_predicted_objects");
+
+  cp.occlusion_ignore_velocity_thresholds.resize(
+    autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1,
+    getOrDeclareParameter<double>(node, ns + ".occlusion.ignore_velocity_thresholds.default"));
+  const auto get_label = [](const std::string & s) {
+    if (s == "CAR") return autoware_auto_perception_msgs::msg::ObjectClassification::CAR;
+    if (s == "TRUCK") return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK;
+    if (s == "BUS") return autoware_auto_perception_msgs::msg::ObjectClassification::BUS;
+    if (s == "TRAILER") return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER;
+    if (s == "MOTORCYCLE")
+      return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE;
+    if (s == "BICYCLE") return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE;
+    if (s == "PEDESTRIAN")
+      return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
+    return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+  };
+  const auto custom_labels = getOrDeclareParameter<std::vector<std::string>>(
+    node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels");
+  const auto custom_velocities = getOrDeclareParameter<std::vector<double>>(
+    node, ns + ".occlusion.ignore_velocity_thresholds.custom_thresholds");
+  for (auto idx = 0UL; idx < std::min(custom_labels.size(), custom_velocities.size()); ++idx) {
+    cp.occlusion_ignore_velocity_thresholds[get_label(custom_labels[idx])] = custom_velocities[idx];
+  }
+  cp.occlusion_extra_objects_size =
+    getOrDeclareParameter<double>(node, ns + ".occlusion.extra_predicted_objects_size");
 }
 
 void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

@@ -0,0 +1,173 @@
+// Copyright 2024 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.
+
+#include "occluded_crosswalk.hpp"
+
+#include "behavior_velocity_crosswalk_module/util.hpp"
+
+#include <grid_map_ros/GridMapRosConverter.hpp>
+#include <grid_map_utils/polygon_iterator.hpp>
+
+#include <algorithm>
+#include <vector>
+
+namespace behavior_velocity_planner
+{
+bool is_occluded(
+  const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx,
+  const behavior_velocity_planner::CrosswalkModule::PlannerParam & params)
+{
+  grid_map::Index idx_offset;
+  for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) {
+    for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) {
+      const auto index = idx + idx_offset;
+      if ((index < grid_map.getSize()).all()) {
+        const auto cell_value = grid_map.at("layer", index);
+        if (
+          cell_value < params.occlusion_free_space_max ||
+          cell_value > params.occlusion_occupied_min)
+          return false;
+      }
+    }
+  }
+  return true;
+}
+
+lanelet::BasicPoint2d interpolate_point(
+  const lanelet::BasicSegment2d & segment, const double extra_distance)
+{
+  const auto direction_vector = (segment.second - segment.first).normalized();
+  return segment.second + extra_distance * direction_vector;
+}
+
+std::vector<lanelet::BasicPolygon2d> calculate_detection_areas(
+  const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin,
+  const double detection_range)
+{
+  std::vector<lanelet::BasicPolygon2d> detection_areas = {
+    crosswalk_lanelet.polygon2d().basicPolygon()};
+  const std::vector<std::function<lanelet::BasicSegment2d(lanelet::ConstLineString2d)>>
+    segment_getters = {
+      [](const auto & ls) -> lanelet::BasicSegment2d {
+        return {ls[1].basicPoint2d(), ls[0].basicPoint2d()};
+      },
+      [](const auto & ls) -> lanelet::BasicSegment2d {
+        return {ls[ls.size() - 2].basicPoint2d(), ls[ls.size() - 1].basicPoint2d()};
+      }};
+  if (
+    crosswalk_lanelet.centerline2d().size() > 1 && crosswalk_lanelet.leftBound2d().size() > 1 &&
+    crosswalk_lanelet.rightBound2d().size() > 1) {
+    for (const auto & segment_getter : segment_getters) {
+      const auto center_segment = segment_getter(crosswalk_lanelet.centerline2d());
+      const auto left_segment = segment_getter(crosswalk_lanelet.leftBound2d());
+      const auto right_segment = segment_getter(crosswalk_lanelet.rightBound2d());
+      const auto dist = lanelet::geometry::distance2d(center_segment.second, crosswalk_origin);
+      if (dist < detection_range) {
+        const auto target_left = interpolate_point(left_segment, detection_range - dist);
+        const auto target_right = interpolate_point(right_segment, detection_range - dist);
+        detection_areas.push_back(
+          {left_segment.second, target_left, target_right, right_segment.second});
+      }
+    }
+  }
+  return detection_areas;
+}
+
+std::vector<autoware_auto_perception_msgs::msg::PredictedObject> select_and_inflate_objects(
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
+  const std::vector<double> velocity_thresholds, const double inflate_size)
+{
+  std::vector<autoware_auto_perception_msgs::msg::PredictedObject> selected_objects;
+  for (const auto & o : objects) {
+    const auto vel_threshold = velocity_thresholds[o.classification.front().label];
+    if (o.kinematics.initial_twist_with_covariance.twist.linear.x >= vel_threshold) {
+      auto selected_object = o;
+      selected_object.shape.dimensions.x += inflate_size;
+      selected_object.shape.dimensions.y += inflate_size;
+      selected_objects.push_back(selected_object);
+    }
+  }
+  return selected_objects;
+}
+
+void clear_occlusions_behind_objects(
+  grid_map::GridMap & grid_map,
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects)
+{
+  const auto angle_cmp = [&](const auto & p1, const auto & p2) {
+    const auto d1 = p1 - grid_map.getPosition();
+    const auto d2 = p2 - grid_map.getPosition();
+    return std::atan2(d1.y(), d1.x()) < std::atan2(d2.y(), d2.x());
+  };
+  const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition();
+  const auto range = grid_map.getLength().maxCoeff() / 2.0;
+  for (auto object : objects) {
+    const lanelet::BasicPoint2d object_position = {
+      object.kinematics.initial_pose_with_covariance.pose.position.x,
+      object.kinematics.initial_pose_with_covariance.pose.position.y};
+    if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) {
+      lanelet::BasicPoints2d edge_points;
+      const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
+      for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point);
+      std::sort(edge_points.begin(), edge_points.end(), angle_cmp);
+      // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range));
+      grid_map::Polygon polygon_to_clear;
+      polygon_to_clear.addVertex(edge_points.front());
+      polygon_to_clear.addVertex(
+        interpolate_point({grid_map_position, edge_points.front()}, 10.0 * range));
+      polygon_to_clear.addVertex(
+        interpolate_point({grid_map_position, edge_points.back()}, 10.0 * range));
+      polygon_to_clear.addVertex(edge_points.back());
+      for (grid_map_utils::PolygonIterator it(grid_map, polygon_to_clear); !it.isPastEnd(); ++it)
+        grid_map.at("layer", *it) = 0;
+    }
+  }
+}
+
+bool is_crosswalk_occluded(
+  const lanelet::ConstLanelet & crosswalk_lanelet,
+  const nav_msgs::msg::OccupancyGrid & occupancy_grid,
+  const geometry_msgs::msg::Point & path_intersection, const double detection_range,
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & dynamic_objects,
+  const behavior_velocity_planner::CrosswalkModule::PlannerParam & params)
+{
+  grid_map::GridMap grid_map;
+  grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map);
+
+  if (params.occlusion_ignore_behind_predicted_objects) {
+    const auto objects = select_and_inflate_objects(
+      dynamic_objects, params.occlusion_ignore_velocity_thresholds,
+      params.occlusion_extra_objects_size);
+    clear_occlusions_behind_objects(grid_map, objects);
+  }
+  const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution());
+  for (const auto & detection_area : calculate_detection_areas(
+         crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) {
+    grid_map::Polygon poly;
+    for (const auto & p : detection_area) poly.addVertex(grid_map::Position(p.x(), p.y()));
+    for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter)
+      if (is_occluded(grid_map, min_nb_of_cells, *iter, params)) return true;
+  }
+  return false;
+}
+
+double calculate_detection_range(
+  const double occluded_object_velocity, const double dist_ego_to_crosswalk,
+  const double ego_velocity)
+{
+  constexpr double min_ego_velocity = 1.0;
+  const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity);
+  return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0;
+}
+}  // namespace behavior_velocity_planner

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp

@@ -0,0 +1,94 @@
+// Copyright 2024 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.
+
+#ifndef OCCLUDED_CROSSWALK_HPP_
+#define OCCLUDED_CROSSWALK_HPP_
+
+#include "scene_crosswalk.hpp"
+
+#include <grid_map_core/GridMap.hpp>
+#include <rclcpp/time.hpp>
+
+#include <geometry_msgs/msg/point.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/Point.h>
+
+#include <optional>
+#include <vector>
+
+namespace behavior_velocity_planner
+{
+/// @brief check if the gridmap is occluded at the given index
+/// @param [in] grid_map input grid map
+/// @param [in] min_nb_of_cells minimum number of occluded cells needed to detect an occlusion (as
+/// side of a square centered at the index)
+/// @param [in] idx target index in the grid map
+/// @param [in] params parameters
+/// @return true if the index is occluded
+bool is_occluded(
+  const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx,
+  const behavior_velocity_planner::CrosswalkModule::PlannerParam & params);
+
+/// @brief interpolate a point beyond the end of the given segment
+/// @param [in] segment input segment
+/// @param [in] extra_distance desired distance beyond the end of the segment
+/// @return interpolated point beyond the end of the segment
+lanelet::BasicPoint2d interpolate_point(
+  const lanelet::BasicSegment2d & segment, const double extra_distance);
+
+/// @brief check if the crosswalk is occluded
+/// @param crosswalk_lanelet lanelet of the crosswalk
+/// @param occupancy_grid occupancy grid with the occlusion information
+/// @param path_intersection intersection between the crosswalk and the ego path
+/// @param detection_range range away from the crosswalk until occlusions are considered
+/// @param dynamic_objects dynamic objects
+/// @param params parameters
+/// @return true if the crosswalk is occluded
+bool is_crosswalk_occluded(
+  const lanelet::ConstLanelet & crosswalk_lanelet,
+  const nav_msgs::msg::OccupancyGrid & occupancy_grid,
+  const geometry_msgs::msg::Point & path_intersection, const double detection_range,
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & dynamic_objects,
+  const behavior_velocity_planner::CrosswalkModule::PlannerParam & params);
+
+/// @brief calculate the distance away from the crosswalk that should be checked for occlusions
+/// @param occluded_objects_velocity assumed velocity of the objects coming out of occlusions
+/// @param dist_ego_to_crosswalk distance between ego and the crosswalk
+/// @param ego_velocity current velocity of ego
+double calculate_detection_range(
+  const double occluded_object_velocity, const double dist_ego_to_crosswalk,
+  const double ego_velocity);
+
+/// @brief select a subset of objects meeting the velocity threshold and inflate their shape
+/// @param objects input objects
+/// @param velocity_threshold minimum velocity for an object to be selected
+/// @param skip_pedestrians if true, pedestrians are not selected regardless of their velocities
+/// @param inflate_size [m] size by which the shape of the selected objects are inflated
+/// @return selected and inflated objects
+std::vector<autoware_auto_perception_msgs::msg::PredictedObject> select_and_inflate_objects(
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
+  const double velocity_threshold, const bool skip_pedestrians, const double inflate_size);
+
+/// @brief clear occlusions behind the given objects
+/// @details masks behind the object assuming rays from the center of the grid map
+/// @param grid_map grid map
+/// @param objects objects
+void clear_occlusions_behind_objects(
+  grid_map::GridMap & grid_map,
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects);
+}  // namespace behavior_velocity_planner
+
+#endif  // OCCLUDED_CROSSWALK_HPP_