diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp index fdfd3a25eb3ad..077c66f9dd6bc 100644 --- a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -40,10 +39,10 @@ class VelocityFactorInterface void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } + template void set( - const std::vector & points, - const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string & detail = ""); + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail = ""); private: VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp index eabd00fae85d6..20742af0b6c0c 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/motion_utils/src/factor/velocity_factor_interface.cpp @@ -15,12 +15,16 @@ #include #include +#include +#include +#include + namespace motion_utils { +template void VelocityFactorInterface::set( - const std::vector & points, - const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string & detail) + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail) { const auto & curr_point = curr_pose.position; const auto & stop_point = stop_pose.position; @@ -32,4 +36,14 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, + const Pose &, const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, + const Pose &, const VelocityFactorStatus, const std::string &); + } // namespace motion_utils 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 18de04fd9e317..f8deccb188b71 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,6 +2,24 @@ + + + + + + + + + + @@ -91,12 +109,44 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/.pages b/planning/.pages index 28a791801b805..4514894605bbc 100644 --- a/planning/.pages +++ b/planning/.pages @@ -63,6 +63,10 @@ nav: - 'Surround Obstacle Checker': - 'About Surround Obstacle Checker': planning/surround_obstacle_checker - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja + - 'Motion Velocity Planner': + - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ + - 'Available Modules': + - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ - 'Motion Velocity Smoother': - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt new file mode 100644 index 0000000000000..b96ca3017a031 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_out_of_lane_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_filter_predicted_objects.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md new file mode 100644 index 0000000000000..99396eb0edf22 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -0,0 +1,165 @@ +## 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/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml new file mode 100644 index 0000000000000..e57b5a45d8be6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -0,0 +1,43 @@ +/**: + 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 + + 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 trajectory 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 trajectory + 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/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png new file mode 100644 index 0000000000000..f095467b3b0ac Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png new file mode 100644 index 0000000000000..2f358975b9491 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png new file mode 100644 index 0000000000000..fdb75ac0c6eb8 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml new file mode 100644 index 0000000000000..5ba740773d5a5 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -0,0 +1,41 @@ + + + + autoware_motion_velocity_out_of_lane_module + 0.1.0 + The motion_velocity_out_of_lane_module package + + Maxime Clement + Tomoya Kimura + Shumpei Wakabayashi + Takayuki Murooka + + Apache License 2.0 + + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_motion_velocity_planner_common + 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_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml new file mode 100644 index 0000000000000..0a05163e8f1d0 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp new file mode 100644 index 0000000000000..157d545dcac0d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -0,0 +1,85 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "calculate_slowdown_points.hpp" + +#include "footprint.hpp" + +#include +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel) +{ + // TODO(Maxime): use the library function + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.pose.position, 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); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, + const tier4_autoware_utils::Polygon2d & footprint, + const std::optional & prev_slowdown_point, const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); + const auto to_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0, decision.target_trajectory_idx); + TrajectoryPoint interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_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.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; +} + +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::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..19ed066548d0f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,58 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +namespace autoware::motion_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 TrajectoryPoint & point, const double target_vel); + +/// @brief calculate the last pose along the trajectory 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 tier4_autoware_utils::Polygon2d & footprint, + const std::optional & prev_slowdown_point, const PlannerParam & params); + +/// @brief calculate the slowdown point to insert in the trajectory +/// @param ego_data ego data (trajectory, 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 trajectory +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, + const std::optional prev_slowdown_point, PlannerParam params); +} // namespace autoware::motion_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp new file mode 100644 index 0000000000000..585f4760290ef --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -0,0 +1,242 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "debug.hpp" + +#include "types.hpp" + +#include + +#include + +#include +#include + +namespace autoware::motion_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; +} +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 std::vector & trajectory_points, + 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( + trajectory_points[first_ego_idx + range.entering_trajectory_idx].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( + trajectory_points[first_ego_idx + range.exiting_trajectory_idx].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); +} + +void add_decision_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_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++; + } + 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); + } +} +} // namespace +visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data) +{ + constexpr auto z = 0.0; + visualization_msgs::msg::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.trajectory_lanelets, "trajectory_lanelets", + tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), + debug_data.prev_trajectory_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.trajectory_points, + debug_data.first_trajectory_idx, z, debug_data.prev_ranges); + debug::add_decision_markers(debug_marker_array, debug_data.ranges, z, debug_data.prev_ranges); + return debug_marker_array; +} + +motion_utils::VirtualWalls create_virtual_walls( + const DebugData & debug_data, const PlannerParam & params) +{ + 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.pose; + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::motion_velocity_planner::out_of_lane::debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp new file mode 100644 index 0000000000000..4a5be9dfb07ac --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp @@ -0,0 +1,31 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DEBUG_HPP_ +#define DEBUG_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane::debug +{ +visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); +motion_utils::VirtualWalls create_virtual_walls( + const DebugData & debug_data, const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane::debug + +#endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp new file mode 100644 index 0000000000000..56047e459bf51 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp @@ -0,0 +1,388 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "decisions.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +namespace autoware::motion_velocity_planner::out_of_lane +{ +double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx) +{ + return motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); +} + +double time_along_trajectory( + const EgoData & ego_data, const size_t target_idx, const double min_velocity) +{ + const auto dist = distance_along_trajectory(ego_data, target_idx); + const auto v = std::max( + std::max(ego_data.velocity, min_velocity), + ego_data.trajectory_points[ego_data.first_trajectory_idx + target_idx] + .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_auto_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_auto_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 + // trajectory + 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_trajectory(inputs.ego_data, range.entering_trajectory_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_trajectory(inputs.ego_data, range.exiting_trajectory_idx, params.ego_min_velocity); + RCLCPP_DEBUG( + logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", + range.entering_trajectory_idx, range.exiting_trajectory_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_trajectory_idx = inputs.ego_data.first_trajectory_idx + + range.entering_trajectory_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; + const auto ego_dist_to_range = + distance_along_trajectory(inputs.ego_data, range.entering_trajectory_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_trajectory_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::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp new file mode 100644 index 0000000000000..2aa3a8b490bf6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DECISIONS_HPP_ +#define DECISIONS_HPP_ + +#include "types.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief calculate the distance along the ego trajectory between ego and some target trajectory +/// index +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] target_idx target ego trajectory index +/// @return distance between ego and the target [m] +double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx); +/// @brief estimate the time when ego will reach some target trajectory index +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] target_idx target ego trajectory 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_trajectory(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_auto_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_auto_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::motion_velocity_planner::out_of_lane + +#endif // DECISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp new file mode 100644 index 0000000000000..31631fb577b09 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -0,0 +1,150 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "filter_predicted_objects.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) +{ + if (predicted_path.path.empty() || stop_line.size() < 2) return; + + 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_auto_perception_msgs::msg::PredictedPath & path, + const std::shared_ptr 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->get_traffic_signal(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_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std::shared_ptr 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_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const std::shared_ptr planner_data, const EgoData & ego_data, + const PlannerParam & params) +{ + autoware_auto_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_auto_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::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp new file mode 100644 index 0000000000000..0770fbc0dcff1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -0,0 +1,60 @@ +// Copyright 2024-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 +#include + +namespace autoware::motion_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_auto_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_auto_perception_msgs::msg::PredictedPath & path, + const std::shared_ptr 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_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std::shared_ptr 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_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const std::shared_ptr planner_data, const EgoData & ego_data, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp new file mode 100644 index 0000000000000..824a847b17414 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -0,0 +1,93 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "footprint.hpp" + +#include + +#include + +#include +#include + +#include +#include + +namespace autoware::motion_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_trajectory_footprints( + const EgoData & ego_data, const PlannerParam & params) +{ + const auto base_footprint = make_base_footprint(params); + std::vector trajectory_footprints; + trajectory_footprints.reserve(ego_data.trajectory_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_trajectory_idx; + i < ego_data.trajectory_points.size() && length < range; ++i) { + const auto & trajectory_pose = ego_data.trajectory_points[i].pose; + const auto angle = tf2::getYaw(trajectory_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() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); + trajectory_footprints.push_back(footprint); + if (i + 1 < ego_data.trajectory_points.size()) + length += tier4_autoware_utils::calcDistance2d( + trajectory_pose, ego_data.trajectory_points[i + 1].pose); + } + return trajectory_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::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp new file mode 100644 index 0000000000000..7b80dd9618bb7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -0,0 +1,59 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace autoware::motion_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 trajectory 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 trajectory) +/// @param [in] params parameters +/// @return polygon footprints for each trajectory point starting from ego's current position +std::vector calculate_trajectory_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::motion_velocity_planner + +#endif // FOOTPRINT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp new file mode 100644 index 0000000000000..23dbf77f08151 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -0,0 +1,126 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lanelets_selection.hpp" + +#include + +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +lanelet::ConstLanelets consecutive_lanelets( + const std::shared_ptr 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 & trajectory_lanelets, + const std::shared_ptr 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 : trajectory_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(trajectory_lanelets, l.id()) && + contains_lanelet(consecutives, l.id()); + }); + return missing_lane_change_lanelets; +} + +lanelet::ConstLanelets calculate_trajectory_lanelets( + const EgoData & ego_data, const std::shared_ptr route_handler) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + lanelet::ConstLanelets trajectory_lanelets; + lanelet::BasicLineString2d trajectory_ls; + for (const auto & p : ego_data.trajectory_points) + trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); + for (const auto & dist_lanelet : + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, trajectory_ls)) { + if (!contains_lanelet(trajectory_lanelets, dist_lanelet.second.id())) + trajectory_lanelets.push_back(dist_lanelet.second); + } + const auto missing_lanelets = + get_missing_lane_change_lanelets(trajectory_lanelets, route_handler); + trajectory_lanelets.insert( + trajectory_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); + return trajectory_lanelets; +} + +lanelet::ConstLanelets calculate_ignored_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params) +{ + lanelet::ConstLanelets ignored_lanelets; + // ignore lanelets directly behind ego + const auto behind = + tier4_autoware_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); + const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.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_trajectory_lanelet = contains_lanelet(trajectory_lanelets, l.second.id()); + if (!is_trajectory_lanelet) ignored_lanelets.push_back(l.second); + } + return ignored_lanelets; +} + +lanelet::ConstLanelets calculate_other_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets, + const std::shared_ptr 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_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.second.id()); + const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); + if (!is_trajectory_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); + } + return other_lanelets; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp new file mode 100644 index 0000000000000..c7351f0a98e89 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANELETS_SELECTION_HPP_ +#define LANELETS_SELECTION_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace autoware::motion_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 trajectory +/// @details calculated from the ids of the trajectory msg, the lanelets containing trajectory +/// 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_trajectory_lanelets( + const EgoData & ego_data, const std::shared_ptr route_handler); +/// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during +/// a lane change +/// @param [in] trajectory_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 +/// trajectory_lanelets) +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler); +/// @brief calculate lanelets that should be ignored +/// @param [in] ego_data data about the ego vehicle +/// @param [in] trajectory_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 & trajectory_lanelets, + const std::shared_ptr 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] trajectory_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 & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp new file mode 100644 index 0000000000000..b662186049342 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -0,0 +1,305 @@ +// Copyright 2024 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 "out_of_lane_module.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 + +namespace autoware::motion_velocity_planner +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + logger_ = node.get_logger(); + clock_ = node.get_clock(); + init_parameters(node); + velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + debug_publisher_ = + node.create_publisher("~/" + ns_ + "/debug_markers", 1); + virtual_wall_publisher_ = + node.create_publisher("~/" + ns_ + "/virtual_walls", 1); +} +void OutOfLaneModule::init_parameters(rclcpp::Node & node) +{ + using tier4_autoware_utils::getOrDeclareParameter; + auto & pp = params_; + + pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); + pp.skip_if_already_overlapping = + getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); + + 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 OutOfLaneModule::update_parameters(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + auto & pp = params_; + updateParam(parameters, ns_ + ".mode", pp.mode); + updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); + + updateParam(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); + updateParam(parameters, ns_ + ".intervals.ego_time_buffer", pp.intervals_ego_buffer); + updateParam(parameters, ns_ + ".intervals.objects_time_buffer", pp.intervals_obj_buffer); + updateParam(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); + + updateParam(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); + updateParam(parameters, ns_ + ".objects.use_predicted_paths", pp.objects_use_predicted_paths); + updateParam( + parameters, ns_ + ".objects.predicted_path_min_confidence", pp.objects_min_confidence); + updateParam(parameters, ns_ + ".objects.distance_buffer", pp.objects_dist_buffer); + updateParam( + parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", + pp.objects_cut_predicted_paths_beyond_red_lights); + + updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); + updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); + + updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); + updateParam(parameters, ns_ + ".action.precision", pp.precision); + updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); + updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer); + updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); + updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); + updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); + + updateParam(parameters, ns_ + ".ego.min_assumed_velocity", pp.ego_min_velocity); + updateParam(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); + updateParam(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); + updateParam(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); + updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); +} + +VelocityPlanningResult OutOfLaneModule::plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + VelocityPlanningResult result; + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + out_of_lane::EgoData ego_data; + ego_data.pose = planner_data->current_odometry->pose; + ego_data.trajectory_points = ego_trajectory_points; + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.velocity = planner_data->current_velocity->twist.linear.x; + ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); + stopwatch.tic("calculate_trajectory_footprints"); + const auto current_ego_footprint = + out_of_lane::calculate_current_ego_footprint(ego_data, params_, true); + const auto trajectory_footprints = + out_of_lane::calculate_trajectory_footprints(ego_data, params_); + const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); + // Calculate lanelets to ignore and consider + stopwatch.tic("calculate_lanelets"); + const auto trajectory_lanelets = + out_of_lane::calculate_trajectory_lanelets(ego_data, planner_data->route_handler); + const auto ignored_lanelets = out_of_lane::calculate_ignored_lanelets( + ego_data, trajectory_lanelets, planner_data->route_handler, params_); + const auto other_lanelets = out_of_lane::calculate_other_lanelets( + ego_data, trajectory_lanelets, ignored_lanelets, planner_data->route_handler, params_); + const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); + + debug_data_.reset_data(); + debug_data_.trajectory_points = ego_trajectory_points; + debug_data_.footprints = trajectory_footprints; + debug_data_.trajectory_lanelets = trajectory_lanelets; + debug_data_.ignored_lanelets = ignored_lanelets; + debug_data_.other_lanelets = other_lanelets; + debug_data_.first_trajectory_idx = ego_data.first_trajectory_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); + RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module\n"); + return result; + } + } + // Calculate overlapping ranges + stopwatch.tic("calculate_overlapping_ranges"); + const auto ranges = out_of_lane::calculate_overlapping_ranges( + trajectory_footprints, trajectory_lanelets, other_lanelets, params_); + const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); + // Calculate stop and slowdown points + out_of_lane::DecisionInputs inputs; + inputs.ranges = ranges; + inputs.ego_data = ego_data; + stopwatch.tic("filter_predicted_objects"); + inputs.objects = out_of_lane::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 = out_of_lane::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 = + out_of_lane::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( + ego_trajectory_points, 0LU, point_to_insert->point.pose.position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, prev_inserted_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 trajectory changed the prev point is no longer on the trajectory so we project it + const auto insert_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + prev_inserted_point_->point.pose = + motion_utils::calcInterpolatedPose(ego_trajectory_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}; + if (point_to_insert->slowdown.velocity == 0.0) + result.stop_points.push_back(point_to_insert->point.pose.position); + else + result.slowdown_intervals.emplace_back( + point_to_insert->point.pose.position, point_to_insert->point.pose.position, + point_to_insert->slowdown.velocity); + + const auto is_approaching = motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.pose.position, + point_to_insert->point.pose.position) > 0.1 && + ego_data.velocity > 0.1; + const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING + : motion_utils::VelocityFactor::STOPPED; + velocity_factor_interface_.set( + ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); + result.velocity_factor = velocity_factor_interface_.get(); + } 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_trajectory_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_trajectory_footprints_us, + calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calc_slowdown_points_us, insert_slowdown_points_us); + debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); + virtual_wall_marker_creator.add_virtual_walls( + out_of_lane::debug::create_virtual_walls(debug_data_, params_)); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + return result; +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::OutOfLaneModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp new file mode 100644 index 0000000000000..8a20d5c850a09 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OUT_OF_LANE_MODULE_HPP_ +#define OUT_OF_LANE_MODULE_HPP_ + +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class OutOfLaneModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override { return module_name_; } + +private: + void init_parameters(rclcpp::Node & node); + out_of_lane::PlannerParam params_; + + inline static const std::string ns_ = "out_of_lane"; + std::string module_name_; + std::optional prev_inserted_point_{}; + rclcpp::Clock::SharedPtr clock_{}; + rclcpp::Time prev_inserted_point_time_{}; + +protected: + // Debug + mutable out_of_lane::DebugData debug_data_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OUT_OF_LANE_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp new file mode 100644 index 0000000000000..f89c620b75fb6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -0,0 +1,127 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "overlapping_range.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +Overlap calculate_overlap( + const lanelet::BasicPolygon2d & trajectory_footprint, + const lanelet::ConstLanelets & trajectory_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 trajectory footprint, for each lanelet) are too + // expensive + const auto overlap_left = boost::geometry::intersects(trajectory_footprint, left_bound); + const auto overlap_right = boost::geometry::intersects(trajectory_footprint, right_bound); + + lanelet::BasicPolygons2d overlapping_polygons; + if (overlap_left || overlap_right) + boost::geometry::intersection( + trajectory_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(trajectory_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 & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, + const PlannerParam & params) +{ + OverlapRanges ranges; + OtherLane other_lane(lanelet); + std::vector overlaps; + for (auto i = 0UL; i < trajectory_footprints.size(); ++i) { + const auto overlap = calculate_overlap(trajectory_footprints[i], trajectory_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 & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, + const PlannerParam & params) +{ + OverlapRanges ranges; + for (auto & lanelet : lanelets) { + const auto lanelet_ranges = + calculate_overlapping_ranges(trajectory_footprints, trajectory_lanelets, lanelet, params); + ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); + } + return ranges; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp new file mode 100644 index 0000000000000..07c8663ea21bc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OVERLAPPING_RANGE_HPP_ +#define OVERLAPPING_RANGE_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace autoware::motion_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] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @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 & trajectory_footprint, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet); +/// @brief calculate the overlapping ranges between the trajectory footprints and a lanelet +/// @param [in] trajectory_footprints footprints used to calculate the overlaps +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @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 & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, + const PlannerParam & params); +/// @brief calculate the overlapping ranges between the trajectory footprints and some lanelets +/// @param [in] trajectory_footprints footprints used to calculate the overlaps +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @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 trajectory +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp new file mode 100644 index 0000000000000..04f9f1ccac4c2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -0,0 +1,236 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES_HPP_ +#define TYPES_HPP_ + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +/// @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 + + 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 trajectory 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 trajectory + 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_trajectory_idx{}; // we want to slowdown before this trajectory 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 trajectory +struct SlowdownToInsert +{ + Slowdown slowdown{}; + autoware_auto_planning_msgs::msg::TrajectoryPoint 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 trajectory where ego overlaps another lane +struct OverlapRange +{ + lanelet::ConstLanelet lane{}; + size_t entering_trajectory_idx{}; + size_t exiting_trajectory_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_trajectory_idx = first_range_bound.index; + range.entering_point = first_range_bound.point; + range.exiting_trajectory_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 +{ + std::vector trajectory_points{}; + size_t first_trajectory_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_auto_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 trajectory_lanelets; + lanelet::ConstLanelets ignored_lanelets; + lanelet::ConstLanelets other_lanelets; + std::vector trajectory_points; + size_t first_trajectory_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_trajectory_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_trajectory_lanelets = trajectory_lanelets.size(); + trajectory_lanelets.clear(); + prev_other_lanelets = other_lanelets.size(); + other_lanelets.clear(); + } +}; + +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp new file mode 100644 index 0000000000000..4961cd064efaf --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp @@ -0,0 +1,58 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/filter_predicted_objects.hpp" + +#include +#include +#include + +#include +#include + +TEST(TestCollisionDistance, CutPredictedPathBeyondLine) +{ + using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line; + autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + lanelet::BasicLineString2d stop_line; + double object_front_overhang = 0.0; + const auto eps = 1e-9; + + EXPECT_NO_THROW(cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang)); + + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + predicted_path.path.push_back(pose); + pose.position.y = 1.0; + predicted_path.path.push_back(pose); + pose.position.y = 2.0; + predicted_path.path.push_back(pose); + pose.position.y = 3.0; + predicted_path.path.push_back(pose); + + cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang); + EXPECT_EQ(predicted_path.path.size(), 4UL); + for (auto i = 0UL; i < predicted_path.path.size(); ++i) { + EXPECT_EQ(predicted_path.path[i].position.x, 0.0); + EXPECT_NEAR(predicted_path.path[i].position.y, static_cast(i), eps); + } + stop_line = {{-1.0, 2.0}, {1.0, 2.0}}; + cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang); + EXPECT_EQ(predicted_path.path.size(), 2UL); + for (auto i = 0UL; i < predicted_path.path.size(); ++i) { + EXPECT_EQ(predicted_path.path[i].position.x, 0.0); + EXPECT_NEAR(predicted_path.path[i].position.y, static_cast(i), eps); + } +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..f1eb7ff047fdc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) + +autoware_package() + +# ament_auto_add_library(${PROJECT_NAME}_lib SHARED +# DIRECTORY src +# ) + +ament_auto_package(INSTALL_TO_SHARE + include +) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp new file mode 100644 index 0000000000000..84591f9429a66 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -0,0 +1,106 @@ +// Copyright 2024 Autoware Foundation +// +// 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 AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + autoware_perception_msgs::msg::TrafficSignal signal; +}; +struct PlannerData +{ + explicit PlannerData(rclcpp::Node & node) + : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + { + } + + // msgs from callbacks that are used for data-ready + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; + autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + pcl::PointCloud::ConstPtr no_ground_pointcloud; + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + std::shared_ptr route_handler; + + // nearest search + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + + // other internal data + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; + std::optional external_velocity_limit; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + + // velocity smoother + std::shared_ptr velocity_smoother_{}; + // parameters + vehicle_info_util::VehicleInfo vehicle_info_; + + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional get_traffic_signal( + const lanelet::Id id, const bool keep_last_observation = false) const + { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + return std::make_optional(traffic_light_id_map.at(id)); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp new file mode 100644 index 0000000000000..9011d3fb7e079 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ + +#include "planner_data.hpp" +#include "velocity_planning_result.hpp" + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ + +class PluginModuleInterface +{ +public: + virtual ~PluginModuleInterface() = default; + virtual void init(rclcpp::Node & node, const std::string & module_name) = 0; + virtual void update_parameters(const std::vector & parameters) = 0; + virtual VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) = 0; + virtual std::string get_module_name() const = 0; + motion_utils::VelocityFactorInterface velocity_factor_interface_; + rclcpp::Logger logger_ = rclcpp::get_logger(""); + rclcpp::Publisher::SharedPtr debug_publisher_; + rclcpp::Publisher::SharedPtr virtual_wall_publisher_; + motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp new file mode 100644 index 0000000000000..9b011b74503f2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 Autoware Foundation +// +// 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 AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct SlowdownInterval +{ + SlowdownInterval( + const geometry_msgs::msg::Point & from_, const geometry_msgs::msg::Point & to_, + const double vel) + : from{from_}, to{to_}, velocity{vel} + { + } + geometry_msgs::msg::Point from{}; + geometry_msgs::msg::Point to{}; + double velocity{}; +}; +struct VelocityPlanningResult +{ + std::vector stop_points{}; + std::vector slowdown_intervals{}; + std::optional velocity_factor{}; +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml new file mode 100644 index 0000000000000..ad791d247ee11 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -0,0 +1,40 @@ + + + + autoware_motion_velocity_planner_common + 0.1.0 + Common functions and interfaces for motion_velocity_planner modules + + Maxime Clement + + Apache License 2.0 + + Maxime Clement + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + motion_velocity_smoother + rclcpp + route_handler + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt new file mode 100644 index 0000000000000..0af4da6fd4426 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_planner_node) + +find_package(autoware_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "srv/LoadPlugin.srv" + "srv/UnloadPlugin.srv" + DEPENDENCIES +) + +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + DIRECTORY src +) + +rclcpp_components_register_node(${PROJECT_NAME}_lib + PLUGIN "autoware::motion_velocity_planner::MotionVelocityPlannerNode" + EXECUTABLE ${PROJECT_NAME}_exe +) + +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${PROJECT_NAME}_lib + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") +endif() + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME}_lib + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md new file mode 100644 index 0000000000000..f8c5eb172abd2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -0,0 +1,58 @@ +# Motion Velocity Planner + +## Overview + +`motion_velocity_planner` is a planner to adjust the trajectory velocity based on the obstacles around the vehicle. +It loads modules as plugins. Please refer to the links listed below for detail on each module. + +![Architecture](./docs/MotionVelocityPlanner-InternalInterface.drawio.svg) + +- [Out of Lane](../autoware_motion_velocity_out_of_lane_module/README.md) + +Each module calculates stop and slow down points to be inserted in the ego trajectory. +These points are assumed to correspond to the `base_link` frame of the ego vehicle as it follows the trajectory. +This means that to stop before a wall, a stop point is inserted in the trajectory at a distance ahead of the wall equal to the vehicle front offset (wheelbase + front overhang, see the [vehicle dimensions](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/). + +![set_stop_velocity](./docs/set_stop_velocity.drawio.svg) + +## Input topics + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------------- | ----------------------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | input trajectory | +| `~/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | +| `~/input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | +| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | + +## Output topics + +| Name | Type | Description | +| --------------------------- | ------------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | + +## Services + +| Name | Type | Description | +| ------------------------- | -------------------------------------------------------- | ---------------------------- | +| `~/service/load_plugin` | autoware_motion_velocity_planner_node::srv::LoadPlugin | To request loading a plugin | +| `~/service/unload_plugin` | autoware_motion_velocity_planner_node::srv::UnloadPlugin | To request unloaded a plugin | + +## Node parameters + +| Parameter | Type | Description | +| ---------------- | ---------------- | ---------------------- | +| `launch_modules` | vector\ | module names to launch | + +In addition, the following parameters should be provided to the node: + +- [nearest search parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml); +- [vehicle info parameters](https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml); +- [common planning parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml); +- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/motion_velocity_smoother/#parameters) +- Parameters of each plugin that will be loaded. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml new file mode 100644 index 0000000000000..5b2fea537d4f7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg new file mode 100644 index 0000000000000..ea3b7ad99d4e5 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg @@ -0,0 +1,538 @@ + + + + + + + + + + + + + + + +
+
+
+ ~/input/dynamic_objects +
+
+
+ + + + + + + + + + + + + +
+
+
+ ~/input/vehicle_odometry +
+
+
+ + + + + + + + + + + + + +
+
+
+ ~/input/trajectory +
+
+
+ + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + + + + Motion Velocity Planner + + + + + + + + + +
+
+
+ PlanVelocity +
+
+
+ + + + + + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + + + + +
+
+
+ + Updated +
+ Trajectory + +
+
+
+ + + + + + + + + + + + + +
+
+
+
Stop/Slowdown
+
+ points +
+
+
+
+
+ + + + + + + + + + + + + +
+
+
+ Data +
+
+
+ + + + + + + + + + + + +
+
+
+ Planner Data +
+
+
+ + + + + + + + + + + + + Loaded Modules + + + + + + + + + + out_of_lane + + + + + + + + + + obstacle_velocity_limiter + + + + + + + + + + dynamic_obstacle_stop + + + + + + + + + +
+
+
...
+
+
+ + + + + + + + + + + + +
+
+
+ Smooth Velocity +
+
+
+ + + + + + + + + + + + + obstacle_stop + + + + + + + + + + +
+
+
+ ~/input/vector_map +
+
+
+ + + + + + + + + + + + +
+
+
+ + ~/output/trajectory + +
+
+
+ + + + + + + + + + + + + +
+
+
+ ... +
+
+
+ + + + + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg new file mode 100644 index 0000000000000..5ae7d7dbb5461 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg @@ -0,0 +1,197 @@ + + + + + + + + + + + +
+
+
+ +

