From a0bac37096604019cd1858a0aa54df3448e27913 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 27 Mar 2024 13:46:08 +0900 Subject: [PATCH] feat(behavior_velocity_run_out_module): exclude obstacle crossing ego back line (#6680) * add method to ignore target obstacles that cross ego cut lane Signed-off-by: Daniel Sanchez * WIP add debug support Signed-off-by: Daniel Sanchez * add params and finish debug marker Signed-off-by: Daniel Sanchez * change lane to line Signed-off-by: Daniel Sanchez * use autoware utils to get the cut line Signed-off-by: Daniel Sanchez * simplify code wit calcOffsetPose Signed-off-by: Daniel Sanchez * Update readme and eliminate unused code Signed-off-by: Daniel Sanchez * update readme Signed-off-by: Daniel Sanchez * eliminate unused function Signed-off-by: Daniel Sanchez * readme Signed-off-by: Daniel Sanchez * comments and readme Signed-off-by: Daniel Sanchez * eliminate unused include Signed-off-by: Daniel Sanchez * typo Signed-off-by: Daniel Sanchez * rename param for consistency Signed-off-by: Daniel Sanchez * change lane to line for consistency Signed-off-by: Daniel Sanchez * rename for clarity, add brief Signed-off-by: Daniel Sanchez * fix indentation Signed-off-by: Daniel Sanchez * update description Signed-off-by: Daniel Sanchez * lane ->line Signed-off-by: Daniel Sanchez * lane -> line Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../README.md | 8 ++ .../config/run_out.param.yaml | 2 + .../docs/ego_cut_line.svg | 129 ++++++++++++++++++ .../src/debug.cpp | 19 +++ .../src/debug.hpp | 2 + .../src/manager.cpp | 2 + .../src/scene.cpp | 38 +++++- .../src/scene.hpp | 11 ++ .../src/utils.cpp | 22 +++ .../src/utils.hpp | 8 ++ 10 files changed, 235 insertions(+), 6 deletions(-) create mode 100644 planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index c48f2a951cd55..5cd4b2708fd2d 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -108,6 +108,14 @@ You can choose whether to use this feature by parameter of `use_partition_lanele ![brief](./docs/exclude_obstacles_by_partition.svg) +##### Exclude obstacles that cross the ego vehicle's "cut line" + +This module can exclude obstacles that have predicted paths that will cross the back side of the ego vehicle. It excludes obstacles if their predicted path crosses the ego's "cut line". The "cut line" is a virtual line segment that is perpendicular to the ego vehicle and that passes through the ego's base link. + +You can choose whether to use this feature by setting the parameter `use_ego_cut_line` to `true` or `false`. The width of the line can be tuned with the parameter `ego_cut_line_length`. + +![brief](./docs/ego_cut_line.svg) + #### Collision detection ##### Detect collision with dynamic obstacles diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index 196f7c6296cb4..15264e1e4e2b1 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -4,6 +4,7 @@ detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet + use_ego_cut_line: true # [-] filter obstacles that pass the backside of ego: if a dynamic obstacle's predicted path intersects this line, it is ignored specify_decel_jerk: true # [-] whether to specify jerk when ego decelerates stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin @@ -11,6 +12,7 @@ detection_distance: 45.0 # [m] ahead distance from ego to detect the obstacles detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision + ego_cut_line_length: 3.0 # The width of the ego's cut line # Parameter to create abstracted dynamic obstacles dynamic_obstacle: diff --git a/planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg b/planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg new file mode 100644 index 0000000000000..de6bec5e172df --- /dev/null +++ b/planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg @@ -0,0 +1,129 @@ + + + + + + + + + + diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp index c0d026b5ffaf4..23764bc73fbff 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_run_out_module/src/debug.cpp @@ -86,6 +86,14 @@ void RunOutDebug::pushCollisionPoints(const geometry_msgs::msg::Point & point) collision_points_.push_back(point_with_height); } +void RunOutDebug::pushEgoCutLine(const std::vector & line) +{ + for (const auto & point : line) { + const auto point_with_height = createPoint(point.x, point.y, height_); + ego_cut_line_.push_back(point_with_height); + } +} + void RunOutDebug::pushCollisionPoints(const std::vector & points) { for (const auto & p : points) { @@ -160,6 +168,7 @@ void RunOutDebug::clearDebugMarker() predicted_obstacle_polygons_.clear(); collision_obstacle_polygons_.clear(); travel_time_texts_.clear(); + ego_cut_line_.clear(); } visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray() @@ -265,6 +274,16 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray &msg); } + if (!ego_cut_line_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "ego_cut_line", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.2, 0.2, 0.2), createMarkerColor(0.7, 0.0, 0.7, 0.999)); + for (const auto & p : ego_cut_line_) { + marker.points.push_back(p); + } + msg.markers.push_back(marker); + } + if (!travel_time_texts_.empty()) { auto marker = createDefaultMarker( "map", current_time, "travel_time_texts", 0, diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index b28725a92628e..e9b269ee437f0 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -109,6 +109,7 @@ class RunOutDebug void pushPredictedVehiclePolygons(const std::vector & polygon); void pushPredictedObstaclePolygons(const std::vector & polygon); void pushCollisionObstaclePolygons(const std::vector & polygon); + void pushEgoCutLine(const std::vector & line); void pushDetectionAreaPolygons(const Polygon2d & debug_polygon); void pushMandatoryDetectionAreaPolygons(const Polygon2d & debug_polygon); void pushTravelTimeTexts( @@ -134,6 +135,7 @@ class RunOutDebug rclcpp::Publisher::SharedPtr pub_debug_pointcloud_; std::vector collision_points_; std::vector nearest_collision_point_; + std::vector ego_cut_line_; std::vector stop_pose_; std::vector> predicted_vehicle_polygons_; std::vector> predicted_obstacle_polygons_; diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index f2ef366cbe121..305d74e77f348 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) auto & p = planner_param_.run_out; p.detection_method = getOrDeclareParameter(node, ns + ".detection_method"); p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); + p.use_ego_cut_line = getOrDeclareParameter(node, ns + ".use_ego_cut_line"); p.suppress_on_crosswalk = getOrDeclareParameter(node, ns + ".suppress_on_crosswalk"); p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk"); p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); @@ -66,6 +67,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.detection_distance = getOrDeclareParameter(node, ns + ".detection_distance"); p.detection_span = getOrDeclareParameter(node, ns + ".detection_span"); p.min_vel_ego_kmph = getOrDeclareParameter(node, ns + ".min_vel_ego_kmph"); + p.ego_cut_line_length = getOrDeclareParameter(node, ns + ".ego_cut_line_length"); } { diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 50b69fc6139c9..1dc86aa8bf87e 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -102,9 +102,18 @@ bool RunOutModule::modifyPathVelocity( const auto dynamic_obstacles = dynamic_obstacle_creator_->createDynamicObstacles(); debug_ptr_->setDebugValues(DebugValues::TYPE::NUM_OBSTACLES, dynamic_obstacles.size()); - // extract obstacles using lanelet information - const auto partition_excluded_obstacles = - excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose); + const auto filtered_obstacles = std::invoke([&]() { + // extract obstacles using lanelet information + const auto partition_excluded_obstacles = + excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose); + + if (!planner_param_.run_out.use_ego_cut_line) return partition_excluded_obstacles; + + // extract obstacles that cross the ego's cut line + const auto ego_cut_line_excluded_obstacles = + excludeObstaclesCrossingEgoCutLine(partition_excluded_obstacles, current_pose); + return ego_cut_line_excluded_obstacles; + }); // record time for obstacle creation const auto t_obstacle_creation = std::chrono::system_clock::now(); @@ -122,7 +131,7 @@ bool RunOutModule::modifyPathVelocity( planner_data_->route_handler_->getOverallGraphPtr()) : std::vector>(); const auto dynamic_obstacle = - detectCollision(partition_excluded_obstacles, extended_smoothed_path, crosswalk_lanelets); + detectCollision(filtered_obstacles, extended_smoothed_path, crosswalk_lanelets); // record time for collision check const auto t_collision_check = std::chrono::system_clock::now(); @@ -147,8 +156,7 @@ bool RunOutModule::modifyPathVelocity( applyMaxJerkLimit(current_pose, current_vel, current_acc, *path); } - publishDebugValue( - extended_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); + publishDebugValue(extended_smoothed_path, filtered_obstacles, dynamic_obstacle, current_pose); // record time for collision check const auto t_path_planning = std::chrono::system_clock::now(); @@ -810,6 +818,24 @@ void RunOutModule::applyMaxJerkLimit( run_out_utils::insertPathVelocityFromIndex(stop_point_idx.value(), jerk_limited_vel, path.points); } +std::vector RunOutModule::excludeObstaclesCrossingEgoCutLine( + const std::vector & dynamic_obstacles, + const geometry_msgs::msg::Pose & current_pose) const +{ + std::vector extracted_obstacles; + std::vector ego_cut_line; + const double ego_cut_line_half_width = planner_param_.run_out.ego_cut_line_length / 2.0; + std::for_each(dynamic_obstacles.begin(), dynamic_obstacles.end(), [&](const auto & o) { + const auto predicted_path = run_out_utils::getHighestConfidencePath(o.predicted_paths); + if (!run_out_utils::pathIntersectsEgoCutLine( + predicted_path, current_pose, ego_cut_line_half_width, ego_cut_line)) { + extracted_obstacles.push_back(o); + } + }); + debug_ptr_->pushEgoCutLine(ego_cut_line); + return extracted_obstacles; +} + std::vector RunOutModule::excludeObstaclesOutSideOfPartition( const std::vector & dynamic_obstacles, const PathWithLaneId & path, const geometry_msgs::msg::Pose & current_pose) const diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index def90f036c440..02b17783ab6c1 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -136,6 +136,17 @@ class RunOutModule : public SceneModuleInterface const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, PathWithLaneId & path) const; + /** + * @brief Creates a virtual line segment that is perpendicular to the ego vehicle and that passes + * through the ego's base link and excludes objects with paths that intersect that line segment. + * @param [in] dynamic_obstacles obstacles to be filtered. + * @param [in] current_pose ego vehicle's current pose. + * @return a vector of dynamic obstacles that don't intersect the line segment. + */ + std::vector excludeObstaclesCrossingEgoCutLine( + const std::vector & dynamic_obstacles, + const geometry_msgs::msg::Pose & current_pose) const; + std::vector excludeObstaclesOutSideOfPartition( const std::vector & dynamic_obstacles, const PathWithLaneId & path, const geometry_msgs::msg::Pose & current_pose) const; diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index db989f90fafc9..f17f4c7251ea4 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -176,6 +176,28 @@ std::optional findFirstStopPointIdx(PathPointsWithLaneId & path_points) return {}; } +bool pathIntersectsEgoCutLine( + const std::vector & path, const geometry_msgs::msg::Pose & ego_pose, + const double half_line_length, std::vector & ego_cut_line) +{ + if (path.size() < 2) return false; + const auto p1 = + tier4_autoware_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0).position; + const auto p2 = + tier4_autoware_utils::calcOffsetPose(ego_pose, 0.0, -half_line_length, 0.0).position; + ego_cut_line = {p1, p2}; + + for (size_t i = 1; i < path.size(); ++i) { + const auto & p3 = path.at(i).position; + const auto & p4 = path.at(i - 1).position; + const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + if (intersection.has_value()) { + return true; + } + } + return false; +} + LineString2d createLineString2d(const lanelet::BasicPolygon2d & poly) { LineString2d line_string; diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index c4976a119dd00..63576ca62329f 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -15,6 +15,8 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include #include @@ -56,9 +58,11 @@ struct RunOutParam bool use_partition_lanelet; bool suppress_on_crosswalk; bool specify_decel_jerk; + bool use_ego_cut_line; double stop_margin; double passing_margin; double deceleration_jerk; + double ego_cut_line_length; float detection_distance; float detection_span; float min_vel_ego_kmph; @@ -194,6 +198,10 @@ struct DynamicObstacleData Polygon2d createBoostPolyFromMsg(const std::vector & input_poly); +bool pathIntersectsEgoCutLine( + const std::vector & path, const geometry_msgs::msg::Pose & ego_pose, + const double half_line_length, std::vector & ego_cut_line); + std::uint8_t getHighestProbLabel(const std::vector & classification); std::vector getHighestConfidencePath(