diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 126e66772920a..0ce42fbe1080f 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f); + } // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index ce26952c3d634..2e09d65e35224 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -599,4 +599,27 @@ template bool isTargetPointFront & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity) +{ + const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point); + if (nearest_segment_idx + 1 == trajectory.size()) { + return; + } + for (auto & p : trajectory) { + p.time_from_start = rclcpp::Duration::from_seconds(0); + } + // TODO(Maxime): some points can have very low velocities which introduce huge time errors + // Temporary solution: use a minimum velocity + for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) { + const auto & from = trajectory[idx - 1]; + auto & to = trajectory[idx]; + const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps); + if (velocity != 0.0) { + const auto t = universe_utils::calcDistance2d(from, to) / velocity; + to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start; + } + } +} } // namespace autoware::motion_utils 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 index 99396eb0edf22..54b7cebb7c8ff 100644 --- 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 @@ -129,33 +129,24 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | -------------------- | ------ | ---------------------------------------------------------------- | | `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 /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 | + +| Parameter /overlap | Type | Description | +| ------------------ | ------ | --------------------------------------------------------------------- | +| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | + +| Parameter /action | Type | Description | +| ----------------------------- | ------ | --------------------------------------------------------------------- | +| `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 | | -------------------- | ------ | ---------------------------------------------------- | 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 index a0cf69ee71027..67db597d80752 100644 --- 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 @@ -1,32 +1,23 @@ /**: 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" + mode: ttc # mode used to consider a conflict with an object. "threshold", or "ttc" skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored + max_arc_length: 100.0 # [m] maximum trajectory arc length that is checked for out_of_lane collisions 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 ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored - 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 longitudinal_distance_buffer: 1.5 # [m] safety distance buffer to keep in front of the ego vehicle lateral_distance_buffer: 1.0 # [m] safety distance buffer to keep on the side of the ego vehicle @@ -38,8 +29,8 @@ 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 + # extra footprint offsets to calculate out of lane collisions + extra_front_offset: 0.0 # [m] extra footprint front distance + extra_rear_offset: 0.0 # [m] extra footprint rear distance + extra_right_offset: 0.0 # [m] extra footprint right distance + extra_left_offset: 0.0 # [m] extra footprint left distance 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 index 8b03fa66eab55..b7cd55ce2e642 100644 --- 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 @@ -15,74 +15,117 @@ #include "calculate_slowdown_points.hpp" #include "footprint.hpp" +#include "types.hpp" #include #include +#include -#include +#include +#include +#include +#include + +#include #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) +std::optional calculate_last_avoiding_pose( + const std::vector & trajectory, + const universe_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid, + const double min_arc_length, const double max_arc_length, const double precision) { - // TODO(Maxime): use the library function - const auto dist_ahead_of_ego = autoware::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); + geometry_msgs::msg::Pose interpolated_pose{}; + bool is_avoiding_pose = false; + + auto from = min_arc_length; + auto to = max_arc_length; + while (to - from > precision) { + auto l = from + 0.5 * (to - from); + interpolated_pose = motion_utils::calcInterpolatedPose(trajectory, l); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); + is_avoiding_pose = + std::all_of(polygons_to_avoid.begin(), polygons_to_avoid.end(), [&](const auto & polygon) { + return boost::geometry::disjoint(interpolated_footprint, polygon); + }); + if (is_avoiding_pose) { + from = l; + } else { + to = l; + } + } + if (is_avoiding_pose) { + return interpolated_pose; + } + return std::nullopt; } -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, - const autoware::universe_utils::Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params) +std::optional calculate_pose_ahead_of_collision( + const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, + const universe_utils::Polygon2d & footprint, const double precision) { - const auto from_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); - const auto to_arc_length = autoware::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 = - autoware::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; + const auto first_avoid_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0UL, point_to_avoid.trajectory_index); + for (auto l = first_avoid_arc_length - precision; l >= ego_data.min_stop_arc_length; + l -= precision) { + const auto interpolated_pose = + motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); + if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) { + return interpolated_pose; + } } return std::nullopt; } -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional & prev_slowdown_point, PlannerParam params) +std::optional calculate_slowdown_point( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params) { + const auto point_to_avoid_it = std::find_if( + out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(), + [&](const auto & p) { return p.to_avoid; }); + if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) { + return std::nullopt; + } + const auto raw_footprint = make_base_footprint(params, true); // ignore extra footprint offsets + const auto base_footprint = make_base_footprint(params); params.extra_front_offset += params.lon_dist_buffer; params.extra_right_offset += params.lat_dist_buffer; params.extra_left_offset += params.lat_dist_buffer; - const auto base_footprint = make_base_footprint(params); + const auto expanded_footprint = make_base_footprint(params); // with added distance buffers + lanelet::BasicPolygons2d polygons_to_avoid; + for (const auto & ll : point_to_avoid_it->overlapped_lanelets) { + polygons_to_avoid.push_back(ll.polygon2d().basicPolygon()); + } + // points are ordered by trajectory index so the first one has the smallest index and arc length + const auto first_outside_idx = out_of_lane_data.outside_points.front().trajectory_index; + const auto first_outside_arc_length = + motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, first_outside_idx); + std::optional slowdown_point; // 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}; + // we first try to use the expanded footprint (distance buffers + extra footprint offsets) + for (const auto & footprint : {expanded_footprint, base_footprint, raw_footprint}) { + slowdown_point = calculate_last_avoiding_pose( + ego_data.trajectory_points, footprint, polygons_to_avoid, ego_data.min_stop_arc_length, + first_outside_arc_length, params.precision); + if (slowdown_point) { + break; + } } - return std::nullopt; + // fallback to simply stopping ahead of the collision to avoid (regardless of being out of lane or + // not) + if (!slowdown_point) { + slowdown_point = calculate_pose_ahead_of_collision( + ego_data, *point_to_avoid_it, expanded_footprint, params.precision); + } + return slowdown_point; } } // 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 index 145f21c94c0d0..394334d434558 100644 --- 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 @@ -19,40 +19,39 @@ #include +#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 +/// @brief calculate the last pose along the trajectory where ego does not go out of lane /// @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 +/// @param [in] min_arc_length minimum arc length for the search +/// @param [in] max_arc_length maximum arc length for the search +/// @param [in] precision [m] search precision /// @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 autoware::universe_utils::Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params); +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const autoware::universe_utils::Polygon2d & footprint, + const double min_arc_length, const double max_arc_length, const double precision); + +/// @brief calculate the slowdown pose just ahead of a point to avoid +/// @param ego_data ego data (trajectory, velocity, etc) +/// @param point_to_avoid the point to avoid +/// @param footprint the ego footprint +/// @param params parameters +/// @return optional slowdown point to insert in the trajectory +std::optional calculate_pose_ahead_of_collision( + const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, + const universe_utils::Polygon2d & footprint, const double precision); /// @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 out_of_lane_data data about out of lane areas /// @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); +std::optional calculate_slowdown_point( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, 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 index d9a85e266f790..cb29f9b3b42ea 100644 --- 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 @@ -16,12 +16,22 @@ #include "types.hpp" +#include +#include +#include #include +#include +#include #include +#include + +#include +#include +#include + #include -#include namespace autoware::motion_velocity_planner::out_of_lane::debug { @@ -36,209 +46,167 @@ visualization_msgs::msg::Marker get_base_marker() 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 = autoware::universe_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = autoware::universe_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + base_marker.pose.position = universe_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = universe_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); return base_marker; } -void add_footprint_markers( +void add_polygons_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) + const lanelet::BasicPolygons2d & polygons, const double z, const std::string & ns) { 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( - autoware::universe_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(); + debug_marker.ns = ns; + debug_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + for (const auto & f : polygons) { + boost::geometry::for_each_segment(f, [&](const auto & s) { + const auto & [p1, p2] = s; + debug_marker.points.push_back(universe_utils::createMarkerPosition(p1.x(), p1.y(), z + 0.5)); + debug_marker.points.push_back(universe_utils::createMarkerPosition(p2.x(), p2.y(), z + 0.5)); + }); } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); + 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) + const lanelet::BasicPolygon2d & current_footprint, const double z) { 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(autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z)); + debug_marker.points.push_back(universe_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 = autoware::universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); - else - debug_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker.color = universe_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( - autoware::universe_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( - autoware::universe_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) +void add_ttc_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const EgoData & ego_data, + const OutOfLaneData & out_of_lane_data, const size_t prev_nb) { auto debug_marker = get_base_marker(); - debug_marker.ns = "ranges"; - debug_marker.color = autoware::universe_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(autoware::universe_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - for (const auto & overlap : range.debug.overlaps) { - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + debug_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + debug_marker.scale.z = 0.5; + debug_marker.ns = "ttcs"; + for (const auto & p : out_of_lane_data.outside_points) { + if (p.to_avoid) { + debug_marker.color.r = 1.0; + debug_marker.color.g = 0.0; + } else { + debug_marker.color.r = 0.0; + debug_marker.color.g = 1.0; + } + if (p.ttc) { + debug_marker.pose = ego_data.trajectory_points[p.trajectory_index].pose; + debug_marker.text = std::to_string(*p.ttc); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; } - debug_marker.points.push_back(autoware::universe_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.action = visualization_msgs::msg::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, +size_t add_stop_line_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const StopLinesRtree & rtree, 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 = autoware::universe_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(autoware::universe_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.type = visualization_msgs::msg::Marker::LINE_STRIP; + debug_marker.ns = "stop_lines"; + const auto & add_lanelets_markers = [&](const auto & lanelets) { + for (const auto & ll : lanelets) { + debug_marker.points.clear(); + for (const auto & p : ll.polygon2d().basicPolygon()) { + debug_marker.points.push_back(universe_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_array.markers.push_back(debug_marker); + }; + for (const auto & [_, stop_line] : rtree) { 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.color.r = 1.0; + for (const auto & p : stop_line.stop_line) { + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); } - debug_marker.scale.z = 1.0; - debug_marker.text = ss.str(); debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; + ++debug_marker.id; + debug_marker.color.r = 0.0; + add_lanelets_markers(stop_line.lanelets); } - debug_marker.action = debug_marker.DELETE; + const auto max_id = debug_marker.id; + debug_marker.action = visualization_msgs::msg::Marker::DELETE; for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { debug_marker_array.markers.push_back(debug_marker); } + return max_id; } } // namespace -visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data) + +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) { - constexpr auto z = 0.0; + const auto z = ego_data.pose.position.z; 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", - autoware::universe_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", - autoware::universe_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", - autoware::universe_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); + // add_polygons_markers(debug_marker_array, ego_data.trajectory_footprints, z, "footprints"); + + lanelet::BasicPolygons2d drivable_lane_polygons; + for (const auto & poly : ego_data.drivable_lane_polygons) { + drivable_lane_polygons.push_back(poly.outer); + } + add_polygons_markers(debug_marker_array, drivable_lane_polygons, z, "ego_lane"); + + lanelet::BasicPolygons2d out_of_lane_areas; + for (const auto & p : out_of_lane_data.outside_points) { + out_of_lane_areas.push_back(p.outside_ring); + } + add_polygons_markers(debug_marker_array, out_of_lane_areas, z, "out_of_lane_areas"); + + lanelet::BasicPolygons2d object_polygons; + for (const auto & o : objects.objects) { + for (const auto & path : o.kinematics.predicted_paths) { + for (const auto & pose : path.path) { + // limit the draw distance to improve performance + if (universe_utils::calcDistance2d(pose, ego_data.pose) < 50.0) { + const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); + lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); + object_polygons.push_back(ll_poly); + } + } + } + } + add_polygons_markers(debug_marker_array, object_polygons, z, "objects"); + + add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z); + + add_ttc_markers(debug_marker_array, ego_data, out_of_lane_data, debug_data.prev_ttcs); + debug_data.prev_ttcs = out_of_lane_data.outside_points.size(); + + debug_data.prev_stop_line = add_stop_line_markers( + debug_marker_array, ego_data.stop_lines_rtree, z, debug_data.prev_stop_line); + return debug_marker_array; } -autoware::motion_utils::VirtualWalls create_virtual_walls( - const DebugData & debug_data, const PlannerParam & params) +motion_utils::VirtualWalls create_virtual_walls( + const geometry_msgs::msg::Pose & pose, const bool stop, const PlannerParam & params) { - autoware::motion_utils::VirtualWalls virtual_walls; - autoware::motion_utils::VirtualWall wall; + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; wall.text = "out_of_lane"; wall.longitudinal_offset = params.front_offset; - wall.style = autoware::motion_utils::VirtualWallType::slowdown; - for (const auto & slowdown : debug_data.slowdowns) { - wall.pose = slowdown.point.pose; - virtual_walls.push_back(wall); - } + wall.style = stop ? motion_utils::VirtualWallType::stop : motion_utils::VirtualWallType::slowdown; + wall.pose = 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 index ea225442443c8..dc1f942655612 100644 --- 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 @@ -23,9 +23,11 @@ namespace autoware::motion_velocity_planner::out_of_lane::debug { -visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); -autoware::motion_utils::VirtualWalls create_virtual_walls( - const DebugData & debug_data, const PlannerParam & params); +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data); +motion_utils::VirtualWalls create_virtual_walls( + const geometry_msgs::msg::Pose & pose, const bool stop, 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 deleted file mode 100644 index fc487a2626b88..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp +++ /dev/null @@ -1,389 +0,0 @@ -// 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 autoware::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_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 = - autoware::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 = - autoware::motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); - const auto enter_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, enter_segment_idx, enter_point); - const auto enter_lat_dist = std::abs(autoware::motion_utils::calcLateralOffset( - unique_path_points, enter_point, enter_segment_idx)); - const auto enter_segment_length = autoware::universe_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 = - autoware::motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); - const auto exit_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, exit_segment_idx, exit_point); - const auto exit_lat_dist = std::abs( - autoware::motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); - const auto exit_segment_length = autoware::universe_utils::calcDistance2d( - unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); - const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); - const auto exit_time = - static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; - - RCLCPP_DEBUG( - logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, - enter_time, exit_time); - // predicted path is too far from the overlapping range to be relevant - const auto is_far_from_entering_point = enter_lat_dist > max_deviation; - const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; - if (is_far_from_entering_point && is_far_from_exiting_point) { - RCLCPP_DEBUG( - logger, - " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", - is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, - max_deviation); - continue; - } - const auto is_last_predicted_path_point = - (exit_segment_idx + 2 == unique_path_points.size() || - enter_segment_idx + 2 == unique_path_points.size()); - const auto does_not_reach_overlap = - is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); - if (does_not_reach_overlap) { - RCLCPP_DEBUG( - logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", - std::min(exit_offset, enter_offset), max_lon_deviation); - continue; - } - - const auto same_driving_direction_as_ego = enter_time < exit_time; - if (same_driving_direction_as_ego) { - worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; - } else { - worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; - } - } - if (worst_enter_time && worst_exit_time) { - RCLCPP_DEBUG( - logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, - *worst_exit_time); - return std::make_pair(*worst_enter_time, *worst_exit_time); - } - RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); - return {}; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger) -{ - const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; - const auto object_point = lanelet::BasicPoint2d(p.x, p.y); - const auto half_size = object.shape.dimensions.x / 2.0; - lanelet::ConstLanelets object_lanelets; - for (const auto & ll : inputs.lanelets) - if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) - object_lanelets.push_back(ll); - - geometry_msgs::msg::Pose pose; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - const auto range_size = std::abs(range_enter_length - range_exit_length); - auto worst_enter_dist = std::optional(); - auto worst_exit_dist = std::optional(); - for (const auto & lane : object_lanelets) { - const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); - RCLCPP_DEBUG( - logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); - if (path) { - lanelet::ConstLanelets lls; - for (const auto & ll : *path) lls.push_back(ll); - pose.position.set__x(object_point.x()).set__y(object_point.y()); - const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - RCLCPP_DEBUG( - logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, - enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, - range.exiting_point.x(), range.exiting_point.y()); - const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; - if (already_entered_range) continue; - // multiple paths to the overlap -> be conservative and use the "worst" case - // "worst" = min/max arc length depending on if the lane is running opposite to the ego - // 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", - autoware::universe_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 deleted file mode 100644 index 455cee272f7be..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp +++ /dev/null @@ -1,116 +0,0 @@ -// 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_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger); -/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit -/// points of an overlapping range -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit. -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger); -/// @brief decide whether an object is coming in the range at the same time as ego -/// @details the condition depends on the mode (threshold, intervals, ttc) -/// @param [in] range_times times when ego and the object enter/exit the range -/// @param [in] params parameters -/// @param [in] logger ros logger -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); -/// @brief check whether we should avoid entering the given range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return true if we should avoid entering the range -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief set the velocity of a decision (or unset it) based on the distance away from the range -/// @param [out] decision decision to update (either set its velocity or unset the decision) -/// @param [in] distance distance between ego and the range corresponding to the decision -/// @param [in] params parameters -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params); -/// @brief calculate the decision to slowdown or stop before an overlapping range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return an optional decision to slowdown or stop -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief calculate decisions to slowdown or stop before some overlapping ranges -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return the calculated decisions to slowdown or stop -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -} // namespace autoware::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 index a53129f372e3d..79bdefee4c4c7 100644 --- 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 @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -53,7 +54,7 @@ void cut_predicted_path_beyond_line( auto cut_idx = stop_line_idx; double arc_length = 0; while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += autoware::universe_utils::calcDistance2d( + arc_length += universe_utils::calcDistance2d( predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); --cut_idx; } @@ -102,12 +103,11 @@ void cut_predicted_path_beyond_red_lights( } autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const std::shared_ptr planner_data, const EgoData & ego_data, - const PlannerParam & params) + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data->predicted_objects.header; - for (const auto & object : planner_data->predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects.header; + for (const auto & object : planner_data.predicted_objects.objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; @@ -125,10 +125,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( 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 = autoware::motion_utils::removeOverlapPoints(predicted_path.path); + 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( - autoware::motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + 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( @@ -136,23 +136,21 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( 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, ego_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()); - } + 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, ego_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()) + if (!filtered_object.kinematics.predicted_paths.empty()) filtered_objects.objects.push_back(filtered_object); } return filtered_objects; 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 index baf07e4b06ea5..3b2ae11718810 100644 --- 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 @@ -19,7 +19,6 @@ #include -#include #include namespace autoware::motion_velocity_planner::out_of_lane @@ -52,8 +51,7 @@ void cut_predicted_path_beyond_red_lights( /// @param [in] params parameters /// @return filtered predicted objects autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const std::shared_ptr planner_data, const EgoData & ego_data, - const PlannerParam & params); + const PlannerData & 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 index f5d68c4fa9bba..564bf5de7ef2e 100644 --- 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 @@ -21,15 +21,13 @@ #include #include -#include #include namespace autoware::motion_velocity_planner::out_of_lane { -autoware::universe_utils::Polygon2d make_base_footprint( - const PlannerParam & p, const bool ignore_offset) +universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool ignore_offset) { - autoware::universe_utils::Polygon2d base_footprint; + universe_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; @@ -43,10 +41,10 @@ autoware::universe_utils::Polygon2d make_base_footprint( } lanelet::BasicPolygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) + const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_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); @@ -59,22 +57,15 @@ std::vector calculate_trajectory_footprints( 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) { + for (auto i = ego_data.first_trajectory_idx; i < ego_data.trajectory_points.size(); ++i) { const auto & trajectory_pose = ego_data.trajectory_points[i].pose; const auto angle = tf2::getYaw(trajectory_pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_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 += autoware::universe_utils::calcDistance2d( - trajectory_pose, ego_data.trajectory_points[i + 1].pose); } return trajectory_footprints; } @@ -84,7 +75,7 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( { const auto base_footprint = make_base_footprint(params, ignore_offset); const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_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); 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 index ebe7ab0fa9d7f..1cb2092586bf0 100644 --- 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 @@ -30,15 +30,14 @@ namespace out_of_lane /// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the /// footprint /// @return base ego footprint -autoware::universe_utils::Polygon2d make_base_footprint( +universe_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 autoware::universe_utils::Polygon2d & base_footprint, - const geometry_msgs::msg::Pose & pose); + const universe_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 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 index bca849f806f63..f520a564519f0 100644 --- 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 @@ -14,36 +14,46 @@ #include "lanelets_selection.hpp" +#include "types.hpp" + #include #include +#include +#include #include -#include #include #include -#include namespace autoware::motion_velocity_planner::out_of_lane { +namespace +{ +bool is_road_lanelet(const lanelet::ConstLanelet & lanelet) +{ + return lanelet.hasAttribute(lanelet::AttributeName::Subtype) && + lanelet.attribute(lanelet::AttributeName::Subtype) == lanelet::AttributeValueString::Road; +} +} // namespace + lanelet::ConstLanelets consecutive_lanelets( - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lanelet) + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) { - lanelet::ConstLanelets consecutives = route_handler->getRoutingGraphPtr()->following(lanelet); - const auto previous = route_handler->getRoutingGraphPtr()->previous(lanelet); + 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) + const lanelet::ConstLanelets & trajectory_lanelets, + const route_handler::RouteHandler & route_handler) { lanelet::ConstLanelets missing_lane_change_lanelets; - const auto & routing_graph = *route_handler->getRoutingGraphPtr(); + const auto & routing_graph = *route_handler.getRoutingGraphPtr(); lanelet::ConstLanelets adjacents; lanelet::ConstLanelets consecutives; for (const auto & ll : trajectory_lanelets) { @@ -67,9 +77,9 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const std::shared_ptr route_handler) + const EgoData & ego_data, const route_handler::RouteHandler & route_handler) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); lanelet::ConstLanelets trajectory_lanelets; lanelet::BasicLineString2d trajectory_ls; for (const auto & p : ego_data.trajectory_points) @@ -77,7 +87,9 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( const auto candidates = lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls)); for (const auto & ll : candidates) { - if (!boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) { + if ( + is_road_lanelet(ll) && + !boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) { trajectory_lanelets.push_back(ll); } } @@ -89,43 +101,65 @@ lanelet::ConstLanelets calculate_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) + const lanelet::ConstLanelets & trajectory_lanelets, + const route_handler::RouteHandler & route_handler) { lanelet::ConstLanelets ignored_lanelets; - // ignore lanelets directly behind ego - const auto behind = - autoware::universe_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); + // ignore lanelets directly preceding a trajectory lanelet + for (const auto & trajectory_lanelet : trajectory_lanelets) { + for (const auto & ll : route_handler.getPreviousLanelets(trajectory_lanelet)) { + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.id()); + if (!is_trajectory_lanelet) ignored_lanelets.push_back(ll); + } } 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) +void calculate_drivable_lane_polygons( + EgoData & ego_data, const route_handler::RouteHandler & route_handler) +{ + const auto route_lanelets = calculate_trajectory_lanelets(ego_data, route_handler); + const auto ignored_lanelets = + out_of_lane::calculate_ignored_lanelets(route_lanelets, route_handler); + for (const auto & ll : route_lanelets) { + out_of_lane::Polygons tmp; + boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); + ego_data.drivable_lane_polygons = tmp; + } + for (const auto & ll : ignored_lanelets) { + out_of_lane::Polygons tmp; + boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); + ego_data.drivable_lane_polygons = tmp; + } +} + +void calculate_overlapped_lanelets( + OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler) { - 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); + out_of_lane_point.overlapped_lanelets = lanelet::ConstLanelets(); + const auto candidates = route_handler.getLaneletMapPtr()->laneletLayer.search( + lanelet::geometry::boundingBox2d(out_of_lane_point.outside_ring)); + for (const auto & ll : candidates) { + if ( + is_road_lanelet(ll) && !contains_lanelet(out_of_lane_point.overlapped_lanelets, ll.id()) && + boost::geometry::within(out_of_lane_point.outside_ring, ll.polygon2d().basicPolygon())) { + out_of_lane_point.overlapped_lanelets.push_back(ll); + } + } +} + +void calculate_overlapped_lanelets( + OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler) +{ + for (auto it = out_of_lane_data.outside_points.begin(); + it != out_of_lane_data.outside_points.end();) { + calculate_overlapped_lanelets(*it, route_handler); + if (it->overlapped_lanelets.empty()) { + // do not keep out of lane points that do not overlap any lanelet + out_of_lane_data.outside_points.erase(it); + } else { + ++it; + } } - 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 index 8618d808d7d40..a7729deb539b6 100644 --- 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 @@ -44,6 +44,7 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @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 @@ -51,30 +52,28 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( /// @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); + const 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] trajectory_lanelets lanelets followed 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); + const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr & route_handler); + +/// @brief calculate the polygons representing the ego lane and add it to the ego data +/// @param [inout] ego_data ego data +/// @param [in] route_handler route handler with map information +void calculate_drivable_lane_polygons( + EgoData & ego_data, const route_handler::RouteHandler & route_handler); + +/// @brief calculate the lanelets overlapped at each out of lane point +/// @param [out] out_of_lane_data data with the out of lane points +/// @param [in] route_handler route handler with the lanelet map +void calculate_overlapped_lanelets( + OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler); } // 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_collisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp new file mode 100644 index 0000000000000..c375bcc35c1ee --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -0,0 +1,166 @@ +// 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 "out_of_lane_collisions.hpp" + +#include "types.hpp" + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +void update_collision_times( + OutOfLaneData & out_of_lane_data, const std::unordered_set & potential_collision_indexes, + const universe_utils::Polygon2d & object_footprint, const double time) +{ + for (const auto index : potential_collision_indexes) { + auto & out_of_lane_point = out_of_lane_data.outside_points[index]; + if (out_of_lane_point.collision_times.count(time) == 0UL) { + const auto has_collision = + !boost::geometry::disjoint(out_of_lane_point.outside_ring, object_footprint.outer()); + if (has_collision) { + out_of_lane_point.collision_times.insert(time); + } + } + } +} + +void calculate_object_path_time_collisions( + OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedPath & object_path, + const autoware_perception_msgs::msg::Shape & object_shape) +{ + const auto time_step = rclcpp::Duration(object_path.time_step).seconds(); + auto time = time_step; + for (const auto & object_pose : object_path.path) { + time += time_step; + const auto object_footprint = universe_utils::toPolygon2d(object_pose, object_shape); + std::vector query_results; + out_of_lane_data.outside_areas_rtree.query( + boost::geometry::index::intersects(object_footprint.outer()), + std::back_inserter(query_results)); + std::unordered_set potential_collision_indexes; + for (const auto & [_, index] : query_results) { + potential_collision_indexes.insert(index); + } + update_collision_times(out_of_lane_data, potential_collision_indexes, object_footprint, time); + } +} + +void calculate_objects_time_collisions( + OutOfLaneData & out_of_lane_data, + const std::vector & objects) +{ + for (const auto & object : objects) { + for (const auto & path : object.kinematics.predicted_paths) { + calculate_object_path_time_collisions(out_of_lane_data, path, object.shape); + } + } +} + +void calculate_min_max_arrival_times( + OutOfLanePoint & out_of_lane_point, + const std::vector & trajectory) +{ + auto min_time = std::numeric_limits::infinity(); + auto max_time = -std::numeric_limits::infinity(); + for (const auto & t : out_of_lane_point.collision_times) { + min_time = std::min(t, min_time); + max_time = std::max(t, max_time); + } + if (min_time <= max_time) { + out_of_lane_point.min_object_arrival_time = min_time; + out_of_lane_point.max_object_arrival_time = max_time; + const auto & ego_time = + rclcpp::Duration(trajectory[out_of_lane_point.trajectory_index].time_from_start).seconds(); + if (ego_time >= min_time && ego_time <= max_time) { + out_of_lane_point.ttc = 0.0; + } else { + out_of_lane_point.ttc = + std::min(std::abs(ego_time - min_time), std::abs(ego_time - max_time)); + } + } +}; + +void calculate_collisions_to_avoid( + OutOfLaneData & out_of_lane_data, + const std::vector & trajectory, + const PlannerParam & params) +{ + for (auto & out_of_lane_point : out_of_lane_data.outside_points) { + calculate_min_max_arrival_times(out_of_lane_point, trajectory); + } + for (auto & p : out_of_lane_data.outside_points) { + if (params.mode == "ttc") { + p.to_avoid = p.ttc && p.ttc <= params.ttc_threshold; + } else { + p.to_avoid = p.min_object_arrival_time && p.min_object_arrival_time <= params.time_threshold; + } + } +} + +OutOfLaneData calculate_outside_points(const EgoData & ego_data) +{ + OutOfLaneData out_of_lane_data; + out_of_lane::OutOfLanePoint p; + for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { + p.trajectory_index = i + ego_data.first_trajectory_idx; + const auto & footprint = ego_data.trajectory_footprints[i]; + out_of_lane::Polygons out_of_lane_polygons; + boost::geometry::difference(footprint, ego_data.drivable_lane_polygons, out_of_lane_polygons); + for (const auto & area : out_of_lane_polygons) { + if (!area.outer.empty()) { + p.outside_ring = area.outer; + out_of_lane_data.outside_points.push_back(p); + } + } + } + return out_of_lane_data; +} + +OutOfLaneData calculate_out_of_lane_areas(const EgoData & ego_data) +{ + auto out_of_lane_data = calculate_outside_points(ego_data); + std::vector rtree_nodes; + for (auto i = 0UL; i < out_of_lane_data.outside_points.size(); ++i) { + OutAreaNode n; + n.first = boost::geometry::return_envelope( + out_of_lane_data.outside_points[i].outside_ring); + n.second = i; + rtree_nodes.push_back(n); + } + out_of_lane_data.outside_areas_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; + return out_of_lane_data; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp new file mode 100644 index 0000000000000..b9048cc358a07 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -0,0 +1,55 @@ +// 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_COLLISIONS_HPP_ +#define OUT_OF_LANE_COLLISIONS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief calculate the times and points where ego collides with an object's path outside of its +/// lane +void calculate_object_path_time_collisions( + OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedPath & object_path, + const autoware_perception_msgs::msg::Shape & object_shape); + +/// @brief calculate the times and points where ego collides with an object outside of its lane +void calculate_objects_time_collisions( + OutOfLaneData & out_of_lane_data, + const std::vector & objects); + +/// @brief calculate the collisions to avoid +/// @details either uses the time to collision or just the time when the object will arrive at the +/// point +void calculate_collisions_to_avoid( + OutOfLaneData & out_of_lane_data, + const std::vector & trajectory, + const PlannerParam & params); + +/// @brief calculate the areas where ego will drive outside of its lane +/// @details the OutOfLaneData points and rtree are filled +OutOfLaneData calculate_out_of_lane_areas(const EgoData & ego_data); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // OUT_OF_LANE_COLLISIONS_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 index a6b34a1566e19..ca7ed3b9dc7bd 100644 --- 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 @@ -16,11 +16,10 @@ #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 "out_of_lane_collisions.hpp" #include "types.hpp" #include @@ -34,11 +33,13 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -56,65 +57,54 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + 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); - processing_diag_publisher_ = std::make_shared( + processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); processing_time_publisher_ = node.create_publisher( "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { - using autoware::universe_utils::getOrDeclareParameter; + using universe_utils::getOrDeclareParameter; auto & pp = params_; pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); pp.skip_if_already_overlapping = getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); + pp.ignore_lane_changeable_lanelets = + getOrDeclareParameter(node, ns_ + ".ignore_overlaps_over_lane_changeable_lanelets"); + pp.max_arc_length = getOrDeclareParameter(node, ns_ + ".max_arc_length"); 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.objects_ignore_behind_ego = getOrDeclareParameter(node, ns_ + ".objects.ignore_behind_ego"); - 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.lon_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.longitudinal_distance_buffer"); pp.lat_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.lateral_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 = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(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; @@ -123,44 +113,82 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) void OutOfLaneModule::update_parameters(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using universe_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_ + ".max_arc_length", pp.max_arc_length); + updateParam( + parameters, ns_ + ".ignore_overlaps_over_lane_changeable_lanelets", + pp.ignore_lane_changeable_lanelets); 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_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego); - 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.longitudinal_distance_buffer", pp.lon_dist_buffer); updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_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); } +void OutOfLaneModule::limit_trajectory_size( + out_of_lane::EgoData & ego_data, + const std::vector & ego_trajectory_points, + const double max_arc_length) +{ + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_trajectory_index = + motion_utils::calcLongitudinalOffsetToSegment( + ego_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position); + auto l = -ego_data.longitudinal_offset_to_first_trajectory_index; + ego_data.trajectory_points.push_back(ego_trajectory_points[ego_data.first_trajectory_idx]); + for (auto i = ego_data.first_trajectory_idx + 1; i < ego_trajectory_points.size(); ++i) { + l += universe_utils::calcDistance2d(ego_trajectory_points[i - 1], ego_trajectory_points[i]); + if (l >= max_arc_length) { + break; + } + ego_data.trajectory_points.push_back(ego_trajectory_points[i]); + } +} + +void OutOfLaneModule::calculate_min_stop_and_slowdown_distances( + out_of_lane::EgoData & ego_data, const PlannerData & planner_data, + std::optional & previous_slowdown_pose_, const double slow_velocity) +{ + ego_data.min_stop_distance = planner_data.calculate_min_deceleration_distance(0.0).value_or(0.0); + ego_data.min_slowdown_distance = + planner_data.calculate_min_deceleration_distance(slow_velocity).value_or(0.0); + if (previous_slowdown_pose_) { + // Ensure we do not remove the previous slowdown point due to the min distance limit + const auto previous_slowdown_pose_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.first_trajectory_idx, previous_slowdown_pose_->position); + ego_data.min_stop_distance = + std::min(previous_slowdown_pose_arc_length, ego_data.min_stop_distance); + ego_data.min_slowdown_distance = + std::min(previous_slowdown_pose_arc_length, ego_data.min_slowdown_distance); + } + ego_data.min_stop_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0UL, ego_data.first_trajectory_idx) + + ego_data.longitudinal_offset_to_first_trajectory_index + + ego_data.min_stop_distance; +} + void prepare_stop_lines_rtree( out_of_lane::EgoData & ego_data, const PlannerData & planner_data, const double search_distance) { @@ -202,163 +230,140 @@ VelocityPlanningResult OutOfLaneModule::plan( const std::shared_ptr planner_data) { VelocityPlanningResult result; - autoware::universe_utils::StopWatch stopwatch; + universe_utils::StopWatch stopwatch; stopwatch.tic(); + + stopwatch.tic("preprocessing"); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - ego_data.trajectory_points = ego_trajectory_points; - ego_data.first_trajectory_idx = - autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); - ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x; - ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); - stopwatch.tic("preprocessing"); - prepare_stop_lines_rtree(ego_data, *planner_data, 100.0); + limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length); + calculate_min_stop_and_slowdown_distances( + ego_data, *planner_data, previous_slowdown_pose_, params_.slow_velocity); + prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length); const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("calculate_trajectory_footprints"); - const auto current_ego_footprint = + ego_data.current_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_); + ego_data.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_); + out_of_lane::calculate_drivable_lane_polygons(ego_data, *planner_data->route_handler); 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("calculate_out_of_lane_areas"); + auto out_of_lane_data = calculate_out_of_lane_areas(ego_data); + out_of_lane::calculate_overlapped_lanelets(out_of_lane_data, *planner_data->route_handler); + const auto calculate_out_of_lane_areas_us = stopwatch.toc("calculate_out_of_lane_areas"); + stopwatch.tic("filter_predicted_objects"); - inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); + const auto objects = out_of_lane::filter_predicted_objects(*planner_data, ego_data, params_); const auto filter_predicted_objects_us = 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"); + + stopwatch.tic("calculate_time_collisions"); + out_of_lane::calculate_objects_time_collisions(out_of_lane_data, objects.objects); + const auto calculate_time_collisions_us = stopwatch.toc("calculate_time_collisions"); + + stopwatch.tic("calculate_times"); + // calculate times + out_of_lane::calculate_collisions_to_avoid(out_of_lane_data, ego_data.trajectory_points, params_); + const auto calculate_times_us = stopwatch.toc("calculate_times"); + + if ( + params_.skip_if_already_overlapping && !ego_data.drivable_lane_polygons.empty() && + !lanelet::geometry::within(ego_data.current_footprint, ego_data.drivable_lane_polygons)) { + RCLCPP_WARN(logger_, "Ego is already out of lane, skipping the module\n"); + debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array( + ego_data, out_of_lane_data, objects, debug_data_)); + return result; + } + 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 + previous_slowdown_pose_ && + (clock_->now() - previous_slowdown_time_).seconds() > params_.min_decision_duration) { + previous_slowdown_pose_.reset(); + } + + stopwatch.tic("calculate_slowdown_point"); + auto slowdown_pose = out_of_lane::calculate_slowdown_point(ego_data, out_of_lane_data, params_); + const auto calculate_slowdown_point_us = stopwatch.toc("calculate_slowdown_point"); + + if ( // reset the timer if there is no previous inserted point + slowdown_pose && (!previous_slowdown_pose_)) { + previous_slowdown_time_ = clock_->now(); + } + // reuse previous stop pose 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 = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, point_to_insert->point.pose.position); - const auto prev_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + const auto should_use_previous_pose = [&]() { + if (slowdown_pose && previous_slowdown_pose_) { + const auto arc_length = + motion_utils::calcSignedArcLength(ego_trajectory_points, 0LU, slowdown_pose->position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, previous_slowdown_pose_->position); return prev_arc_length < arc_length; } - return !point_to_insert && prev_inserted_point_; + return !slowdown_pose && previous_slowdown_pose_; }(); - if (should_use_prev_inserted_point) { + if (should_use_previous_pose) { // if the trajectory changed the prev point is no longer on the trajectory so we project it - const auto insert_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); - prev_inserted_point_->point.pose = - autoware::motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); - point_to_insert = prev_inserted_point_; + const auto new_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.first_trajectory_idx, previous_slowdown_pose_->position); + slowdown_pose = motion_utils::calcInterpolatedPose(ego_trajectory_points, new_arc_length); } - 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 + if (slowdown_pose) { + const auto arc_length = + motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.first_trajectory_idx, slowdown_pose->position) - + ego_data.longitudinal_offset_to_first_trajectory_index; + const auto slowdown_velocity = + arc_length <= params_.stop_dist_threshold ? 0.0 : params_.slow_velocity; + previous_slowdown_pose_ = slowdown_pose; + if (slowdown_velocity == 0.0) { + result.stop_points.push_back(slowdown_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 = autoware::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 ? autoware::motion_utils::VelocityFactor::APPROACHING - : autoware::motion_utils::VelocityFactor::STOPPED; + slowdown_pose->position, slowdown_pose->position, slowdown_velocity); + } + + const auto is_approaching = + motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 && + planner_data->current_odometry.twist.twist.linear.x > 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"); + ego_trajectory_points, ego_data.pose, *slowdown_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)"); + virtual_wall_marker_creator.add_virtual_walls( + out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + } else if (std::any_of( + out_of_lane_data.outside_points.begin(), out_of_lane_data.outside_points.end(), + [](const auto & p) { return p.to_avoid; })) { + RCLCPP_WARN( + logger_, "[out_of_lane] Could not insert slowdown point because of deceleration limits"); } - const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); - debug_data_.ranges = inputs.ranges; + stopwatch.tic("gen_debug"); + const auto markers = + out_of_lane::debug::create_debug_marker_array(ego_data, out_of_lane_data, objects, debug_data_); + const auto markers_us = stopwatch.toc("gen_debug"); + stopwatch.tic("pub"); + debug_publisher_->publish(markers); + const auto pub_markers_us = stopwatch.toc("pub"); const auto total_time_us = stopwatch.toc(); - RCLCPP_DEBUG( - logger_, - "Total time = %2.2fus\n" - "\tpreprocessing = %2.0fus\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", - preprocessing_us, total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, - calculate_overlapping_ranges_us, filter_predicted_objects_us, 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())); std::map processing_times; processing_times["preprocessing"] = preprocessing_us / 1000; processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000; processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000; - processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000; + processing_times["calculate_out_of_lane_areas"] = calculate_out_of_lane_areas_us / 1000; processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000; - processing_times["calculate_decision"] = calculate_decisions_us / 1000; - processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000; - processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000; + processing_times["calculate_time_collisions"] = calculate_time_collisions_us / 1000; + processing_times["calculate_times"] = calculate_times_us / 1000; + processing_times["calculate_slowdown_point"] = calculate_slowdown_point_us / 1000; + processing_times["generate_markers"] = markers_us / 1000; + processing_times["publish_markers"] = pub_markers_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); tier4_debug_msgs::msg::Float64Stamped processing_time_msg; 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 index 310a73c04d643..5225a6dd4abd9 100644 --- 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 @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -46,13 +47,24 @@ class OutOfLaneModule : public PluginModuleInterface private: void init_parameters(rclcpp::Node & node); + /// @brief resize the trajectory to start from the segment closest to ego and to have at most the + /// given length + static void limit_trajectory_size( + out_of_lane::EgoData & ego_data, + const std::vector & ego_trajectory_points, + const double max_arc_length); + /// @brief calculate the minimum stop and slowdown distances of ego + static void calculate_min_stop_and_slowdown_distances( + out_of_lane::EgoData & ego_data, const PlannerData & planner_data, + std::optional & previous_slowdown_pose_, const double slow_velocity); + 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_{}; + rclcpp::Clock::SharedPtr clock_; + std::optional previous_slowdown_pose_; + rclcpp::Time previous_slowdown_time_; protected: // Debug 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 deleted file mode 100644 index a1a722bb07f14..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// 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 deleted file mode 100644 index bc5872db16e03..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// 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 index 96c54064e296c..8067d9e8b3410 100644 --- 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 @@ -28,52 +28,46 @@ #include #include +#include -#include -#include -#include #include +#include #include #include #include namespace autoware::motion_velocity_planner::out_of_lane { -using autoware_planning_msgs::msg::TrajectoryPoint; +using Polygons = boost::geometry::model::multi_polygon; -/// @brief parameters for the "out of lane" module +/// @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 max_arc_length; // [m] maximum arc length along the trajectory to check for collision + bool ignore_lane_changeable_lanelets; // if true, ignore overlaps on lane changeable lanelets - 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 time_threshold; // [s](mode="threshold") objects time threshold 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 objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path bool objects_ignore_behind_ego; // if true, objects behind the ego vehicle are ignored - 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 lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle - double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle - 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 + // action to insert in the trajectory if an object causes a collision at an overlap + double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle + double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle + double slow_velocity; // [m/s] slowdown velocity + double stop_dist_threshold; // [m] if a collision is detected bellow this distance ahead of ego, + // try to insert a stop point + double precision; // [m] precision when inserting a stop pose in the trajectory + double + min_decision_duration; // [s] duration needed before a stop or slowdown point can be removed + // 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) @@ -85,98 +79,6 @@ struct PlannerParam 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_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; - } -}; - namespace bgi = boost::geometry::index; struct StopLine { @@ -185,68 +87,52 @@ struct StopLine }; using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; -using OutAreaRtree = bgi::rtree, bgi::rstar<16>>; +using OutAreaNode = std::pair; +using OutAreaRtree = bgi::rtree>; /// @brief data related to the ego vehicle struct EgoData { - std::vector trajectory_points{}; + std::vector trajectory_points; + geometry_msgs::msg::Pose pose; size_t first_trajectory_idx{}; - double velocity{}; // [m/s] - double max_decel{}; // [m/s²] - geometry_msgs::msg::Pose pose{}; + double longitudinal_offset_to_first_trajectory_index{}; + double min_stop_distance{}; + double min_slowdown_distance{}; + double min_stop_arc_length{}; + + Polygons drivable_lane_polygons; + + lanelet::BasicPolygon2d current_footprint; + std::vector trajectory_footprints; + StopLinesRtree stop_lines_rtree; }; -/// @brief data needed to make decisions -struct DecisionInputs +struct OutOfLanePoint { - OverlapRanges ranges; - EgoData ego_data; - autoware_perception_msgs::msg::PredictedObjects objects; - std::shared_ptr route_handler; - lanelet::ConstLanelets lanelets; + size_t trajectory_index; + lanelet::BasicPolygon2d outside_ring; + std::set collision_times; + std::optional min_object_arrival_time; + std::optional max_object_arrival_time; + std::optional ttc; + lanelet::ConstLanelets overlapped_lanelets; + bool to_avoid = false; +}; +struct OutOfLaneData +{ + std::vector outside_points; + OutAreaRtree outside_areas_rtree; }; /// @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(); - } + size_t prev_out_of_lane_areas = 0; + size_t prev_ttcs = 0; + size_t prev_objects = 0; + size_t prev_stop_line = 0; }; } // namespace autoware::motion_velocity_planner::out_of_lane 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 index f1eb7ff047fdc..b63e1e0d51f00 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt @@ -5,9 +5,21 @@ find_package(autoware_cmake REQUIRED) autoware_package() -# ament_auto_add_library(${PROJECT_NAME}_lib SHARED -# DIRECTORY src -# ) +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_collision_checker.cpp + test/test_planner_data.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 include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp new file mode 100644 index 0000000000000..bf471c62af969 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp @@ -0,0 +1,69 @@ +// 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__COLLISION_CHECKER_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +namespace bgi = boost::geometry::index; +using RtreeNode = std::pair; +using Rtree = bgi::rtree>; + +/// @brief collision as a trajectory index and corresponding collision points +struct Collision +{ + size_t trajectory_index{}; + autoware::universe_utils::MultiPoint2d collision_points; +}; + +/// @brief collision checker for a trajectory as a sequence of 2D footprints +class CollisionChecker +{ + const autoware::universe_utils::MultiPolygon2d trajectory_footprints_; + std::shared_ptr rtree_; + +public: + /// @brief construct the collisions checker + /// @param trajectory_footprints footprints of the trajectory to be used for collision checking + /// @details internally, the rtree is built with the packing algorithm + explicit CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints); + + /// @brief efficiently calculate collisions with a geometry + /// @tparam Geometry boost geometry type + /// @param geometry geometry to check for collisions + /// @return collisions between the trajectory footprints and the input geometry + template + [[nodiscard]] std::vector get_collisions(const Geometry & geometry) const; + + /// @brief direct access to the rtree storing the polygon footprints + /// @return rtree of the polygon footprints + [[nodiscard]] std::shared_ptr get_rtree() const { return rtree_; } + + /// @brief get the size of the trajectory used by this collision checker + [[nodiscard]] size_t trajectory_size() const { return trajectory_footprints_.size(); } +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ 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 index 5dbb872d99ca9..9c6c439b47f7b 100644 --- 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 @@ -15,7 +15,10 @@ #ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#include +#include #include +#include #include #include @@ -37,8 +40,6 @@ #include #include -#include -#include #include #include #include @@ -59,11 +60,11 @@ struct PlannerData } // msgs from callbacks that are used for data-ready - nav_msgs::msg::Odometry current_odometry{}; - geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration{}; - autoware_perception_msgs::msg::PredictedObjects predicted_objects{}; - pcl::PointCloud no_ground_pointcloud{}; - nav_msgs::msg::OccupancyGrid occupancy_grid{}; + nav_msgs::msg::Odometry current_odometry; + geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; + autoware_perception_msgs::msg::PredictedObjects predicted_objects; + pcl::PointCloud no_ground_pointcloud; + nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; // nearest search @@ -79,7 +80,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_; // parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; @@ -88,7 +89,7 @@ struct PlannerData *@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( + [[nodiscard]] std::optional get_traffic_signal( const lanelet::Id id, const bool keep_last_observation = false) const { const auto & traffic_light_id_map = @@ -98,6 +99,15 @@ struct PlannerData } return std::make_optional(traffic_light_id_map.at(id)); } + + [[nodiscard]] std::optional calculate_min_deceleration_distance( + const double target_velocity) const + { + return motion_utils::calcDecelDistWithJerkAndAccConstraints( + current_odometry.twist.twist.linear.x, target_velocity, + current_acceleration.accel.accel.linear.x, velocity_smoother_->getMinDecel(), + std::abs(velocity_smoother_->getMinJerk()), velocity_smoother_->getMinJerk()); + } }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp new file mode 100644 index 0000000000000..6e84b63a3c7fc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp @@ -0,0 +1,70 @@ +// 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. + +#include "autoware/motion_velocity_planner_common/collision_checker.hpp" + +namespace autoware::motion_velocity_planner +{ +CollisionChecker::CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints) +: trajectory_footprints_(std::move(trajectory_footprints)) +{ + std::vector nodes; + nodes.reserve(trajectory_footprints_.size()); + for (auto i = 0UL; i < trajectory_footprints_.size(); ++i) { + nodes.emplace_back( + boost::geometry::return_envelope(trajectory_footprints_[i]), + i); + } + rtree_ = std::make_shared(nodes.begin(), nodes.end()); +} + +template +std::vector CollisionChecker::get_collisions(const Geometry & geometry) const +{ + std::vector collisions; + std::vector approximate_results; + autoware::universe_utils::MultiPoint2d intersections; + ; + rtree_->query(bgi::intersects(geometry), std::back_inserter(approximate_results)); + for (const auto & result : approximate_results) { + intersections.clear(); + boost::geometry::intersection(trajectory_footprints_[result.second], geometry, intersections); + if (!intersections.empty()) { + Collision c; + c.trajectory_index = result.second; + c.collision_points = intersections; + collisions.push_back(c); + } + } + return collisions; +} + +template std::vector CollisionChecker::get_collisions( + const autoware::universe_utils::Point2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware::universe_utils::Line2d & geometry) const; +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiPolygon2d & geometry) const; + +// @warn Multi geometries usually lead to very inefficient queries +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiPoint2d & geometry) const; +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiLineString2d & geometry) const; +template std::vector CollisionChecker::get_collisions< + autoware::universe_utils::Polygon2d>(const autoware::universe_utils::Polygon2d & geometry) const; +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp new file mode 100644 index 0000000000000..df813db336a9d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp @@ -0,0 +1,176 @@ +// 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 "autoware/motion_velocity_planner_common/collision_checker.hpp" + +#include + +#include + +#include +#include + +using autoware::motion_velocity_planner::CollisionChecker; +using autoware::universe_utils::Line2d; +using autoware::universe_utils::MultiLineString2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +Point2d random_point() +{ + static std::random_device r; + static std::default_random_engine e1(r()); + static std::uniform_real_distribution uniform_dist(-100, 100); + return {uniform_dist(e1), uniform_dist(e1)}; +} + +Line2d random_line() +{ + const auto point = random_point(); + const auto point2 = Point2d{point.x() + 1, point.y() + 1}; + const auto point3 = Point2d{point2.x() - 1, point2.y() + 1}; + const auto point4 = Point2d{point3.x() + 1, point3.y() + 1}; + return {point, point2, point3, point4}; +} + +Polygon2d random_polygon() +{ + Polygon2d polygon; + const auto point = random_point(); + const auto point2 = Point2d{point.x() + 1, point.y() + 4}; + const auto point3 = Point2d{point.x() + 4, point.y() + 4}; + const auto point4 = Point2d{point.x() + 3, point.y() + 1}; + polygon.outer() = {point, point2, point3, point4, point}; + return polygon; +} + +bool all_within(const MultiPoint2d & pts1, const MultiPoint2d & pts2) +{ + // results from the collision checker and the direct checks can have some small precision errors + constexpr auto eps = 1e-3; + for (const auto & p1 : pts1) { + bool found = false; + for (const auto & p2 : pts2) { + if (boost::geometry::comparable_distance(p1, p2) < eps) { + found = true; + break; + } + } + if (!found) return false; + } + return true; +} + +TEST(TestCollisionChecker, Benchmark) +{ + constexpr auto nb_ego_footprints = 1000; + constexpr auto nb_obstacles = 1000; + MultiPolygon2d ego_footprints; + ego_footprints.reserve(nb_ego_footprints); + for (auto i = 0; i < nb_ego_footprints; ++i) { + ego_footprints.push_back(random_polygon()); + } + const auto cc_constr_start = std::chrono::system_clock::now(); + CollisionChecker collision_checker(ego_footprints); + const auto cc_constr_end = std::chrono::system_clock::now(); + const auto cc_constr_ns = + std::chrono::duration_cast(cc_constr_end - cc_constr_start).count(); + std::printf( + "Collision checker construction (with %d footprints): %ld ns\n", nb_ego_footprints, + cc_constr_ns); + MultiPolygon2d poly_obstacles; + MultiPoint2d point_obstacles; + MultiLineString2d line_obstacles; + for (auto i = 0; i < nb_obstacles; ++i) { + poly_obstacles.push_back(random_polygon()); + point_obstacles.push_back(random_point()); + line_obstacles.push_back(random_line()); + } + const auto check_obstacles_one_by_one = [&](const auto & obstacles) { + std::chrono::nanoseconds collision_checker_ns{}; + std::chrono::nanoseconds naive_ns{}; + for (const auto & obs : obstacles) { + const auto cc_start = std::chrono::system_clock::now(); + const auto collisions = collision_checker.get_collisions(obs); + MultiPoint2d cc_collision_points; + for (const auto & c : collisions) + cc_collision_points.insert( + cc_collision_points.end(), c.collision_points.begin(), c.collision_points.end()); + const auto cc_end = std::chrono::system_clock::now(); + const auto naive_start = std::chrono::system_clock::now(); + MultiPoint2d naive_collision_points; + for (const auto & ego_footprint : ego_footprints) { + MultiPoint2d points; + boost::geometry::intersection(ego_footprint, obs, points); + naive_collision_points.insert(naive_collision_points.end(), points.begin(), points.end()); + } + const auto naive_end = std::chrono::system_clock::now(); + const auto equal = all_within(cc_collision_points, naive_collision_points) && + all_within(naive_collision_points, cc_collision_points); + EXPECT_TRUE(equal); + if (!equal) { + std::cout << "cc: " << boost::geometry::wkt(cc_collision_points) << std::endl; + std::cout << "naive: " << boost::geometry::wkt(naive_collision_points) << std::endl; + } + collision_checker_ns += + std::chrono::duration_cast(cc_end - cc_start); + naive_ns += std::chrono::duration_cast(naive_end - naive_start); + } + std::printf("%20s%10ld ns\n", "collision checker : ", collision_checker_ns.count()); + std::printf("%20s%10ld ns\n", "naive : ", naive_ns.count()); + }; + const auto check_obstacles = [&](const auto & obstacles) { + std::chrono::nanoseconds collision_checker_ns{}; + std::chrono::nanoseconds naive_ns{}; + const auto cc_start = std::chrono::system_clock::now(); + const auto collisions = collision_checker.get_collisions(obstacles); + MultiPoint2d cc_collision_points; + for (const auto & c : collisions) + cc_collision_points.insert( + cc_collision_points.end(), c.collision_points.begin(), c.collision_points.end()); + const auto cc_end = std::chrono::system_clock::now(); + const auto naive_start = std::chrono::system_clock::now(); + MultiPoint2d naive_collision_points; + boost::geometry::intersection(ego_footprints, obstacles, naive_collision_points); + const auto naive_end = std::chrono::system_clock::now(); + const auto equal = all_within(cc_collision_points, naive_collision_points) && + all_within(naive_collision_points, cc_collision_points); + EXPECT_TRUE(equal); + if (!equal) { + std::cout << "cc: " << boost::geometry::wkt(cc_collision_points) << std::endl; + std::cout << "naive: " << boost::geometry::wkt(naive_collision_points) << std::endl; + } + collision_checker_ns += std::chrono::duration_cast(cc_end - cc_start); + naive_ns += std::chrono::duration_cast(naive_end - naive_start); + std::printf("%20s%10ld ns\n", "collision checker : ", collision_checker_ns.count()); + std::printf("%20s%10ld ns\n", "naive : ", naive_ns.count()); + }; + + std::cout << "* check one by one\n"; + std::printf("%d Polygons:\n", nb_obstacles); + check_obstacles_one_by_one(poly_obstacles); + std::printf("%d Lines:\n", nb_obstacles); + check_obstacles_one_by_one(line_obstacles); + std::printf("%d Points:\n", nb_obstacles); + check_obstacles_one_by_one(point_obstacles); + std::cout << "* check all at once\n"; + std::printf("%d Polygons:\n", nb_obstacles); + check_obstacles(poly_obstacles); + std::printf("%d Lines:\n", nb_obstacles); + check_obstacles(line_obstacles); + std::printf("%d Points:\n", nb_obstacles); + check_obstacles(point_obstacles); +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_planner_data.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_planner_data.cpp new file mode 100644 index 0000000000000..d111f480c6c70 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_planner_data.cpp @@ -0,0 +1,32 @@ +// 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 "autoware/motion_velocity_planner_common/planner_data.hpp" + +#include + +#include + +using autoware::motion_velocity_planner::Collision; +using autoware::motion_velocity_planner::CollisionChecker; +using autoware::universe_utils::Line2d; +using autoware::universe_utils::MultiLineString2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +TEST(TestPlannerData, CollisionChecker) +{ +} 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 index 9be416f9e9bb0..867a08e84503c 100644 --- 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 @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -83,8 +82,10 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); - processing_time_publisher_ = this->create_publisher( - "~/debug/total_time/processing_time_ms", 1); + processing_time_publisher_ = + this->create_publisher("~/debug/processing_time_ms", 1); + debug_viz_pub_ = + this->create_publisher("~/debug/markers", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); @@ -98,7 +99,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // 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 == "") { + if (name.empty()) { break; } planner_manager_.load_module_plugin(*this, name); @@ -134,7 +135,7 @@ bool MotionVelocityPlannerNode::update_planner_data() const auto check_with_log = [&](const auto ptr, const auto & log) { constexpr auto throttle_duration = 3000; // [ms] if (!ptr) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log); + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "%s", log); is_ready = false; return false; } @@ -277,7 +278,6 @@ void MotionVelocityPlannerNode::on_trajectory( 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; processing_times["generate_trajectory"] = stop_watch.toc(true); @@ -325,7 +325,8 @@ void MotionVelocityPlannerNode::insert_slowdown( autoware::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 = slowdown_interval.velocity; + trajectory.points[idx].longitudinal_velocity_mps = + static_cast(slowdown_interval.velocity); } else { RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); } @@ -360,16 +361,14 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s 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()); + clipped.end(), std::next(traj_resampled.begin(), static_cast(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) { autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + 0LU, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; } @@ -377,13 +376,21 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) { + universe_utils::StopWatch stop_watch; autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; - if (smooth_velocity_before_planning_) + if (smooth_velocity_before_planning_) { + stop_watch.tic("smooth"); input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); - const auto resampled_trajectory = - autoware::motion_utils::resampleTrajectory(output_trajectory_msg, 0.5); - + RCLCPP_DEBUG(get_logger(), "smooth: %2.2f us", stop_watch.toc("smooth")); + } + autoware_planning_msgs::msg::Trajectory smooth_velocity_trajectory; + smooth_velocity_trajectory.points = { + input_trajectory_points.begin(), input_trajectory_points.end()}; + auto resampled_trajectory = + autoware::motion_utils::resampleTrajectory(smooth_velocity_trajectory, 0.5); + motion_utils::calculate_time_from_start( + resampled_trajectory.points, planner_data_.current_odometry.pose.pose.position); const auto planning_results = planner_manager_.plan_velocities( resampled_trajectory.points, std::make_shared(planner_data_));