+ + stop point + +

+ +
+
+
+ + stop point + + + + + + + + + + + +
+
+
+ +

+ + v + +

+ +
+
+
+ + v + + + + + + +
+
+
+ +

+ + x + +

+ +
+
+
+ + x + + + + + + +
+
+
+ +

+ + baselink to front + +

+ +
+
+
+ + baselink to front + + + + + + + + + + + + + + +
+
+
+ +

+ + output velocity + +

+ +
+
+
+ + output velocity + + + + + + + +
+
+
+ +

+ + reference velocity + +

+ +
+
+
+ + reference velocity + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml new file mode 100644 index 0000000000000..3732d86ef380a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml new file mode 100644 index 0000000000000..2b52610c0276a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -0,0 +1,57 @@ + + + + autoware_motion_velocity_planner_node + 0.1.0 + Node of the motion_velocity_planner + + Maxime Clement + + Apache License 2.0 + + Maxime Clement + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + rosidl_default_generators + + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_motion_velocity_planner_common + diagnostic_msgs + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + motion_velocity_smoother + pcl_conversions + pluginlib + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + rosidl_default_runtime + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_motion_velocity_out_of_lane_module + autoware_planning_test_manager + + rosidl_interface_packages + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json new file mode 100644 index 0000000000000..7db22e5e39d17 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for the Motion Velocity Planner", + "type": "object", + "definitions": { + "motion_velocity_planner": { + "type": "object", + "properties": { + "smooth_velocity_before_planning": { + "type": "boolean", + "default": true, + "description": "if true, smooth the velocity profile of the input trajectory before planning" + } + }, + "required": ["smooth_velocity_before_planning"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/motion_velocity_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp new file mode 100644 index 0000000000000..c0373678270dd --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -0,0 +1,458 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "node.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace +{ +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node_ptr) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} +} // namespace + +namespace autoware::motion_velocity_planner +{ +MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & node_options) +: Node("motion_velocity_planner_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + planner_data_(*this) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + // Subscribers + sub_trajectory_ = this->create_subscription( + "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1), + create_subscription_options(this)); + sub_predicted_objects_ = + this->create_subscription( + "~/input/dynamic_objects", 1, + std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1), + create_subscription_options(this)); + sub_no_ground_pointcloud_ = this->create_subscription( + "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), + std::bind(&MotionVelocityPlannerNode::on_no_ground_pointcloud, this, _1), + create_subscription_options(this)); + sub_vehicle_odometry_ = this->create_subscription( + "~/input/vehicle_odometry", 1, std::bind(&MotionVelocityPlannerNode::on_odometry, this, _1), + create_subscription_options(this)); + sub_acceleration_ = this->create_subscription( + "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), + create_subscription_options(this)); + sub_lanelet_map_ = this->create_subscription( + "~/input/vector_map", rclcpp::QoS(10).transient_local(), + std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1), + create_subscription_options(this)); + sub_traffic_signals_ = + this->create_subscription( + "~/input/traffic_signals", 1, + std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1), + create_subscription_options(this)); + sub_virtual_traffic_light_states_ = + this->create_subscription( + "~/input/virtual_traffic_light_states", 1, + std::bind(&MotionVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), + create_subscription_options(this)); + sub_occupancy_grid_ = this->create_subscription( + "~/input/occupancy_grid", 1, std::bind(&MotionVelocityPlannerNode::on_occupancy_grid, this, _1), + create_subscription_options(this)); + + srv_load_plugin_ = create_service( + "~/service/load_plugin", std::bind(&MotionVelocityPlannerNode::on_load_plugin, this, _1, _2)); + srv_unload_plugin_ = create_service( + "~/service/unload_plugin", + std::bind(&MotionVelocityPlannerNode::on_unload_plugin, this, _1, _2)); + + // Publishers + trajectory_pub_ = + this->create_publisher("~/output/trajectory", 1); + velocity_factor_publisher_ = + this->create_publisher( + "~/output/velocity_factors", 1); + + // Parameters + smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); + // nearest search + planner_data_.ego_nearest_dist_threshold = + declare_parameter("ego_nearest_dist_threshold"); + planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + // set velocity smoother param + set_velocity_smoother_params(); + + // Initialize PlannerManager + for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } + planner_manager_.load_module_plugin(*this, name); + } + + set_param_callback_ = this->add_on_set_parameters_callback( + std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); +} + +void MotionVelocityPlannerNode::on_load_plugin( + const LoadPlugin::Request::SharedPtr request, + [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.load_module_plugin(*this, request->plugin_name); +} + +void MotionVelocityPlannerNode::on_unload_plugin( + const UnloadPlugin::Request::SharedPtr request, + [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.unload_module_plugin(*this, request->plugin_name); +} + +// NOTE: argument planner_data must not be referenced for multithreading +bool MotionVelocityPlannerNode::is_data_ready() const +{ + const auto & d = planner_data_; + auto clock = *get_clock(); + const auto check_with_msg = [&](const auto ptr, const auto & msg) { + constexpr auto throttle_duration = 3000; // [ms] + if (!ptr) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, msg); + return false; + } + return true; + }; + + return check_with_msg(d.current_odometry, "Waiting for current odometry") && + check_with_msg(d.current_velocity, "Waiting for current velocity") && + check_with_msg(d.current_acceleration, "Waiting for current acceleration") && + check_with_msg(d.predicted_objects, "Waiting for predicted objects") && + check_with_msg(d.no_ground_pointcloud, "Waiting for pointcloud") && + check_with_msg(map_ptr_, "Waiting for the map") && + check_with_msg( + d.velocity_smoother_, "Waiting for the initialization of the velocity smoother") && + check_with_msg(d.occupancy_grid, "Waiting for the occupancy grid"); +} + +void MotionVelocityPlannerNode::on_occupancy_grid( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.occupancy_grid = msg; +} + +void MotionVelocityPlannerNode::on_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.predicted_objects = msg; +} + +void MotionVelocityPlannerNode::on_no_ground_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_.lookupTransform( + "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & e) { + RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what()); + return; + } + + pcl::PointCloud pc; + pcl::fromROSMsg(*msg, pc); + + Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); + pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); + if (!pc.empty()) { + tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); + } + + { + std::lock_guard lock(mutex_); + planner_data_.no_ground_pointcloud = pc_transformed; + } +} + +void MotionVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + auto current_odometry = std::make_shared(); + current_odometry->header = msg->header; + current_odometry->pose = msg->pose.pose; + planner_data_.current_odometry = current_odometry; + + auto current_velocity = std::make_shared(); + current_velocity->header = msg->header; + current_velocity->twist = msg->twist.twist; + planner_data_.current_velocity = current_velocity; +} + +void MotionVelocityPlannerNode::on_acceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.current_acceleration = msg; +} + +void MotionVelocityPlannerNode::set_velocity_smoother_params() +{ + planner_data_.velocity_smoother_ = + std::make_shared(*this); +} + +void MotionVelocityPlannerNode::on_lanelet_map( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + map_ptr_ = msg; + has_received_map_ = true; +} + +void MotionVelocityPlannerNode::on_traffic_signals( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); + for (const auto & signal : msg->signals) { + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; + traffic_signal.signal = signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + old_data->second; + // update timestamp + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + msg->stamp; + } else { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + } + } +} + +void MotionVelocityPlannerNode::on_virtual_traffic_light_states( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.virtual_traffic_light_states = msg; +} + +void MotionVelocityPlannerNode::on_trajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) +{ + std::unique_lock lk(mutex_); + + if (!is_data_ready()) { + return; + } + + if (input_trajectory_msg->points.empty()) { + RCLCPP_WARN(get_logger(), "Input trajectory message is empty"); + return; + } + + if (has_received_map_) { + planner_data_.route_handler = std::make_shared(*map_ptr_); + has_received_map_ = false; + } + + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ + input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()}; + + auto output_trajectory_msg = generate_trajectory(input_trajectory_points); + output_trajectory_msg.header = input_trajectory_msg->header; + + lk.unlock(); + + trajectory_pub_->publish(output_trajectory_msg); + published_time_publisher_->publish_if_subscribed( + trajectory_pub_, output_trajectory_msg.header.stamp); +} + +void MotionVelocityPlannerNode::insert_stop( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & stop_point) const +{ + const auto seg_idx = motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); + const auto insert_idx = motion_utils::insertTargetPoint(seg_idx, stop_point, trajectory.points); + if (insert_idx) { + for (auto idx = *insert_idx; idx < trajectory.points.size(); ++idx) + trajectory.points[idx].longitudinal_velocity_mps = 0.0; + } else { + RCLCPP_WARN(get_logger(), "Failed to insert stop point"); + } +} + +void MotionVelocityPlannerNode::insert_slowdown( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const +{ + const auto from_seg_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.from); + const auto from_insert_idx = + motion_utils::insertTargetPoint(from_seg_idx, slowdown_interval.from, trajectory.points); + const auto to_seg_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.to); + const auto to_insert_idx = + motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); + if (from_insert_idx && to_insert_idx) { + for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) + trajectory.points[idx].longitudinal_velocity_mps = 0.0; + } else { + RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); + } +} + +autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::smooth_trajectory( + const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, + const autoware::motion_velocity_planner::PlannerData & planner_data) const +{ + const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry->pose; + const double v0 = planner_data.current_velocity->twist.linear.x; + const double a0 = planner_data.current_acceleration->accel.accel.linear.x; + const auto & external_v_limit = planner_data.external_velocity_limit; + const auto & smoother = planner_data.velocity_smoother_; + + const auto traj_lateral_acc_filtered = + smoother->applyLateralAccelerationFilter(trajectory_points); + + const auto traj_steering_rate_limited = + smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false); + + // Resample trajectory with ego-velocity based interval distances + auto traj_resampled = smoother->resampleTrajectory( + traj_steering_rate_limited, v0, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + std::vector debug_trajectories; + // Clip trajectory from closest point + autoware::motion_velocity_planner::TrajectoryPoints clipped; + autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + RCLCPP_ERROR(get_logger(), "failed to smooth"); + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + + if (external_v_limit) { + motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + } + return traj_smoothed; +} + +autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) +{ + autoware_auto_planning_msgs::msg::Trajectory output_trajectory_msg; + output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; + if (smooth_velocity_before_planning_) + input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); + + const auto planning_results = planner_manager_.plan_velocities( + input_trajectory_points, std::make_shared(planner_data_)); + + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; + velocity_factors.header.frame_id = "map"; + velocity_factors.header.stamp = get_clock()->now(); + + for (const auto & planning_result : planning_results) { + for (const auto & stop_point : planning_result.stop_points) + insert_stop(output_trajectory_msg, stop_point); + for (const auto & slowdown_interval : planning_result.slowdown_intervals) + insert_slowdown(output_trajectory_msg, slowdown_interval); + if (planning_result.velocity_factor) + velocity_factors.factors.push_back(*planning_result.velocity_factor); + } + + velocity_factor_publisher_->publish(velocity_factors); + return output_trajectory_msg; +} + +rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + { + std::unique_lock lk(mutex_); // for planner_manager_ + planner_manager_.update_module_parameters(parameters); + } + + updateParam(parameters, "smooth_velocity_before_planning", smooth_velocity_before_planning_); + updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); + updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); + + set_velocity_smoother_params(); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} +} // namespace autoware::motion_velocity_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion_velocity_planner::MotionVelocityPlannerNode) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp new file mode 100644 index 0000000000000..ecf128bf9a012 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -0,0 +1,140 @@ +// Copyright 2024 Autoware Foundation +// +// 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 NODE_HPP_ +#define NODE_HPP_ + +#include "planner_manager.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_motion_velocity_planner_node::srv::LoadPlugin; +using autoware_motion_velocity_planner_node::srv::UnloadPlugin; +using TrajectoryPoints = std::vector; + +class MotionVelocityPlannerNode : public rclcpp::Node +{ +public: + explicit MotionVelocityPlannerNode(const rclcpp::NodeOptions & node_options); + +private: + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // subscriber + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_objects_; + rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; + rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; + rclcpp::Subscription::SharedPtr sub_acceleration_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr + sub_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_virtual_traffic_light_states_; + rclcpp::Subscription::SharedPtr sub_occupancy_grid_; + + void on_trajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); + void on_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + void on_lanelet_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_traffic_signals( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void on_virtual_traffic_light_states( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); + void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); + + // publishers + rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr + velocity_factor_publisher_; + + // parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; + bool smooth_velocity_before_planning_{}; + /// @brief set parameters of the velocity smoother + void set_velocity_smoother_params(); + + // members + PlannerData planner_data_; + MotionVelocityPlannerManager planner_manager_; + HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + bool has_received_map_ = false; + + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; + void on_unload_plugin( + const UnloadPlugin::Request::SharedPtr request, + const UnloadPlugin::Response::SharedPtr response); + void on_load_plugin( + const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + rcl_interfaces::msg::SetParametersResult on_set_param( + const std::vector & parameters); + + // mutex for planner_data_ + std::mutex mutex_; + + // function + bool is_data_ready() const; + void insert_stop( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & stop_point) const; + void insert_slowdown( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const; + autoware::motion_velocity_planner::TrajectoryPoints smooth_trajectory( + const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, + const autoware::motion_velocity_planner::PlannerData & planner_data) const; + autoware_auto_planning_msgs::msg::Trajectory generate_trajectory( + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); + + std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // NODE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp new file mode 100644 index 0000000000000..fa3a3b1ae5dcb --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -0,0 +1,83 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "planner_manager.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ + +MotionVelocityPlannerManager::MotionVelocityPlannerManager() +: plugin_loader_( + "autoware_motion_velocity_planner_node", + "autoware::motion_velocity_planner::PluginModuleInterface") +{ +} + +void MotionVelocityPlannerManager::load_module_plugin(rclcpp::Node & node, const std::string & name) +{ + // Check if the plugin is already loaded. + if (plugin_loader_.isClassLoaded(name)) { + RCLCPP_WARN_STREAM(node.get_logger(), "The plugin '" << name << "' is already loaded."); + return; + } + if (plugin_loader_.isClassAvailable(name)) { + const auto plugin = plugin_loader_.createSharedInstance(name); + plugin->init(node, name); + + // register + loaded_plugins_.push_back(plugin); + RCLCPP_DEBUG_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); + } else { + RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); + } +} + +void MotionVelocityPlannerManager::unload_module_plugin( + rclcpp::Node & node, const std::string & name) +{ + auto it = std::remove_if(loaded_plugins_.begin(), loaded_plugins_.end(), [&](const auto plugin) { + return plugin->get_module_name() == name; + }); + + if (it == loaded_plugins_.end()) { + RCLCPP_WARN_STREAM( + node.get_logger(), + "The scene plugin '" << name << "' is not found in the registered modules."); + } else { + loaded_plugins_.erase(it, loaded_plugins_.end()); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + } +} + +void MotionVelocityPlannerManager::update_module_parameters( + const std::vector & parameters) +{ + for (auto & plugin : loaded_plugins_) plugin->update_parameters(parameters); +} + +std::vector MotionVelocityPlannerManager::plan_velocities( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + std::vector results; + for (auto & plugin : loaded_plugins_) + results.push_back(plugin->plan(ego_trajectory_points, planner_data)); + return results; +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp new file mode 100644 index 0000000000000..a5e330535fc73 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 Autoware Foundation +// +// 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 PLANNER_MANAGER_HPP_ +#define PLANNER_MANAGER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MotionVelocityPlannerManager +{ +public: + MotionVelocityPlannerManager(); + void load_module_plugin(rclcpp::Node & node, const std::string & name); + void unload_module_plugin(rclcpp::Node & node, const std::string & name); + void update_module_parameters(const std::vector & parameters); + std::vector plan_velocities( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data); + +private: + pluginlib::ClassLoader plugin_loader_; + std::vector> loaded_plugins_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PLANNER_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp new file mode 100644 index 0000000000000..3ad5bab8e070b --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp @@ -0,0 +1,139 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "node.hpp" + +#include +#include +#include + +#include + +#include + +using autoware::motion_velocity_planner::MotionVelocityPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: motion_velocity_planner → test_node_ + test_manager->setTrajectorySubscriber("motion_velocity_planner_node/output/trajectory"); + + // set motion_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("motion_velocity_planner_node/input/trajectory"); + + test_manager->setInitialPoseTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto motion_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node"); + const auto motion_velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + + const auto get_motion_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_motion_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + std::vector module_names; + module_names.emplace_back("autoware::motion_velocity_planner::OutOfLaneModule"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + params.emplace_back("is_simulation", false); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", + motion_velocity_smoother_dir + "/config/Analytical.param.yaml", + motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", + get_motion_velocity_module_config("out_of_lane")}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "motion_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "motion_velocity_planner_node/input/traffic_signals"); + test_manager->publishVirtualTrafficLightState( + test_target_node, "motion_velocity_planner_node/input/virtual_traffic_light_states"); + test_manager->publishOccupancyGrid( + test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + + // make sure motion_velocity_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromTrajectory(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 887e1167a7c3e..c40bfac40a135 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -85,7 +85,8 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning "/planning/velocity_factors/surround_obstacle", "/planning/velocity_factors/traffic_light", "/planning/velocity_factors/virtual_traffic_light", - "/planning/velocity_factors/walkway"}; + "/planning/velocity_factors/walkway", + "/planning/velocity_factors/motion_velocity_planner"}; std::vector steering_factor_topics = { "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection",