diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ddbf26ebc485c..95ddb97d2bbb0 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -178,7 +178,6 @@ planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamo planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index fdb6bc1d90643..463740d978ca6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -32,7 +32,6 @@ - @@ -164,11 +163,6 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'autoware::behavior_velocity_planner::SpeedBumpModulePlugin, '")" if="$(var launch_speed_bump_module)" /> - - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 34e8c9d31d837..196e7b2c7262c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -2,7 +2,7 @@ - + @@ -11,7 +11,7 @@ diff --git a/planning/.pages b/planning/.pages index a16f2a13628a1..c144941662a5d 100644 --- a/planning/.pages +++ b/planning/.pages @@ -29,7 +29,6 @@ nav: - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module - 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module - 'Occlusion Spot': planning/behavior_velocity_occlusion_spot_module - - 'Out of Lane': planning/behavior_velocity_out_of_lane_module - 'Run Out': planning/autoware_behavior_velocity_run_out_module - 'Speed Bump': planning/behavior_velocity_speed_bump_module - 'Stop Line': planning/behavior_velocity_stop_line_module diff --git a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index d4478f87f0610..f52d9637c2134 100644 --- a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -24,7 +24,6 @@ - @@ -68,7 +67,6 @@ - diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index 4d5dc17264df0..70fd92fd9e072 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -76,7 +76,6 @@ behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module - behavior_velocity_out_of_lane_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_traffic_light_module diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index cdff8af4ac56e..cf8b918418c53 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -81,7 +81,6 @@ std::shared_ptr generateNode() module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::OutOfLaneModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin"); std::vector params; @@ -108,7 +107,6 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config_no_prefix("stop_line"), get_behavior_velocity_module_config_no_prefix("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config_no_prefix("out_of_lane"), get_behavior_velocity_module_config_no_prefix("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt deleted file mode 100644 index e8191f9b632d5..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_out_of_lane_module) - -find_package(autoware_cmake REQUIRED) -autoware_package() -pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) - -ament_auto_add_library(${PROJECT_NAME} SHARED - DIRECTORY src -) - -ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md deleted file mode 100644 index 99396eb0edf22..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ /dev/null @@ -1,165 +0,0 @@ -## Out of Lane - -### Role - -`out_of_lane` is the module that decelerates and stops to prevent the ego vehicle from entering another lane with incoming dynamic objects. - -### Activation Timing - -This module is activated if `launch_out_of_lane` is set to true. - -### Inner-workings / Algorithms - -The algorithm is made of the following steps. - -1. Calculate the ego path footprints (red). -2. Calculate the other lanes (magenta). -3. Calculate the overlapping ranges between the ego path footprints and the other lanes (green). -4. For each overlapping range, decide if a stop or slow down action must be taken. -5. For each action, insert the corresponding stop or slow down point in the path. - -![overview](./docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png) - -#### 1. Ego Path Footprints - -In this first step, the ego footprint is projected at each path point and are eventually inflated based on the `extra_..._offset` parameters. - -#### 2. Other lanes - -In the second step, the set of lanes to consider for overlaps is generated. -This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevant lanelets. -The selection distance is chosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`. - -A lanelet is deemed non-relevant if it meets one of the following conditions. - -- It is part of the lanelets followed by the ego path. -- It contains the rear point of the ego footprint. -- It follows one of the ego path lanelets. - -#### 3. Overlapping ranges - -In the third step, overlaps between the ego path footprints and the other lanes are calculated. -For each pair of other lane $l$ and ego path footprint $f$, we calculate the overlapping polygons using `boost::geometry::intersection`. -For each overlapping polygon found, if the distance inside the other lane $l$ is above the `overlap.minimum_distance` threshold, then the overlap is ignored. -Otherwise, the arc length range (relative to the ego path) and corresponding points of the overlapping polygons are stored. -Ultimately, for each other lane $l$, overlapping ranges of successive overlaps are built with the following information: - -- overlapped other lane $l$. -- start and end ego path indexes. -- start and end ego path arc lengths. -- start and end overlap points. - -#### 4. Decisions - -In the fourth step, a decision to either slow down or stop before each overlapping range is taken based on the dynamic objects. -The conditions for the decision depend on the value of the `mode` parameter. - -Whether it is decided to slow down or stop is determined by the distance between the ego vehicle and the start of the overlapping range (in arc length along the ego path). -If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used. -If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used. - - - - - - -
- - - -
- -##### Threshold - -With the `mode` set to `"threshold"`, -a decision to stop or slow down before a range is made if -an incoming dynamic object is estimated to reach the overlap within `threshold.time_threshold`. - -##### TTC (time to collision) - -With the `mode` set to `"ttc"`, -estimates for the times when ego and the dynamic objects reach the start and end of the overlapping range are calculated. -This is then used to calculate the time to collision over the period where ego crosses the overlap. -If the time to collision is predicted to go bellow the `ttc.threshold`, the decision to stop or slow down is made. - -##### Intervals - -With the `mode` set to `"intervals"`, -the estimated times when ego and the dynamic objects reach the start and end points of -the overlapping range are used to create time intervals. -These intervals can be made shorter or longer using the -`intervals.ego_time_buffer` and `intervals.objects_time_buffer` parameters. -If the time interval of ego overlaps with the time interval of an object, the decision to stop or slow down is made. - -##### Time estimates - -###### Ego - -To estimate the times when ego will reach an overlap, it is assumed that ego travels along its path -at its current velocity or at half the velocity of the path points, whichever is higher. - -###### Dynamic objects - -Two methods are used to estimate the time when a dynamic objects with reach some point. -If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. -Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. - -#### 5. Path update - -Finally, for each decision to stop or slow down before an overlapping range, -a point is inserted in the path. -For a decision taken for an overlapping range with a lane $l$ starting at ego path point index $i$, -a point is inserted in the path between index $i$ and $i-1$ such that the ego footprint projected at the inserted point does not overlap $l$. -Such point with no overlap must exist since, by definition of the overlapping range, -we know that there is no overlap at $i-1$. - -If the point would cause a higher deceleration than allowed by the `max_accel` parameter (node parameter), -it is skipped. - -Moreover, parameter `action.distance_buffer` adds an extra distance between the ego footprint and the overlap when possible. - -### Module Parameters - -| Parameter | Type | Description | -| ----------------------------- | ------ | --------------------------------------------------------------------------------- | -| `mode` | string | [-] mode used to consider a dynamic object. Candidates: threshold, intervals, ttc | -| `skip_if_already_overlapping` | bool | [-] if true, do not run this module when ego already overlaps another lane | - -| Parameter /threshold | Type | Description | -| -------------------- | ------ | ---------------------------------------------------------------- | -| `time_threshold` | double | [s] consider objects that will reach an overlap within this time | - -| Parameter /intervals | Type | Description | -| --------------------- | ------ | ------------------------------------------------------- | -| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer | -| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer | - -| Parameter /ttc | Type | Description | -| -------------- | ------ | ------------------------------------------------------------------------------------------------------ | -| `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | - -| Parameter /objects | Type | Description | -| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | -| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | - -| Parameter /overlap | Type | Description | -| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | -| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | -| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | - -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | - -| Parameter /ego | Type | Description | -| -------------------- | ------ | ---------------------------------------------------- | -| `extra_front_offset` | double | [m] extra front distance to add to the ego footprint | -| `extra_rear_offset` | double | [m] extra rear distance to add to the ego footprint | -| `extra_left_offset` | double | [m] extra left distance to add to the ego footprint | -| `extra_right_offset` | double | [m] extra right distance to add to the ego footprint | diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml deleted file mode 100644 index e4d08367ee4f7..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ /dev/null @@ -1,44 +0,0 @@ -/**: - ros__parameters: - out_of_lane: # module to stop or slowdown before overlapping another lane with other objects - mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" - skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane - ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored - - threshold: - time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time - intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego - ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer - objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer - ttc: - threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap - - objects: - minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored - use_predicted_paths: true # if true, use the predicted paths to estimate future positions. - # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. - predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. - distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane - cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights - - overlap: - minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered - extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) - - action: # action to insert in the path if an object causes a conflict at an overlap - skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed - precision: 0.1 # [m] precision when inserting a stop pose in the path - distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane - min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled - slowdown: - distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap - velocity: 2.0 # [m/s] slowdown velocity - stop: - distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap - - ego: - min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego - extra_front_offset: 0.0 # [m] extra front distance - extra_rear_offset: 0.0 # [m] extra rear distance - extra_right_offset: 0.0 # [m] extra right distance - extra_left_offset: 0.0 # [m] extra left distance diff --git a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png b/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png deleted file mode 100644 index f095467b3b0ac..0000000000000 Binary files a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png and /dev/null differ diff --git a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_slow.png b/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_slow.png deleted file mode 100644 index 2f358975b9491..0000000000000 Binary files a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_slow.png and /dev/null differ diff --git a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_stop.png b/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_stop.png deleted file mode 100644 index fdb75ac0c6eb8..0000000000000 Binary files a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_stop.png and /dev/null differ diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml deleted file mode 100644 index 6ea60dd1398f5..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - behavior_velocity_out_of_lane_module - 0.1.0 - The behavior_velocity_out_of_lane_module package - - Maxime Clement - Tomoya Kimura - Shumpei Wakabayashi - Takayuki Murooka - - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - autoware_behavior_velocity_planner_common - autoware_perception_msgs - autoware_planning_msgs - geometry_msgs - lanelet2_extension - libboost-dev - motion_utils - pluginlib - rclcpp - route_handler - tf2 - tier4_autoware_utils - tier4_planning_msgs - traffic_light_utils - vehicle_info_util - visualization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/planning/behavior_velocity_out_of_lane_module/plugins.xml b/planning/behavior_velocity_out_of_lane_module/plugins.xml deleted file mode 100644 index f85eb50367ccc..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp deleted file mode 100644 index 5e3877753d203..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ /dev/null @@ -1,106 +0,0 @@ -// 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. - -#ifndef CALCULATE_SLOWDOWN_POINTS_HPP_ -#define CALCULATE_SLOWDOWN_POINTS_HPP_ - -#include "footprint.hpp" -#include "types.hpp" - -#include -#include - -#include - -#include -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ - -/// @brief estimate whether ego can decelerate without breaking the deceleration limit -/// @details assume ego wants to reach the target point at the target velocity -/// @param [in] ego_data ego data -/// @param [in] point target point -/// @param [in] target_vel target_velocity -bool can_decelerate( - const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel) -{ - if (ego_data.velocity < 0.5) return true; - const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( - ego_data.path.points, ego_data.pose.position, point.point.pose.position); - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); -} - -/// @brief calculate the last pose along the path where ego does not overlap the lane to avoid -/// @param [in] ego_data ego data -/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) -/// @param [in] footprint the ego footprint -/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits -/// @param [in] params parameters -/// @return the last pose that is not out of lane (if found) -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params) -{ - const auto from_arc_length = - motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx); - const auto to_arc_length = - motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx); - PathPointWithLaneId interpolated_point; - for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { - // TODO(Maxime): binary search - interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l); - const auto respect_decel_limit = - !params.skip_if_over_max_decel || prev_slowdown_point || - can_decelerate(ego_data, interpolated_point, decision.velocity); - const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.point.pose); - const auto is_overlap_lane = boost::geometry::overlaps( - interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); - const auto is_overlap_extra_lane = - prev_slowdown_point && - boost::geometry::overlaps( - interpolated_footprint, - prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); - if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) - return interpolated_point; - } - return std::nullopt; -} - -/// @brief calculate the slowdown point to insert in the path -/// @param ego_data ego data (path, velocity, etc) -/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) -/// @param prev_slowdown_point previously calculated slowdown point -/// @param params parameters -/// @return optional slowdown point to insert in the path -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional prev_slowdown_point, PlannerParam params) -{ - params.extra_front_offset += params.dist_buffer; - const auto base_footprint = make_base_footprint(params); - - // search for the first slowdown decision for which a stop point can be inserted - for (const auto & decision : decisions) { - const auto last_in_lane_pose = - calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); - if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; - } - return std::nullopt; -} -} // namespace autoware::behavior_velocity_planner::out_of_lane -#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp deleted file mode 100644 index 08e66f99e2a0c..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// 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. - -#include "debug.hpp" - -#include - -#include - -namespace autoware::behavior_velocity_planner::out_of_lane::debug -{ -namespace -{ -visualization_msgs::msg::Marker get_base_marker() -{ - visualization_msgs::msg::Marker base_marker; - base_marker.header.frame_id = "map"; - base_marker.header.stamp = rclcpp::Time(0); - base_marker.id = 0; - base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); - return base_marker; -} -} // namespace -void add_footprint_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = "footprints"; - for (const auto & f : footprints) { - debug_marker.points.clear(); - for (const auto & p : f) - debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - debug_marker.points.clear(); - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_current_overlap_marker( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = "current_overlap"; - debug_marker.points.clear(); - for (const auto & p : current_footprint) - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z)); - if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); - if (current_overlapped_lanelets.empty()) - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); - else - debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - for (const auto & ll : current_overlapped_lanelets) { - debug_marker.points.clear(); - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_lanelet_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = ns; - debug_marker.color = color; - for (const auto & ll : lanelets) { - debug_marker.points.clear(); - - // add a small z offset to draw above the lanelet map - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_range_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, const double z, - const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = "ranges"; - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); - for (const auto & range : ranges) { - debug_marker.points.clear(); - debug_marker.points.push_back( - path.points[first_ego_idx + range.entering_path_idx].point.pose.position); - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - for (const auto & overlap : range.debug.overlaps) { - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); - } - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - range.exiting_point.x(), range.exiting_point.y(), z)); - debug_marker.points.push_back( - path.points[first_ego_idx + range.exiting_path_idx].point.pose.position); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); - debug_marker.action = debug_marker.ADD; - debug_marker.id = 0; - debug_marker.ns = "decisions"; - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); - debug_marker.points.clear(); - for (const auto & range : ranges) { - debug_marker.type = debug_marker.LINE_STRIP; - if (range.debug.decision) { - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - debug_marker.points.push_back( - range.debug.object->kinematics.initial_pose_with_covariance.pose.position); - } - debug_marker_array.markers.push_back(debug_marker); - debug_marker.points.clear(); - debug_marker.id++; - - debug_marker.type = debug_marker.TEXT_VIEW_FACING; - debug_marker.pose.position.x = range.entering_point.x(); - debug_marker.pose.position.y = range.entering_point.y(); - debug_marker.pose.position.z = z; - std::stringstream ss; - ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time - << "\n"; - if (range.debug.object) { - debug_marker.pose.position.x += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; - debug_marker.pose.position.y += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; - debug_marker.pose.position.x /= 2; - debug_marker.pose.position.y /= 2; - ss << "Obj: " << range.debug.times.object.enter_time << " - " - << range.debug.times.object.exit_time << "\n"; - } - debug_marker.scale.z = 1.0; - debug_marker.text = ss.str(); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -} // namespace autoware::behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp deleted file mode 100644 index 05eed6b35c13c..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// 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. - -#ifndef DEBUG_HPP_ -#define DEBUG_HPP_ - -#include "types.hpp" - -#include - -#include - -#include - -namespace autoware::behavior_velocity_planner::out_of_lane::debug -{ -/// @brief add footprint markers to the given marker array -/// @param [inout] debug_marker_array marker array -/// @param [in] footprints footprints to turn into markers -/// @param [in] z z value to use for the markers -/// @param [in] prev_nb previous number of markers (used to delete the extra ones) -void add_footprint_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb); -/// @brief add footprint markers to the given marker array -/// @param [inout] debug_marker_array marker array -/// @param [in] current_footprint footprint to turn into a marker -/// @param [in] current_overlapped_lanelets lanelets to turn into markers -/// @param [in] z z value to use for the markers -/// @param [in] prev_nb previous number of markers (used to delete the extra ones) -void add_current_overlap_marker( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb); -/// @brief add footprint markers to the given marker array -/// @param [inout] debug_marker_array marker array -/// @param [in] lanelets lanelets to turn into markers -/// @param [in] ns namespace of the markers -/// @param [in] color namespace of the markers -/// @param [in] prev_nb previous number of markers (used to delete the extra ones) -void add_lanelet_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color, const size_t prev_nb); -/// @brief add ranges markers to the given marker array -/// @param [inout] debug_marker_array marker array -/// @param [in] ranges ranges to turn into markers -/// @param [in] path modified ego path that was used to calculate the ranges -/// @param [in] first_path_idx first idx of ego on the path -/// @param [in] z z value to use for the markers -/// @param [in] prev_nb previous number of markers (used to delete the extra ones) -void add_range_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, - const double z, const size_t prev_nb); -} // namespace autoware::behavior_velocity_planner::out_of_lane::debug - -#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp deleted file mode 100644 index 62416c8d38ed6..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ /dev/null @@ -1,383 +0,0 @@ -// 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. - -#include "decisions.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -namespace autoware::behavior_velocity_planner::out_of_lane -{ -double distance_along_path(const EgoData & ego_data, const size_t target_idx) -{ - return motion_utils::calcSignedArcLength( - ego_data.path.points, ego_data.pose.position, ego_data.first_path_idx + target_idx); -} - -double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) -{ - const auto dist = distance_along_path(ego_data, target_idx); - const auto v = std::max( - std::max(ego_data.velocity, min_velocity), - ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * - 0.5); - return dist / v; -} - -bool object_is_incoming( - const lanelet::BasicPoint2d & object_position, - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lane) -{ - const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); - if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; - for (const auto & lls : lanelets) - for (const auto & ll : lls) - if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; - return false; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger) -{ - // skip the dynamic object if it is not in a lane preceding the overlapped lane - // lane changes are intentionally not considered - const auto object_point = lanelet::BasicPoint2d( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - - const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; - const auto max_lon_deviation = object.shape.dimensions.x / 2.0; - auto worst_enter_time = std::optional(); - auto worst_exit_time = std::optional(); - - for (const auto & predicted_path : object.kinematics.predicted_paths) { - const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); - if (unique_path_points.size() < 2) continue; - const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); - const auto enter_point = - geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_segment_idx = - motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); - const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, enter_segment_idx, enter_point); - const auto enter_lat_dist = - std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); - const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( - unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); - const auto enter_offset_ratio = enter_offset / enter_segment_length; - const auto enter_time = - static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; - - const auto exit_point = - geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_segment_idx = - motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); - const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, exit_segment_idx, exit_point); - const auto exit_lat_dist = - std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); - const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( - unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); - const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); - const auto exit_time = - static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; - - RCLCPP_DEBUG( - logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, - enter_time, exit_time); - // predicted path is too far from the overlapping range to be relevant - const auto is_far_from_entering_point = enter_lat_dist > max_deviation; - const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; - if (is_far_from_entering_point && is_far_from_exiting_point) { - RCLCPP_DEBUG( - logger, - " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", - is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, - max_deviation); - continue; - } - const auto is_last_predicted_path_point = - (exit_segment_idx + 2 == unique_path_points.size() || - enter_segment_idx + 2 == unique_path_points.size()); - const auto does_not_reach_overlap = - is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); - if (does_not_reach_overlap) { - RCLCPP_DEBUG( - logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", - std::min(exit_offset, enter_offset), max_lon_deviation); - continue; - } - - const auto same_driving_direction_as_ego = enter_time < exit_time; - if (same_driving_direction_as_ego) { - worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; - } else { - worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; - } - } - if (worst_enter_time && worst_exit_time) { - RCLCPP_DEBUG( - logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, - *worst_exit_time); - return std::make_pair(*worst_enter_time, *worst_exit_time); - } - RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); - return {}; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger) -{ - const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; - const auto object_point = lanelet::BasicPoint2d(p.x, p.y); - const auto half_size = object.shape.dimensions.x / 2.0; - lanelet::ConstLanelets object_lanelets; - for (const auto & ll : inputs.lanelets) - if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) - object_lanelets.push_back(ll); - - geometry_msgs::msg::Pose pose; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - const auto range_size = std::abs(range_enter_length - range_exit_length); - auto worst_enter_dist = std::optional(); - auto worst_exit_dist = std::optional(); - for (const auto & lane : object_lanelets) { - const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); - RCLCPP_DEBUG( - logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); - if (path) { - lanelet::ConstLanelets lls; - for (const auto & ll : *path) lls.push_back(ll); - pose.position.set__x(object_point.x()).set__y(object_point.y()); - const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - RCLCPP_DEBUG( - logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, - enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, - range.exiting_point.x(), range.exiting_point.y()); - const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; - if (already_entered_range) continue; - // multiple paths to the overlap -> be conservative and use the "worst" case - // "worst" = min/max arc length depending on if the lane is running opposite to the ego path - const auto is_opposite = enter_dist > exit_dist; - if (!worst_enter_dist) - worst_enter_dist = enter_dist; - else if (is_opposite) - worst_enter_dist = std::max(*worst_enter_dist, enter_dist); - else - worst_enter_dist = std::min(*worst_enter_dist, enter_dist); - if (!worst_exit_dist) - worst_exit_dist = exit_dist; - else if (is_opposite) - worst_exit_dist = std::max(*worst_exit_dist, exit_dist); - else - worst_exit_dist = std::min(*worst_exit_dist, exit_dist); - } - } - if (worst_enter_dist && worst_exit_dist) { - const auto v = object.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::make_pair((*worst_enter_dist - half_size) / v, (*worst_exit_dist + half_size) / v); - } - return {}; -} - -bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) -{ - const auto enter_within_threshold = - range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; - const auto exit_within_threshold = - range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; - return enter_within_threshold || exit_within_threshold; -} - -bool intervals_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto opposite_way_condition = [&]() -> bool { - const auto ego_exits_before_object_enters = - range_times.ego.exit_time + params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] (opposite way) ego exit %2.2fs < obj enter %2.2fs ? -> should not " - "enter = %d\n", - range_times.ego.exit_time + params.intervals_ego_buffer, - range_times.object.enter_time + params.intervals_obj_buffer, ego_exits_before_object_enters); - return ego_exits_before_object_enters; - }; - const auto same_way_condition = [&]() -> bool { - const auto object_enters_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer && - range_times.object.enter_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - const auto object_exits_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.exit_time + params.intervals_obj_buffer && - range_times.object.exit_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] obj enters during overlap ? %d OR obj exits during overlap %d ? -> should " - "not " - "enter = %d\n", - object_enters_during_overlap, object_exits_during_overlap, - object_enters_during_overlap || object_exits_during_overlap); - return object_enters_during_overlap || object_exits_during_overlap; - }; - - const auto object_is_going_same_way = - range_times.object.enter_time < range_times.object.exit_time; - return (object_is_going_same_way && same_way_condition()) || - (!object_is_going_same_way && opposite_way_condition()); -} - -bool ttc_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto ttc_at_enter = range_times.ego.enter_time - range_times.object.enter_time; - const auto ttc_at_exit = range_times.ego.exit_time - range_times.object.exit_time; - const auto collision_during_overlap = (ttc_at_enter < 0.0) != (ttc_at_exit < 0.0); - const auto ttc_is_bellow_threshold = - std::min(std::abs(ttc_at_enter), std::abs(ttc_at_exit)) <= params.ttc_threshold; - RCLCPP_DEBUG( - logger, "\t\t\t[TTC] (%2.2fs - %2.2fs) -> %d\n", ttc_at_enter, ttc_at_exit, - (collision_during_overlap || ttc_is_bellow_threshold)); - return collision_during_overlap || ttc_is_bellow_threshold; -} - -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - RCLCPP_DEBUG( - logger, " enter at %2.2fs, exits at %2.2fs\n", range_times.object.enter_time, - range_times.object.exit_time); - return (params.mode == "threshold" && threshold_condition(range_times, params)) || - (params.mode == "intervals" && intervals_condition(range_times, params, logger)) || - (params.mode == "ttc" && ttc_condition(range_times, params, logger)); -} - -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - RangeTimes range_times{}; - range_times.ego.enter_time = - time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity); - range_times.ego.exit_time = - time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity); - RCLCPP_DEBUG( - logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx, - range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time); - for (const auto & object : inputs.objects.objects) { - RCLCPP_DEBUG( - logger, "\t\t[%s] going at %2.2fm/s", - tier4_autoware_utils::toHexString(object.object_id).c_str(), - object.kinematics.initial_twist_with_covariance.twist.linear.x); - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { - RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); - continue; // skip objects with velocity bellow a threshold - } - // skip objects that are already on the interval - const auto enter_exit_time = - params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_dist_buffer, logger) - : object_time_to_range(object, range, inputs, logger); - if (!enter_exit_time) { - RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); - continue; // skip objects that are not driving towards the overlapping range - } - - range_times.object.enter_time = enter_exit_time->first; - range_times.object.exit_time = enter_exit_time->second; - if (will_collide_on_range(range_times, params, logger)) { - range.debug.times = range_times; - range.debug.object = object; - return true; - } - } - range.debug.times = range_times; - return false; -} - -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params) -{ - if (distance < params.stop_dist_threshold) { - decision->velocity = 0.0; - } else if (distance < params.slow_dist_threshold) { - decision->velocity = params.slow_velocity; - } else { - decision.reset(); - } -} - -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - std::optional decision; - if (should_not_enter(range, inputs, params, logger)) { - decision.emplace(); - decision->target_path_idx = - inputs.ego_data.first_path_idx + range.entering_path_idx; // add offset from curr pose - decision->lane_to_avoid = range.lane; - const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx); - set_decision_velocity(decision, ego_dist_to_range, params); - } - return decision; -} - -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger) -{ - std::vector decisions; - for (const auto & range : inputs.ranges) { - if (range.entering_path_idx == 0UL) continue; // skip if we already entered the range - const auto optional_decision = calculate_decision(range, inputs, params, logger); - range.debug.decision = optional_decision; - if (optional_decision) decisions.push_back(*optional_decision); - } - return decisions; -} - -} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp deleted file mode 100644 index 27c5b00c96b3f..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ /dev/null @@ -1,115 +0,0 @@ -// 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. - -#ifndef DECISIONS_HPP_ -#define DECISIONS_HPP_ - -#include "types.hpp" - -#include -#include - -#include - -#include - -#include -#include -#include -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ -/// @brief calculate the distance along the ego path between ego and some target path index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego path index -/// @return distance between ego and the target [m] -double distance_along_path(const EgoData & ego_data, const size_t target_idx); -/// @brief estimate the time when ego will reach some target path index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego path index -/// @param [in] min_velocity minimum ego velocity used to estimate the time -/// @return time taken by ego to reach the target [s] -double time_along_path(const EgoData & ego_data, const size_t target_idx); -/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit -/// points of an overlapping range -/// @details times when the predicted paths of the object enters/exits the range are calculated -/// but may not exist (e.g,, predicted path ends before reaching the end of the range) -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger); -/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit -/// points of an overlapping range -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit. -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger); -/// @brief decide whether an object is coming in the range at the same time as ego -/// @details the condition depends on the mode (threshold, intervals, ttc) -/// @param [in] range_times times when ego and the object enter/exit the range -/// @param [in] params parameters -/// @param [in] logger ros logger -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); -/// @brief check whether we should avoid entering the given range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return true if we should avoid entering the range -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief set the velocity of a decision (or unset it) based on the distance away from the range -/// @param [out] decision decision to update (either set its velocity or unset the decision) -/// @param [in] distance distance between ego and the range corresponding to the decision -/// @param [in] params parameters -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params); -/// @brief calculate the decision to slowdown or stop before an overlapping range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return an optional decision to slowdown or stop -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief calculate decisions to slowdown or stop before some overlapping ranges -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return the calculated decisions to slowdown or stop -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -} // namespace autoware::behavior_velocity_planner::out_of_lane - -#endif // DECISIONS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp deleted file mode 100644 index bb352f625580f..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ /dev/null @@ -1,146 +0,0 @@ -// Copyright 2023-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 "filter_predicted_objects.hpp" - -#include -#include - -#include - -#include -#include - -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ -void cut_predicted_path_beyond_line( - autoware_perception_msgs::msg::PredictedPath & predicted_path, - const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) -{ - auto stop_line_idx = 0UL; - bool found = false; - lanelet::BasicSegment2d path_segment; - path_segment.first.x() = predicted_path.path.front().position.x; - path_segment.first.y() = predicted_path.path.front().position.y; - for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { - path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; - path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; - if (boost::geometry::intersects(stop_line, path_segment)) { - found = true; - break; - } - path_segment.first = path_segment.second; - } - if (found) { - auto cut_idx = stop_line_idx; - double arc_length = 0; - while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += tier4_autoware_utils::calcDistance2d( - predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); - --cut_idx; - } - predicted_path.path.resize(cut_idx); - } -} - -std::optional find_next_stop_line( - const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) -{ - lanelet::ConstLanelets lanelets; - lanelet::BasicLineString2d query_line; - for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); - const auto query_results = lanelet::geometry::findWithin2d( - planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_line); - for (const auto & r : query_results) lanelets.push_back(r.second); - for (const auto & ll : lanelets) { - for (const auto & element : ll.regulatoryElementsAs()) { - const auto traffic_signal_stamped = planner_data.getTrafficSignal(element->id()); - if ( - traffic_signal_stamped.has_value() && element->stopLine().has_value() && - traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { - lanelet::BasicLineString2d stop_line; - for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); - return stop_line; - } - } - } - return std::nullopt; -} - -void cut_predicted_path_beyond_red_lights( - autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, - const double object_front_overhang) -{ - const auto stop_line = find_next_stop_line(predicted_path, planner_data); - if (stop_line) { - // we use a longer stop line to also cut predicted paths that slightly go around the stop line - auto longer_stop_line = *stop_line; - const auto diff = stop_line->back() - stop_line->front(); - longer_stop_line.front() -= diff * 0.5; - longer_stop_line.back() += diff * 0.5; - cut_predicted_path_beyond_line(predicted_path, longer_stop_line, object_front_overhang); - } -} - -autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) -{ - autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data.predicted_objects->header; - for (const auto & object : planner_data.predicted_objects->objects) { - const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); - if (is_pedestrian) continue; - - auto filtered_object = object; - const auto is_invalid_predicted_path = [&](const auto & predicted_path) { - const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); - if (no_overlap_path.size() <= 1) return true; - const auto lat_offset_to_current_ego = - std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); - const auto is_crossing_ego = - lat_offset_to_current_ego <= - object.shape.dimensions.y / 2.0 + std::max( - params.left_offset + params.extra_left_offset, - params.right_offset + params.extra_right_offset); - return is_low_confidence || is_crossing_ego; - }; - if (params.objects_use_predicted_paths) { - auto & predicted_paths = filtered_object.kinematics.predicted_paths; - const auto new_end = - std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); - predicted_paths.erase(new_end, predicted_paths.end()); - if (params.objects_cut_predicted_paths_beyond_red_lights) - for (auto & predicted_path : predicted_paths) - cut_predicted_path_beyond_red_lights( - predicted_path, planner_data, filtered_object.shape.dimensions.x); - predicted_paths.erase( - std::remove_if( - predicted_paths.begin(), predicted_paths.end(), - [](const auto & p) { return p.path.empty(); }), - predicted_paths.end()); - } - - if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) - filtered_objects.objects.push_back(filtered_object); - } - return filtered_objects; -} - -} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp deleted file mode 100644 index a12b37e4c7d6d..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2023-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 FILTER_PREDICTED_OBJECTS_HPP_ -#define FILTER_PREDICTED_OBJECTS_HPP_ - -#include "types.hpp" - -#include - -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ -/// @brief cut a predicted path beyond the given stop line -/// @param [inout] predicted_path predicted path to cut -/// @param [in] stop_line stop line used for cutting -/// @param [in] object_front_overhang extra distance to cut ahead of the stop line -void cut_predicted_path_beyond_line( - autoware_perception_msgs::msg::PredictedPath & predicted_path, - const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); - -/// @brief find the next red light stop line along the given path -/// @param [in] path predicted path to check for a stop line -/// @param [in] planner_data planner data with stop line information -/// @return the first red light stop line found along the path (if any) -std::optional find_next_stop_line( - const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); - -/// @brief cut predicted path beyond stop lines of red lights -/// @param [inout] predicted_path predicted path to cut -/// @param [in] planner_data planner data to get the map and traffic light information -void cut_predicted_path_beyond_red_lights( - autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, - const double object_front_overhang); - -/// @brief filter predicted objects and their predicted paths -/// @param [in] planner_data planner data -/// @param [in] ego_data ego data -/// @param [in] params parameters -/// @return filtered predicted objects -autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); -} // namespace autoware::behavior_velocity_planner::out_of_lane - -#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp deleted file mode 100644 index 48992e46ec74f..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ /dev/null @@ -1,90 +0,0 @@ -// 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. - -#include "footprint.hpp" - -#include - -#include - -#include -#include - -#include -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ -tier4_autoware_utils::Polygon2d make_base_footprint( - const PlannerParam & p, const bool ignore_offset) -{ - tier4_autoware_utils::Polygon2d base_footprint; - const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; - const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; - const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; - const auto left_offset = ignore_offset ? 0.0 : p.extra_left_offset; - base_footprint.outer() = { - {p.front_offset + front_offset, p.left_offset + left_offset}, - {p.front_offset + front_offset, p.right_offset - right_offset}, - {p.rear_offset - rear_offset, p.right_offset - right_offset}, - {p.rear_offset - rear_offset, p.left_offset + left_offset}}; - return base_footprint; -} - -lanelet::BasicPolygon2d project_to_pose( - const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) -{ - const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); - lanelet::BasicPolygon2d footprint; - for (const auto & p : rotated_footprint.outer()) - footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); - return footprint; -} - -std::vector calculate_path_footprints( - const EgoData & ego_data, const PlannerParam & params) -{ - const auto base_footprint = make_base_footprint(params); - std::vector path_footprints; - path_footprints.reserve(ego_data.path.points.size()); - double length = 0.0; - const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + - params.front_offset + params.extra_front_offset; - for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size() && length < range; ++i) { - const auto & path_pose = ego_data.path.points[i].point.pose; - const auto angle = tf2::getYaw(path_pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); - lanelet::BasicPolygon2d footprint; - for (const auto & p : rotated_footprint.outer()) - footprint.emplace_back(p.x() + path_pose.position.x, p.y() + path_pose.position.y); - path_footprints.push_back(footprint); - if (i + 1 < ego_data.path.points.size()) - length += tier4_autoware_utils::calcDistance2d(path_pose, ego_data.path.points[i + 1].point); - } - return path_footprints; -} - -lanelet::BasicPolygon2d calculate_current_ego_footprint( - const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset) -{ - const auto base_footprint = make_base_footprint(params, ignore_offset); - const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); - lanelet::BasicPolygon2d footprint; - for (const auto & p : rotated_footprint.outer()) - footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); - return footprint; -} -} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp deleted file mode 100644 index 1a0d38600fe73..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// 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. - -#ifndef FOOTPRINT_HPP_ -#define FOOTPRINT_HPP_ - -#include "types.hpp" - -#include - -#include - -namespace autoware::behavior_velocity_planner -{ -namespace out_of_lane -{ -/// @brief create the base footprint of ego -/// @param [in] p parameters used to create the footprint -/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the -/// footprint -/// @return base ego footprint -tier4_autoware_utils::Polygon2d make_base_footprint( - const PlannerParam & p, const bool ignore_offset = false); -/// @brief project a footprint to the given pose -/// @param [in] base_footprint footprint to project -/// @param [in] pose projection pose -/// @return footprint projected to the given pose -lanelet::BasicPolygon2d project_to_pose( - const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); -/// @brief calculate the path footprints -/// @details the resulting polygon follows the format used by the lanelet library: clockwise order -/// and implicit closing edge -/// @param [in] ego_data data related to the ego vehicle (includes its path) -/// @param [in] params parameters -/// @return polygon footprints for each path point starting from ego's current position -std::vector calculate_path_footprints( - const EgoData & ego_data, const PlannerParam & params); -/// @brief calculate the current ego footprint -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] params parameters -/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the -/// footprint -lanelet::BasicPolygon2d calculate_current_ego_footprint( - const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); -} // namespace out_of_lane -} // namespace autoware::behavior_velocity_planner - -#endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp deleted file mode 100644 index 42df39853f4f5..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ /dev/null @@ -1,130 +0,0 @@ -// 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. - -#include "lanelets_selection.hpp" - -#include - -#include -#include - -#include -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ - -lanelet::ConstLanelets consecutive_lanelets( - const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) -{ - lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet); - const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet); - consecutives.insert(consecutives.end(), previous.begin(), previous.end()); - return consecutives; -} - -lanelet::ConstLanelets get_missing_lane_change_lanelets( - lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler) -{ - lanelet::ConstLanelets missing_lane_change_lanelets; - const auto & routing_graph = *route_handler.getRoutingGraphPtr(); - lanelet::ConstLanelets adjacents; - lanelet::ConstLanelets consecutives; - for (const auto & ll : path_lanelets) { - const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll); - std::copy_if( - consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives), - [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); }); - const auto adjacents_of_ll = routing_graph.besides(ll); - std::copy_if( - adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents), - [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); }); - } - std::copy_if( - adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets), - [&](const auto & l) { - return !contains_lanelet(missing_lane_change_lanelets, l.id()) && - !contains_lanelet(path_lanelets, l.id()) && contains_lanelet(consecutives, l.id()); - }); - return missing_lane_change_lanelets; -} - -lanelet::ConstLanelets calculate_path_lanelets( - const EgoData & ego_data, const route_handler::RouteHandler & route_handler) -{ - const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); - lanelet::ConstLanelets path_lanelets; - lanelet::BasicLineString2d path_ls; - for (const auto & p : ego_data.path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - for (const auto & dist_lanelet : - lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, path_ls)) { - if (!contains_lanelet(path_lanelets, dist_lanelet.second.id())) - path_lanelets.push_back(dist_lanelet.second); - } - const auto missing_lanelets = get_missing_lane_change_lanelets(path_lanelets, route_handler); - path_lanelets.insert(path_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); - return path_lanelets; -} - -void add_lane_changeable_lanelets( - lanelet::ConstLanelets & lanelets_to_ignore, const lanelet::ConstLanelets & path_lanelets, - const route_handler::RouteHandler & route_handler) -{ - for (const auto & path_lanelet : path_lanelets) - for (const auto & ll : route_handler.getLaneChangeableNeighbors(path_lanelet)) - if (!contains_lanelet(lanelets_to_ignore, ll.id())) lanelets_to_ignore.push_back(ll); -} - -lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, - const route_handler::RouteHandler & route_handler, const PlannerParam & params) -{ - lanelet::ConstLanelets ignored_lanelets; - // ignore lanelets directly behind ego - const auto behind = - planning_utils::calculateOffsetPoint2d(ego_data.pose, params.rear_offset, 0.0); - const lanelet::BasicPoint2d behind_point(behind.x(), behind.y()); - const auto behind_lanelets = lanelet::geometry::findWithin2d( - route_handler.getLaneletMapPtr()->laneletLayer, behind_point, 0.0); - for (const auto & l : behind_lanelets) { - const auto is_path_lanelet = contains_lanelet(path_lanelets, l.second.id()); - if (!is_path_lanelet) ignored_lanelets.push_back(l.second); - } - if (params.ignore_overlaps_over_lane_changeable_lanelets) - add_lane_changeable_lanelets(ignored_lanelets, path_lanelets, route_handler); - return ignored_lanelets; -} - -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const route_handler::RouteHandler & route_handler, const PlannerParam & params) -{ - lanelet::ConstLanelets other_lanelets; - const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); - const auto lanelets_within_range = lanelet::geometry::findWithin2d( - route_handler.getLaneletMapPtr()->laneletLayer, ego_point, - std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + - params.extra_front_offset); - for (const auto & ll : lanelets_within_range) { - if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") - continue; - const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); - const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - if (!is_path_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); - } - return other_lanelets; -} -} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp deleted file mode 100644 index 8985a49a12853..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// 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. - -#ifndef LANELETS_SELECTION_HPP_ -#define LANELETS_SELECTION_HPP_ - -#include "types.hpp" - -#include - -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ -/// @brief checks if a lanelet is already contained in a vector of lanelets -/// @param [in] lanelets vector to check -/// @param [in] id lanelet id to check -/// @return true if the given vector contains a lanelet of the given id -inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lanelet::Id id) -{ - return std::find_if(lanelets.begin(), lanelets.end(), [&](const auto & l) { - return l.id() == id; - }) != lanelets.end(); -}; - -/// @brief calculate lanelets crossed by the ego path -/// @details calculated from the ids of the path msg, the lanelets containing path points -/// @param [in] ego_data data about the ego vehicle -/// @param [in] route_handler route handler -/// @return lanelets crossed by the ego vehicle -lanelet::ConstLanelets calculate_path_lanelets( - const EgoData & ego_data, const route_handler::RouteHandler & route_handler); -/// @brief calculate lanelets that may not be crossed by the path but may be overlapped during a -/// lane change -/// @param [in] path_lanelets lanelets driven by the ego vehicle -/// @param [in] route_handler route handler -/// @return lanelets that may be overlapped by a lane change (and are not already in path_lanelets) -lanelet::ConstLanelets get_missing_lane_change_lanelets( - lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler); -/// @brief calculate lanelets that should be ignored -/// @param [in] ego_data data about the ego vehicle -/// @param [in] path_lanelets lanelets driven by the ego vehicle -/// @param [in] route_handler route handler -/// @param [in] params parameters -/// @return lanelets to ignore -lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, - const route_handler::RouteHandler & route_handler, const PlannerParam & params); -/// @brief calculate lanelets that should be checked by the module -/// @param [in] ego_data data about the ego vehicle -/// @param [in] path_lanelets lanelets driven by the ego vehicle -/// @param [in] ignored_lanelets lanelets to ignore -/// @param [in] route_handler route handler -/// @param [in] params parameters -/// @return lanelets to check for overlaps -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const route_handler::RouteHandler & route_handler, const PlannerParam & params); -} // namespace autoware::behavior_velocity_planner::out_of_lane - -#endif // LANELETS_SELECTION_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp deleted file mode 100644 index 61ec1a832bf8e..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// 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. - -#include "manager.hpp" - -#include "scene_out_of_lane.hpp" - -#include -#include - -#include - -#include -#include -#include - -namespace autoware::behavior_velocity_planner -{ -using tier4_autoware_utils::getOrDeclareParameter; - -OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) -{ - const std::string ns(getModuleName()); - auto & pp = planner_param_; - - pp.mode = getOrDeclareParameter(node, ns + ".mode"); - pp.skip_if_already_overlapping = - getOrDeclareParameter(node, ns + ".skip_if_already_overlapping"); - pp.ignore_overlaps_over_lane_changeable_lanelets = - getOrDeclareParameter(node, ns + ".ignore_overlaps_over_lane_changeable_lanelets"); - - pp.time_threshold = getOrDeclareParameter(node, ns + ".threshold.time_threshold"); - pp.intervals_ego_buffer = getOrDeclareParameter(node, ns + ".intervals.ego_time_buffer"); - pp.intervals_obj_buffer = - getOrDeclareParameter(node, ns + ".intervals.objects_time_buffer"); - pp.ttc_threshold = getOrDeclareParameter(node, ns + ".ttc.threshold"); - - pp.objects_min_vel = getOrDeclareParameter(node, ns + ".objects.minimum_velocity"); - pp.objects_use_predicted_paths = - getOrDeclareParameter(node, ns + ".objects.use_predicted_paths"); - pp.objects_min_confidence = - getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); - pp.objects_dist_buffer = getOrDeclareParameter(node, ns + ".objects.distance_buffer"); - pp.objects_cut_predicted_paths_beyond_red_lights = - getOrDeclareParameter(node, ns + ".objects.cut_predicted_paths_beyond_red_lights"); - - pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); - pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); - - pp.skip_if_over_max_decel = - getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); - pp.precision = getOrDeclareParameter(node, ns + ".action.precision"); - pp.min_decision_duration = getOrDeclareParameter(node, ns + ".action.min_duration"); - pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); - pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); - pp.slow_dist_threshold = - getOrDeclareParameter(node, ns + ".action.slowdown.distance_threshold"); - pp.stop_dist_threshold = - getOrDeclareParameter(node, ns + ".action.stop.distance_threshold"); - - pp.ego_min_velocity = getOrDeclareParameter(node, ns + ".ego.min_assumed_velocity"); - pp.extra_front_offset = getOrDeclareParameter(node, ns + ".ego.extra_front_offset"); - pp.extra_rear_offset = getOrDeclareParameter(node, ns + ".ego.extra_rear_offset"); - pp.extra_left_offset = getOrDeclareParameter(node, ns + ".ego.extra_left_offset"); - pp.extra_right_offset = getOrDeclareParameter(node, ns + ".ego.extra_right_offset"); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - pp.front_offset = vehicle_info.max_longitudinal_offset_m; - pp.rear_offset = vehicle_info.min_longitudinal_offset_m; - pp.left_offset = vehicle_info.max_lateral_offset_m; - pp.right_offset = vehicle_info.min_lateral_offset_m; -} - -void OutOfLaneModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - if (path.points.empty()) return; - // general - if (!isModuleRegistered(module_id_)) { - registerModule(std::make_shared( - module_id_, planner_param_, logger_.get_child("out_of_lane_module"), clock_)); - } -} - -std::function &)> -OutOfLaneModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { - return false; - }; -} -} // namespace autoware::behavior_velocity_planner - -#include -PLUGINLIB_EXPORT_CLASS( - autoware::behavior_velocity_planner::OutOfLaneModulePlugin, - autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp deleted file mode 100644 index 5ab7126d88d4d..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// 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. - -#ifndef MANAGER_HPP_ -#define MANAGER_HPP_ - -#include "scene_out_of_lane.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -namespace autoware::behavior_velocity_planner -{ -class OutOfLaneModuleManager : public SceneModuleManagerInterface -{ -public: - explicit OutOfLaneModuleManager(rclcpp::Node & node); - - const char * getModuleName() override { return "out_of_lane"; } - -private: - using PlannerParam = out_of_lane::PlannerParam; - - PlannerParam planner_param_; - int64_t module_id_; - - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; -}; - -class OutOfLaneModulePlugin : public PluginWrapper -{ -}; - -} // namespace autoware::behavior_velocity_planner - -#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp deleted file mode 100644 index a081df0b52028..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// 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. - -#include "overlapping_range.hpp" - -#include -#include - -#include - -#include -#include - -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ - -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & path_footprint, const lanelet::ConstLanelets & path_lanelets, - const lanelet::ConstLanelet & lanelet) -{ - Overlap overlap; - const auto & left_bound = lanelet.leftBound2d().basicLineString(); - const auto & right_bound = lanelet.rightBound2d().basicLineString(); - // TODO(Maxime): these intersects (for each path footprint, for each lanelet) are too expensive - const auto overlap_left = boost::geometry::intersects(path_footprint, left_bound); - const auto overlap_right = boost::geometry::intersects(path_footprint, right_bound); - - lanelet::BasicPolygons2d overlapping_polygons; - if (overlap_left || overlap_right) - boost::geometry::intersection( - path_footprint, lanelet.polygon2d().basicPolygon(), overlapping_polygons); - for (const auto & overlapping_polygon : overlapping_polygons) { - for (const auto & point : overlapping_polygon) { - if (overlap_left && overlap_right) - overlap.inside_distance = boost::geometry::distance(left_bound, right_bound); - else if (overlap_left) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, left_bound)); - else if (overlap_right) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, right_bound)); - geometry_msgs::msg::Pose p; - p.position.x = point.x(); - p.position.y = point.y(); - const auto length = lanelet::utils::getArcCoordinates(path_lanelets, p).length; - if (length > overlap.max_arc_length) { - overlap.max_arc_length = length; - overlap.max_overlap_point = point; - } - if (length < overlap.min_arc_length) { - overlap.min_arc_length = length; - overlap.min_overlap_point = point; - } - } - } - return overlap; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & path_footprints, - const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params) -{ - OverlapRanges ranges; - OtherLane other_lane(lanelet); - std::vector overlaps; - for (auto i = 0UL; i < path_footprints.size(); ++i) { - const auto overlap = calculate_overlap(path_footprints[i], path_lanelets, lanelet); - const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; - if (has_overlap) { // open/update the range - overlaps.push_back(overlap); - if (!other_lane.range_is_open) { - other_lane.first_range_bound.index = i; - other_lane.first_range_bound.point = overlap.min_overlap_point; - other_lane.first_range_bound.arc_length = - overlap.min_arc_length - params.overlap_extra_length; - other_lane.first_range_bound.inside_distance = overlap.inside_distance; - other_lane.range_is_open = true; - } - other_lane.last_range_bound.index = i; - other_lane.last_range_bound.point = overlap.max_overlap_point; - other_lane.last_range_bound.arc_length = overlap.max_arc_length + params.overlap_extra_length; - other_lane.last_range_bound.inside_distance = overlap.inside_distance; - } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - } - // close the range if it is still open - if (other_lane.range_is_open) { - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - return ranges; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & path_footprints, - const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params) -{ - OverlapRanges ranges; - for (auto & lanelet : lanelets) { - const auto lanelet_ranges = - calculate_overlapping_ranges(path_footprints, path_lanelets, lanelet, params); - ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); - } - return ranges; -} - -} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp deleted file mode 100644 index afb71c11b3ae4..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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. - -#ifndef OVERLAPPING_RANGE_HPP_ -#define OVERLAPPING_RANGE_HPP_ - -#include "types.hpp" - -#include - -#include - -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ - -/// @brief calculate the overlap between the given footprint and lanelet -/// @param [in] path_footprint footprint used to calculate the overlap -/// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path -/// @param [in] lanelet lanelet used to calculate the overlap -/// @return the found overlap between the footprint and the lanelet -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & path_footprint, const lanelet::ConstLanelets & path_lanelets, - const lanelet::ConstLanelet & lanelet); -/// @brief calculate the overlapping ranges between the path footprints and a lanelet -/// @param [in] path_footprints footprints used to calculate the overlaps -/// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path -/// @param [in] lanelet lanelet used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelet -OverlapRanges calculate_overlapping_ranges( - const std::vector & path_footprints, - const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params); -/// @brief calculate the overlapping ranges between the path footprints and some lanelets -/// @param [in] path_footprints footprints used to calculate the overlaps -/// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path -/// @param [in] lanelets lanelets used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelets, sorted by -/// increasing arc length along the path -OverlapRanges calculate_overlapping_ranges( - const std::vector & path_footprints, - const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params); -} // namespace autoware::behavior_velocity_planner::out_of_lane - -#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp deleted file mode 100644 index 3f34e8441e8be..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ /dev/null @@ -1,244 +0,0 @@ -// Copyright 2023 TIER IV, Inc. All rights reserved. -// -// 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 "scene_out_of_lane.hpp" - -#include "calculate_slowdown_points.hpp" -#include "debug.hpp" -#include "decisions.hpp" -#include "filter_predicted_objects.hpp" -#include "footprint.hpp" -#include "lanelets_selection.hpp" -#include "overlapping_range.hpp" -#include "types.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include -#include -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ - -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; - -OutOfLaneModule::OutOfLaneModule( - const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) -{ - velocity_factor_.init(PlanningBehavior::ROUTE_OBSTACLE); -} - -bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) -{ - debug_data_.reset_data(); - *stop_reason = planning_utils::initializeStopReason(StopReason::OUT_OF_LANE); - if (!path || path->points.size() < 2) return true; - tier4_autoware_utils::StopWatch stopwatch; - stopwatch.tic(); - EgoData ego_data; - ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path.points = path->points; - ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); - motion_utils::removeOverlapPoints(ego_data.path.points); - ego_data.velocity = planner_data_->current_velocity->twist.linear.x; - ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; - stopwatch.tic("calculate_path_footprints"); - const auto current_ego_footprint = calculate_current_ego_footprint(ego_data, params_, true); - const auto path_footprints = calculate_path_footprints(ego_data, params_); - const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); - // Calculate lanelets to ignore and consider - stopwatch.tic("calculate_lanelets"); - const auto path_lanelets = calculate_path_lanelets(ego_data, *planner_data_->route_handler_); - const auto ignored_lanelets = - calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); - const auto other_lanelets = calculate_other_lanelets( - ego_data, path_lanelets, ignored_lanelets, *planner_data_->route_handler_, params_); - const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); - - debug_data_.footprints = path_footprints; - debug_data_.path_lanelets = path_lanelets; - debug_data_.ignored_lanelets = ignored_lanelets; - debug_data_.other_lanelets = other_lanelets; - debug_data_.path = ego_data.path; - debug_data_.first_path_idx = ego_data.first_path_idx; - - if (params_.skip_if_already_overlapping) { - debug_data_.current_footprint = current_ego_footprint; - const auto overlapped_lanelet_it = - std::find_if(other_lanelets.begin(), other_lanelets.end(), [&](const auto & ll) { - return boost::geometry::intersects(ll.polygon2d().basicPolygon(), current_ego_footprint); - }); - if (overlapped_lanelet_it != other_lanelets.end()) { - debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); - // TODO(Maxime): we may want to just add the overlapped lane to the ignored lanelets - RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module ()\n"); - return true; - } - } - // Calculate overlapping ranges - stopwatch.tic("calculate_overlapping_ranges"); - const auto ranges = - calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_); - const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); - // Calculate stop and slowdown points - DecisionInputs inputs; - inputs.ranges = ranges; - inputs.ego_data = ego_data; - stopwatch.tic("filter_predicted_objects"); - inputs.objects = filter_predicted_objects(*planner_data_, ego_data, params_); - const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); - inputs.route_handler = planner_data_->route_handler_; - inputs.lanelets = other_lanelets; - stopwatch.tic("calculate_decisions"); - const auto decisions = calculate_decisions(inputs, params_, logger_); - const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); - stopwatch.tic("calc_slowdown_points"); - if ( // reset the previous inserted point if the timer expired - prev_inserted_point_ && - (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) - prev_inserted_point_.reset(); - auto point_to_insert = - calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); - const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); - stopwatch.tic("insert_slowdown_points"); - debug_data_.slowdowns.clear(); - if ( // reset the timer if there is no previous inserted point or if we avoid the same lane - point_to_insert && - (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == - point_to_insert->slowdown.lane_to_avoid.id())) - prev_inserted_point_time_ = clock_->now(); - // reuse previous stop point if there is no new one or if its velocity is not higher than the new - // one and its arc length is lower - const auto should_use_prev_inserted_point = [&]() { - if ( - point_to_insert && prev_inserted_point_ && - prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { - const auto arc_length = motion_utils::calcSignedArcLength( - path->points, 0LU, point_to_insert->point.point.pose.position); - const auto prev_arc_length = motion_utils::calcSignedArcLength( - path->points, 0LU, prev_inserted_point_->point.point.pose.position); - return prev_arc_length < arc_length; - } - return !point_to_insert && prev_inserted_point_; - }(); - if (should_use_prev_inserted_point) { - // if the path changed the prev point is no longer on the path so we project it - const auto insert_arc_length = motion_utils::calcSignedArcLength( - path->points, 0LU, prev_inserted_point_->point.point.pose.position); - prev_inserted_point_->point.point.pose = - motion_utils::calcInterpolatedPose(path->points, insert_arc_length); - point_to_insert = prev_inserted_point_; - } - if (point_to_insert) { - prev_inserted_point_ = point_to_insert; - RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); - debug_data_.slowdowns = {*point_to_insert}; - auto path_idx = motion_utils::findNearestSegmentIndex( - path->points, point_to_insert->point.point.pose.position) + - 1; - planning_utils::insertVelocity( - *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx); - auto stop_pose_reached = false; - if (point_to_insert->slowdown.velocity == 0.0) { - const auto dist_to_stop_pose = motion_utils::calcSignedArcLength( - path->points, ego_data.pose.position, point_to_insert->point.point.pose.position); - if (ego_data.velocity < 1e-3 && dist_to_stop_pose < 1e-3) stop_pose_reached = true; - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = point_to_insert->point.point.pose; - stop_factor.dist_to_stop_pose = dist_to_stop_pose; - planning_utils::appendStopReason(stop_factor, stop_reason); - } - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose, - stop_pose_reached ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING, "out_of_lane"); - } else if (!decisions.empty()) { - RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); - } - const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); - debug_data_.ranges = inputs.ranges; - - const auto total_time_us = stopwatch.toc(); - RCLCPP_DEBUG( - logger_, - "Total time = %2.2fus\n" - "\tcalculate_lanelets = %2.0fus\n" - "\tcalculate_path_footprints = %2.0fus\n" - "\tcalculate_overlapping_ranges = %2.0fus\n" - "\tfilter_pred_objects = %2.0fus\n" - "\tcalculate_decisions = %2.0fus\n" - "\tcalc_slowdown_points = %2.0fus\n" - "\tinsert_slowdown_points = %2.0fus\n", - total_time_us, calculate_lanelets_us, calculate_path_footprints_us, - calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, - calc_slowdown_points_us, insert_slowdown_points_us); - return true; -} - -MarkerArray OutOfLaneModule::createDebugMarkerArray() -{ - constexpr auto z = 0.0; - MarkerArray debug_marker_array; - - debug::add_footprint_markers( - debug_marker_array, debug_data_.footprints, z, debug_data_.prev_footprints); - debug::add_current_overlap_marker( - debug_marker_array, debug_data_.current_footprint, debug_data_.current_overlapped_lanelets, z, - debug_data_.prev_current_overlapped_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data_.path_lanelets, "path_lanelets", - tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), debug_data_.prev_path_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data_.ignored_lanelets, "ignored_lanelets", - tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data_.prev_ignored_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data_.other_lanelets, "other_lanelets", - tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data_.prev_other_lanelets); - debug::add_range_markers( - debug_marker_array, debug_data_.ranges, debug_data_.path, debug_data_.first_path_idx, z, - debug_data_.prev_ranges); - return debug_marker_array; -} - -motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() -{ - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; - wall.text = "out_of_lane"; - wall.longitudinal_offset = params_.front_offset; - wall.style = motion_utils::VirtualWallType::slowdown; - for (const auto & slowdown : debug_data_.slowdowns) { - wall.pose = slowdown.point.point.pose; - virtual_walls.push_back(wall); - } - return virtual_walls; -} - -} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp deleted file mode 100644 index 38101d6a9ba1d..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ /dev/null @@ -1,65 +0,0 @@ -// 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. - -#ifndef SCENE_OUT_OF_LANE_HPP_ -#define SCENE_OUT_OF_LANE_HPP_ - -#include "types.hpp" - -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ -class OutOfLaneModule : public SceneModuleInterface -{ -public: - OutOfLaneModule( - const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); - - /// @brief insert stop or slow down points to prevent dangerously entering another lane - /// @param [inout] path the path to update - /// @param [inout] stop_reason reason for stopping - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; - - visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; - -private: - PlannerParam params_; - - std::optional prev_inserted_point_{}; - rclcpp::Time prev_inserted_point_time_{}; - -protected: - int64_t module_id_{}; - - // Debug - mutable DebugData debug_data_; -}; -} // namespace autoware::behavior_velocity_planner::out_of_lane - -#endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp deleted file mode 100644 index f1c48a6e96d17..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ /dev/null @@ -1,235 +0,0 @@ -// 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. - -#ifndef TYPES_HPP_ -#define TYPES_HPP_ - -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::behavior_velocity_planner::out_of_lane -{ -/// @brief parameters for the "out of lane" module -struct PlannerParam -{ - std::string mode; // mode used to consider a conflict with an object - bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps - // another lane - bool ignore_overlaps_over_lane_changeable_lanelets; // if true, overlaps on lane changeable - // lanelets are ignored - - double time_threshold; // [s](mode="threshold") objects time threshold - double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range - double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range - double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object - double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - - bool objects_use_predicted_paths; // whether to use the objects' predicted paths - bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red - // lights' stop lines - double objects_min_vel; // [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // minimum confidence to consider a predicted path - double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in - // the other lane - - double overlap_extra_length; // [m] extra length to add around an overlap range - double overlap_min_dist; // [m] min distance inside another lane to consider an overlap - // action to insert in the path if an object causes a conflict at an overlap - bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - double dist_buffer; - double slow_velocity; - double slow_dist_threshold; - double stop_dist_threshold; - double precision; // [m] precision when inserting a stop pose in the path - double min_decision_duration; // [s] minimum duration needed a decision can be canceled - // ego dimensions used to create its polygon footprint - double front_offset; // [m] front offset (from vehicle info) - double rear_offset; // [m] rear offset (from vehicle info) - double right_offset; // [m] right offset (from vehicle info) - double left_offset; // [m] left offset (from vehicle info) - double extra_front_offset; // [m] extra front distance - double extra_rear_offset; // [m] extra rear distance - double extra_right_offset; // [m] extra right distance - double extra_left_offset; // [m] extra left distance -}; - -struct EnterExitTimes -{ - double enter_time{}; - double exit_time{}; -}; -struct RangeTimes -{ - EnterExitTimes ego{}; - EnterExitTimes object{}; -}; - -/// @brief action taken by the "out of lane" module -struct Slowdown -{ - size_t target_path_idx{}; // we want to slowdown before this path index - double velocity{}; // desired slow down velocity - lanelet::ConstLanelet lane_to_avoid; // we want to slowdown before entering this lane -}; -/// @brief slowdown to insert in a path -struct SlowdownToInsert -{ - Slowdown slowdown; - tier4_planning_msgs::msg::PathWithLaneId::_points_type::value_type point; -}; - -/// @brief bound of an overlap range (either the first, or last bound) -struct RangeBound -{ - size_t index; - lanelet::BasicPoint2d point; - double arc_length; - double inside_distance; -}; - -/// @brief representation of an overlap between the ego footprint and some other lane -struct Overlap -{ - double inside_distance = 0.0; ///!< distance inside the overlap - double min_arc_length = std::numeric_limits::infinity(); - double max_arc_length = 0.0; - lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length - lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length -}; - -/// @brief range along the path where ego overlaps another lane -struct OverlapRange -{ - lanelet::ConstLanelet lane; - size_t entering_path_idx{}; - size_t exiting_path_idx{}; - lanelet::BasicPoint2d entering_point; // pose of the overlapping point closest along the lane - lanelet::BasicPoint2d exiting_point; // pose of the overlapping point furthest along the lane - double inside_distance{}; // [m] how much ego footprint enters the lane - mutable struct - { - std::vector overlaps; - std::optional decision; - RangeTimes times; - std::optional object{}; - } debug; -}; -using OverlapRanges = std::vector; -/// @brief representation of a lane and its current overlap range -struct OtherLane -{ - bool range_is_open = false; - RangeBound first_range_bound{}; - RangeBound last_range_bound{}; - lanelet::ConstLanelet lanelet; - lanelet::BasicPolygon2d polygon; - - explicit OtherLane(lanelet::ConstLanelet ll) : lanelet(std::move(ll)) - { - polygon = lanelet.polygon2d().basicPolygon(); - } - - [[nodiscard]] OverlapRange close_range() - { - OverlapRange range; - range.lane = lanelet; - range.entering_path_idx = first_range_bound.index; - range.entering_point = first_range_bound.point; - range.exiting_path_idx = last_range_bound.index; - range.exiting_point = last_range_bound.point; - range.inside_distance = - std::max(first_range_bound.inside_distance, last_range_bound.inside_distance); - range_is_open = false; - last_range_bound = {}; - return range; - } -}; - -/// @brief data related to the ego vehicle -struct EgoData -{ - tier4_planning_msgs::msg::PathWithLaneId path{}; - size_t first_path_idx{}; - double velocity{}; // [m/s] - double max_decel{}; // [m/s²] - geometry_msgs::msg::Pose pose; -}; - -/// @brief data needed to make decisions -struct DecisionInputs -{ - OverlapRanges ranges{}; - EgoData ego_data; - autoware_perception_msgs::msg::PredictedObjects objects{}; - std::shared_ptr route_handler{}; - lanelet::ConstLanelets lanelets{}; -}; - -/// @brief debug data -struct DebugData -{ - std::vector footprints; - std::vector slowdowns; - geometry_msgs::msg::Pose ego_pose; - OverlapRanges ranges; - lanelet::BasicPolygon2d current_footprint; - lanelet::ConstLanelets current_overlapped_lanelets; - lanelet::ConstLanelets path_lanelets; - lanelet::ConstLanelets ignored_lanelets; - lanelet::ConstLanelets other_lanelets; - tier4_planning_msgs::msg::PathWithLaneId path; - size_t first_path_idx; - - size_t prev_footprints = 0; - size_t prev_slowdowns = 0; - size_t prev_ranges = 0; - size_t prev_current_overlapped_lanelets = 0; - size_t prev_ignored_lanelets = 0; - size_t prev_path_lanelets = 0; - size_t prev_other_lanelets = 0; - void reset_data() - { - prev_footprints = footprints.size(); - footprints.clear(); - prev_slowdowns = slowdowns.size(); - slowdowns.clear(); - prev_ranges = ranges.size(); - ranges.clear(); - prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); - current_overlapped_lanelets.clear(); - prev_ignored_lanelets = ignored_lanelets.size(); - ignored_lanelets.clear(); - prev_path_lanelets = path_lanelets.size(); - path_lanelets.clear(); - prev_other_lanelets = other_lanelets.size(); - other_lanelets.clear(); - } -}; - -} // namespace autoware::behavior_velocity_planner::out_of_lane - -#endif // TYPES_HPP_