diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 3217b3828b8d7..69dd1ced695ec 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/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) diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 4c7b214763c31..7d6acbfc6aa9a 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -193,6 +193,50 @@ document. | `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | | `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | +### Occlusion + +This feature makes ego slow down for a crosswalk that is occluded. + +Occlusion of the crosswalk is determined using the occupancy grid. +An occlusion is a square of size `min_size` of occluded cells +(i.e., their values are between `free_space_max` and `occupied_min`) +of size `min_size`. +If an occlusion is found within range of the crosswalk, +then the velocity limit at the crosswalk is set to `slow_down_velocity` (or more to not break limits set by `max_slow_down_jerk` and `max_slow_down_accel`). +The range is calculated from the intersection between the ego path and the crosswalk and is equal to the time taken by ego to reach the crosswalk times the `occluded_object_velocity`. +This range is meant to be large when ego is far from the crosswalk and small when ego is close. + +In order to avoid flickering decisions, a time buffer can be used such that the decision to add (or remove) the slow down is only taken +after an occlusion is detected (or not detected) for a consecutive time defined by the `time_buffer` parameter. + +To ignore occlusions when the pedestrian light is red, `ignore_with_red_traffic_light` should be set to true. + +To ignore temporary occlusions caused by moving objects, +`ignore_behind_predicted_objects` should be set to true. +By default, occlusions behind an object with velocity higher than `ignore_velocity_thresholds.default` are ignored. +This velocity threshold can be specified depending on the object type by specifying the object class label and velocity threshold in the parameter lists `ignore_velocity_thresholds.custom_labels` and `ignore_velocity_thresholds.custom_thresholds`. +To inflate the masking behind objects, their footprint can be made bigger using `extra_predicted_objects_size`. + +
+ ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600} +
+ +| Parameter | Unit | Type | Description | +| ---------------------------------------------- | ----- | ----------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | +| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | +| `slow_down_velocity` | [m/s] | double | slow down velocity | +| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | +| `min_size` | [m] | double | minimum size of an occlusion (square side size) | +| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | +| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | +| `ignore_with_red_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | +| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | +| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | +| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_auto_perception_msgs::msg::ObjectClassification` for all the labels) | +| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | +| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | + ### Others In the `common` namespace, the following parameters are defined. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index cf3d2c621322b..09ce64593ff46 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -68,3 +68,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 diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg b/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg new file mode 100644 index 0000000000000..a98995e0da3a9 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ occluded area +
+
+
+ + + + + + + + + + + +
+
+
obstacle
+
+
+ + + + + + + diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 9eef11e72239e..2c38a9836e0af 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -28,9 +28,14 @@ behavior_velocity_planner_common eigen geometry_msgs + grid_map_core + grid_map_ros + grid_map_utils + interpolation lanelet2_extension libboost-dev motion_utils + nav_msgs pcl_conversions pluginlib rclcpp diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 1fa9247548add..39f88d5389c2b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -123,6 +124,46 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".object_filtering.target_object.motorcycle"); cp.look_pedestrian = getOrDeclareParameter(node, ns + ".object_filtering.target_object.pedestrian"); + + // param for occlusions + cp.occlusion_enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + cp.occlusion_occluded_object_velocity = + getOrDeclareParameter(node, ns + ".occlusion.occluded_object_velocity"); + cp.occlusion_slow_down_velocity = + getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); + cp.occlusion_time_buffer = getOrDeclareParameter(node, ns + ".occlusion.time_buffer"); + cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); + cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + cp.occlusion_ignore_with_red_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.ignore_with_red_traffic_light"); + cp.occlusion_ignore_behind_predicted_objects = + getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); + + cp.occlusion_ignore_velocity_thresholds.resize( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, + getOrDeclareParameter(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>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); + const auto custom_velocities = getOrDeclareParameter>( + 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(node, ns + ".occlusion.extra_predicted_objects_size"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp new file mode 100644 index 0000000000000..2455b36d5883f --- /dev/null +++ b/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 +#include + +#include +#include + +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 calculate_detection_areas( + const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin, + const double detection_range) +{ + std::vector detection_areas = { + crosswalk_lanelet.polygon2d().basicPolygon()}; + const std::vector> + 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 select_and_inflate_objects( + const std::vector & objects, + const std::vector velocity_thresholds, const double inflate_size) +{ + std::vector 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 & 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 & 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 diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp new file mode 100644 index 0000000000000..a76fdeb549b88 --- /dev/null +++ b/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 +#include + +#include +#include + +#include +#include + +#include +#include + +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 & 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 select_and_inflate_objects( + const std::vector & 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 & objects); +} // namespace behavior_velocity_planner + +#endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index cee6975df3155..00ace56f41d7c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-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. @@ -14,6 +14,8 @@ #include "scene_crosswalk.hpp" +#include "occluded_crosswalk.hpp" + #include #include #include @@ -31,6 +33,7 @@ #include #include +#include #include #include #include @@ -228,7 +231,48 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Apply safety slow down speed if defined in Lanelet2 map if (crosswalk_.hasAttribute("safety_slow_down_speed")) { - applySafetySlowDownSpeed(*path, path_intersects); + applySafetySlowDownSpeed( + *path, path_intersects, + static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); + } + // Apply safety slow down speed if the crosswalk is occluded + const auto now = clock_->now(); + const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) { + return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer); + }; + const auto is_crosswalk_ignored = + (planner_param_.occlusion_ignore_with_red_traffic_light && isRedSignalForPedestrians()) || + crosswalk_.hasAttribute("skip_occluded_slowdown"); + if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) { + const auto dist_ego_to_crosswalk = + calcSignedArcLength(path->points, ego_pos, path_intersects.front()); + const auto detection_range = + planner_data_->vehicle_info_.max_lateral_offset_m + + calculate_detection_range( + planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk, + planner_data_->current_velocity->twist.linear.x); + const auto is_ego_on_the_crosswalk = + dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m; + if (!is_ego_on_the_crosswalk) { + if (is_crosswalk_occluded( + crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), detection_range, + objects_ptr->objects, planner_param_)) { + if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = now; + if (cmp_with_time_buffer(current_initial_occlusion_time_, std::greater_equal{})) + most_recent_occlusion_time_ = now; + } else if (!cmp_with_time_buffer(most_recent_occlusion_time_, std::greater{})) { + current_initial_occlusion_time_.reset(); + } + + if (cmp_with_time_buffer(most_recent_occlusion_time_, std::less_equal{})) { + const auto target_velocity = calcTargetVelocity(path_intersects.front(), *path); + applySafetySlowDownSpeed( + *path, path_intersects, + std::max(target_velocity, planner_param_.occlusion_slow_down_velocity)); + } else { + most_recent_occlusion_time_.reset(); + } + } } recordTime(2); @@ -666,8 +710,6 @@ std::optional CrosswalkModule::getCollisionPoint( { stop_watch_.tic(__func__); - std::vector ret{}; - const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; const auto & ego_pos = planner_data_->current_odometry->pose.position; @@ -786,7 +828,8 @@ CollisionPoint CrosswalkModule::createCollisionPoint( } void CrosswalkModule::applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects) + PathWithLaneId & output, const std::vector & path_intersects, + const float safety_slow_down_speed) { if (path_intersects.empty()) { return; @@ -795,10 +838,6 @@ void CrosswalkModule::applySafetySlowDownSpeed( const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_path = output; - // Safety slow down speed in [m/s] - const auto & safety_slow_down_speed = - static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get()); - if (!passed_safety_slow_point_) { // Safety slow down distance [m] const double safety_slow_down_distance = @@ -1051,6 +1090,7 @@ void CrosswalkModule::updateObjectState( object.kinematics.initial_pose_with_covariance.pose, object.shape, getLabelColor(collision_state)); } + object_info_manager_.finalize(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d1196bb7b4a9a..5ce5d4020928a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -154,6 +154,18 @@ class CrosswalkModule : public SceneModuleInterface bool look_bicycle; bool look_motorcycle; bool look_pedestrian; + // param for occlusions + bool occlusion_enable; + double occlusion_occluded_object_velocity; + float occlusion_slow_down_velocity; + double occlusion_time_buffer; + double occlusion_min_size; + int occlusion_free_space_max; + int occlusion_occupied_min; + bool occlusion_ignore_with_red_traffic_light; + bool occlusion_ignore_behind_predicted_objects; + std::vector occlusion_ignore_velocity_thresholds; + double occlusion_extra_objects_size; }; struct ObjectInfo @@ -309,7 +321,8 @@ class CrosswalkModule : public SceneModuleInterface private: // main functions void applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects); + PathWithLaneId & output, const std::vector & path_intersects, + const float speed); std::optional> getStopPointWithMargin( const PathWithLaneId & ego_path, @@ -430,6 +443,10 @@ class CrosswalkModule : public SceneModuleInterface // whether use regulatory element const bool use_regulatory_element_; + + // occluded space time buffer + std::optional current_initial_occlusion_time_; + std::optional most_recent_occlusion_time_; }; } // namespace behavior_velocity_planner