From ae833f35a9879456db930a35f8b70e86a4a278b0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 30 Aug 2022 16:02:42 +0900 Subject: [PATCH] fix(behavior_velocity_planner): support u-turn for detection_area, vtl and stop line (#1700) * fix(behavior_velocity_planner): deal with u-turn for detection_area, vtl, stop_line Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * enable behavior velocity test and fix bug Signed-off-by: Takayuki Murooka * fix bug Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix collision check Signed-off-by: Takayuki Murooka * add test Signed-off-by: Takayuki Murooka * remove unnecessary debug output Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../trajectory/path_with_lane_id.hpp | 9 +- .../src/trajectory/test_path_with_lane_id.cpp | 10 +- .../planner_data.hpp | 4 + .../include/scene_module/blind_spot/scene.hpp | 2 +- .../scene_module/detection_area/scene.hpp | 6 +- .../scene_no_stopping_area.hpp | 5 +- .../occlusion_spot/occlusion_spot_utils.hpp | 4 +- .../scene_module/scene_module_interface.hpp | 34 +++ .../include/scene_module/stop_line/scene.hpp | 11 +- .../scene_module/traffic_light/scene.hpp | 8 +- .../virtual_traffic_light/scene.hpp | 21 +- .../include/utilization/arc_lane_util.hpp | 144 +++++++---- .../include/utilization/trajectory_utils.hpp | 53 +---- .../include/utilization/util.hpp | 134 ++++------- .../behavior_velocity_planner/src/node.cpp | 6 + .../src/scene_module/crosswalk/manager.cpp | 7 +- .../scene_module/detection_area/manager.cpp | 3 +- .../src/scene_module/detection_area/scene.cpp | 46 ++-- .../scene_module/no_stopping_area/manager.cpp | 7 +- .../scene_no_stopping_area.cpp | 6 +- .../occlusion_spot/occlusion_spot_utils.cpp | 6 +- .../occlusion_spot/scene_occlusion_spot.cpp | 3 +- .../src/scene_module/run_out/scene.cpp | 3 +- .../src/scene_module/stop_line/scene.cpp | 222 +++-------------- .../scene_module/traffic_light/manager.cpp | 7 +- .../src/scene_module/traffic_light/scene.cpp | 6 +- .../virtual_traffic_light/manager.cpp | 5 +- .../virtual_traffic_light/scene.cpp | 225 +++++++++++------- .../src/utilization/util.cpp | 56 +++-- .../test/src/test_arc_lane_util.cpp | 156 +++++++++++- 30 files changed, 659 insertions(+), 550 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index a10db47a377a2..a4a3842d1047d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -20,13 +20,14 @@ #include +#include #include #include namespace motion_utils { inline boost::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -35,7 +36,7 @@ inline boost::optional> getPathIndexRangeWithLaneId( for (size_t i = 0; i < path.points.size(); ++i) { const auto & p = path.points.at(i); for (const auto & id : p.lane_ids) { - if (id == lane_id) { + if (id == target_lane_id) { if (!found_first_idx) { start_idx = i; found_first_idx = true; @@ -46,6 +47,10 @@ inline boost::optional> getPathIndexRangeWithLaneId( } if (found_first_idx) { + // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. + start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); + end_idx = std::min(path.points.size() - 1, end_idx + 1); + return std::make_pair(start_idx, end_idx); } diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 1eadf7105d087..d3e2565c99f1b 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -72,16 +72,16 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { const auto res = getPathIndexRangeWithLaneId(points, 3); EXPECT_EQ(res->first, 0U); - EXPECT_EQ(res->second, 1U); + EXPECT_EQ(res->second, 2U); } { const auto res = getPathIndexRangeWithLaneId(points, 1); - EXPECT_EQ(res->first, 2U); - EXPECT_EQ(res->second, 2U); + EXPECT_EQ(res->first, 1U); + EXPECT_EQ(res->second, 3U); } { const auto res = getPathIndexRangeWithLaneId(points, 2); - EXPECT_EQ(res->first, 3U); + EXPECT_EQ(res->first, 2U); EXPECT_EQ(res->second, 5U); } { @@ -142,7 +142,7 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId) for (size_t i = 3; i < 9; ++i) { modified_path.points.at(i).lane_ids = {100}; } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 3U); + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U); EXPECT_EQ( findNearestSegmentIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U); } diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp index 1e19a0d4f256b..8c95b6d128992 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp @@ -78,6 +78,10 @@ struct PlannerData // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + // nearest search + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + // other internal data std::map traffic_light_id_map; // external data diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index d3b27163d4c88..4ee5d9d40dc4e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -123,7 +123,7 @@ class BlindSpotModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; private: - int64_t lane_id_; + const int64_t lane_id_; TurnDirection turn_direction_; bool has_traffic_light_; bool is_over_pass_judge_line_; diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index 4d48f8d787853..08da219f9bea7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -66,7 +66,8 @@ class DetectionAreaModule : public SceneModuleInterface public: DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); @@ -85,6 +86,9 @@ class DetectionAreaModule : public SceneModuleInterface bool hasEnoughBrakingDistance( const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; + // Lane id + int64_t lane_id_; + // Key Feature const lanelet::autoware::DetectionArea & detection_area_reg_elem_; diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp index 034247925da40..2cf3a7524efdf 100644 --- a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp @@ -79,7 +79,8 @@ class NoStoppingAreaModule : public SceneModuleInterface public: NoStoppingAreaModule( - const int64_t module_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); @@ -91,6 +92,8 @@ class NoStoppingAreaModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; private: + const int64_t lane_id_; + mutable bool pass_judged_ = false; mutable bool is_stoppable_ = true; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 5f9ac6fcaaa86..5255cb0630117 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -203,8 +203,8 @@ struct DebugData PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0); //!< @brief wrapper for detection area polygon generation bool buildDetectionAreaPolygon( - Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & pose, - const PlannerParam & param); + Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const PlannerParam & param); lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); // Note : consider offset_from_start_to_ego and safety margin for collision here void handleCollisionOffset(std::vector & possible_collisions, double offset); diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index 06586ac3b8c7f..e36dd38516890 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -37,6 +37,7 @@ #include #include #include +#include // Debug #include @@ -110,6 +111,22 @@ class SceneModuleInterface void setSafe(const bool safe) { safe_ = safe; } void setDistance(const double distance) { distance_ = distance; } + + template + size_t findEgoSegmentIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + } + + template + size_t findEgoIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + } }; class SceneModuleManagerInterface @@ -261,6 +278,23 @@ class SceneModuleManagerInterface scene_modules_.erase(scene_module); } + template + size_t findEgoSegmentIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + } + + template + size_t findEgoIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + } + std::set> scene_modules_; std::set registered_module_id_set_; diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp index d109398e33d56..b93e72b2776cf 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp @@ -90,14 +90,8 @@ class StopLineModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; private: - int64_t module_id_; - geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line); - boost::optional findCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const SearchRangeIndex & search_index); - boost::optional findOffsetSegment( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const StopLineModule::SegmentIndexWithPoint2d & collision); @@ -111,8 +105,11 @@ class StopLineModule : public SceneModuleInterface const StopLineModule::SegmentIndexWithPose & insert_index_with_pose, tier4_planning_msgs::msg::StopReason * stop_reason); - lanelet::ConstLineString3d stop_line_; int64_t lane_id_; + + lanelet::ConstLineString3d stop_line_; + + // State machine State state_; // Parameter diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp index ace832e44ab38..56c73f3e1699f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp @@ -66,8 +66,9 @@ class TrafficLightModule : public SceneModuleInterface public: TrafficLightModule( - const int64_t module_id, const lanelet::TrafficLight & traffic_light_reg_elem, - lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const int64_t lane_id, + const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, + const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity( @@ -123,6 +124,9 @@ class TrafficLightModule : public SceneModuleInterface autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState( const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const; + // Lane id + const int64_t lane_id_; + // Key Feature const lanelet::TrafficLight & traffic_light_reg_elem_; lanelet::ConstLanelet lane_; diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 7f8e81891c3e4..24f628927e21b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -75,8 +75,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface public: VirtualTrafficLightModule( - const int64_t module_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, - lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, + const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity( @@ -87,7 +88,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; private: - const int64_t module_id_; + const int64_t lane_id_; const lanelet::autoware::VirtualTrafficLight & reg_elem_; const lanelet::ConstLanelet lane_; const PlannerParam planner_param_; @@ -101,13 +102,15 @@ class VirtualTrafficLightModule : public SceneModuleInterface void setStopReason( const geometry_msgs::msg::Pose & stop_pose, tier4_planning_msgs::msg::StopReason * stop_reason); - bool isBeforeStartLine(); + boost::optional getPathIndexOfFirstEndLine(); - bool isBeforeStopLine(); + bool isBeforeStartLine(const size_t end_line_idx); - bool isAfterAnyEndLine(); + bool isBeforeStopLine(const size_t end_line_idx); - bool isNearAnyEndLine(); + bool isAfterAnyEndLine(const size_t end_line_idx); + + bool isNearAnyEndLine(const size_t end_line_idx); boost::optional findCorrespondingState(); @@ -117,11 +120,11 @@ class VirtualTrafficLightModule : public SceneModuleInterface void insertStopVelocityAtStopLine( autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason); + tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); void insertStopVelocityAtEndLine( autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason); + tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); }; } // namespace behavior_velocity_planner #endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp index 55e16bd77a3a5..dc282785f330d 100644 --- a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp @@ -43,11 +43,53 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; + +namespace +{ +geometry_msgs::msg::Point convertToGeomPoint(const Point2d & p) +{ + geometry_msgs::msg::Point geom_p; + geom_p.x = p.x(); + geom_p.y = p.y(); + + return geom_p; +} + +geometry_msgs::msg::Point operator+( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + geometry_msgs::msg::Point p; + p.x = p1.x + p2.x; + p.y = p1.y + p2.y; + p.z = p1.z + p2.z; + + return p; +} + +[[maybe_unused]] geometry_msgs::msg::Point operator*( + const geometry_msgs::msg::Point & p, const double v) +{ + geometry_msgs::msg::Point multiplied_p; + multiplied_p.x = p.x * v; + multiplied_p.y = p.y * v; + multiplied_p.z = p.z * v; + + return multiplied_p; +} + +[[maybe_unused]] geometry_msgs::msg::Point operator*( + const double v, const geometry_msgs::msg::Point & p) +{ + return p * v; +} +} // namespace + namespace arc_lane_utils { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = std::pair; // front index, point2d -using PathIndexWithOffset = std::pair; // front index, offset +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithPoint = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset inline double calcSignedDistance( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) @@ -58,51 +100,71 @@ inline double calcSignedDistance( return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); } -inline boost::optional getNearestCollisionPoint( - const LineString2d & stop_line, const LineString2d & path_segment) +// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) +inline boost::optional checkCollision( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) { - // Find all collision points - std::vector collision_points; - bg::intersection(stop_line, path_segment, collision_points); - if (collision_points.empty()) { - return {}; + const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x); + + if (det == 0.0) { + // collision is not one point. + return boost::none; } - // To dist list - std::vector dist_list; - dist_list.reserve(collision_points.size()); - std::transform( - collision_points.cbegin(), collision_points.cend(), std::back_inserter(dist_list), - [&path_segment](const Point2d & collision_point) { - return bg::distance(path_segment.front(), collision_point); - }); + const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det; + const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det; - // Find nearest collision point - const auto min_itr = std::min_element(dist_list.cbegin(), dist_list.cend()); - const auto min_idx = std::distance(dist_list.cbegin(), min_itr); + // check collision is outside the segment line + if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) { + return boost::none; + } - return collision_points.at(min_idx); + return p1 * (1.0 - t1) + p2 * t1; } template -boost::optional findCollisionSegment( - const T & path, const LineString2d & stop_line) +boost::optional findCollisionSegment( + const T & path, const geometry_msgs::msg::Point & stop_line_p1, + const geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id) { for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point - const auto & p2 = path.points.at(i + 1).point.pose.position; // Point after collision point + const auto & prev_lane_ids = path.points.at(i).lane_ids; + const auto & next_lane_ids = path.points.at(i + 1).lane_ids; + + const bool is_target_lane_in_prev_lane = + std::find(prev_lane_ids.begin(), prev_lane_ids.end(), target_lane_id) != prev_lane_ids.end(); + const bool is_target_lane_in_next_lane = + std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != next_lane_ids.end(); + if (!is_target_lane_in_prev_lane && !is_target_lane_in_next_lane) { + continue; + } - const LineString2d path_segment = {{p1.x, p1.y}, {p2.x, p2.y}}; + const auto & p1 = + tier4_autoware_utils::getPoint(path.points.at(i)); // Point before collision point + const auto & p2 = + tier4_autoware_utils::getPoint(path.points.at(i + 1)); // Point after collision point - const auto nearest_collision_point = getNearestCollisionPoint(stop_line, path_segment); - if (nearest_collision_point) { - return std::make_pair(i, *nearest_collision_point); + const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); + + if (collision_point) { + return std::make_pair(i, collision_point.get()); } } return {}; } +template +boost::optional findCollisionSegment( + const T & path, const LineString2d & stop_line, const size_t target_lane_id) +{ + const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0)); + const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1)); + + return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_id); +} + template boost::optional findForwardOffsetSegment( const T & path, const size_t base_idx, const double offset_length) @@ -143,20 +205,22 @@ boost::optional findBackwardOffsetSegment( template boost::optional findOffsetSegment( - const T & path, const PathIndexWithPoint2d & collision_segment, const double offset_length) + const T & path, const PathIndexWithPoint & collision_segment, const double offset_length) { const size_t collision_idx = collision_segment.first; - const Point2d & collision_point = collision_segment.second; - const auto p_front = to_bg2d(path.points.at(collision_idx).point.pose.position); - const auto p_back = to_bg2d(path.points.at(collision_idx + 1).point.pose.position); + const auto & collision_point = collision_segment.second; if (offset_length >= 0) { return findForwardOffsetSegment( - path, collision_idx, offset_length + bg::distance(p_front, collision_point)); - } else { - return findBackwardOffsetSegment( - path, collision_idx + 1, -offset_length + bg::distance(p_back, collision_point)); + path, collision_idx, + offset_length + + tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); } + + return findBackwardOffsetSegment( + path, collision_idx + 1, + -offset_length + + tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); } inline boost::optional findOffsetSegment( @@ -201,10 +265,10 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse inline boost::optional createTargetPoint( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const double margin, const double vehicle_offset) + const size_t lane_id, const double margin, const double vehicle_offset) { // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line); + const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); if (!collision_segment) { // No collision return {}; diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index aa5eecc245de6..228cd0768f465 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -85,58 +85,16 @@ inline Quaternion lerpOrientation( return tf2::toMsg(q_interpolated); } -/** - * @brief apply linear interpolation to trajectory point that is nearest to a certain point - * @param [in] points trajectory points - * @param [in] point Interpolated point is nearest to this point. - */ -template -boost::optional getLerpTrajectoryPointWithIdx( - const T & points, const geometry_msgs::msg::Point & point) -{ - TrajectoryPoint interpolated_point; - const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(points, point); - const double len_to_interpolated = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point); - const double len_segment = - motion_utils::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1); - const double ratio = len_to_interpolated / len_segment; - if (ratio <= 0.0 || 1.0 <= ratio) return boost::none; - const double interpolate_ratio = std::clamp(ratio, 0.0, 1.0); - { - const size_t i = nearest_seg_idx; - const auto & pos0 = points.at(i).pose.position; - const auto & pos1 = points.at(i + 1).pose.position; - interpolated_point.pose.position.x = interpolation::lerp(pos0.x, pos1.x, interpolate_ratio); - interpolated_point.pose.position.y = interpolation::lerp(pos0.y, pos1.y, interpolate_ratio); - interpolated_point.pose.position.z = interpolation::lerp(pos0.z, pos1.z, interpolate_ratio); - interpolated_point.pose.orientation = lerpOrientation( - points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( - points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, - interpolate_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( - points.at(i).lateral_velocity_mps, points.at(i + 1).lateral_velocity_mps, interpolate_ratio); - interpolated_point.acceleration_mps2 = interpolation::lerp( - points.at(i).acceleration_mps2, points.at(i + 1).acceleration_mps2, interpolate_ratio); - interpolated_point.heading_rate_rps = interpolation::lerp( - points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio); - } - return std::make_pair(interpolated_point, nearest_seg_idx); -} - //! smooth path point with lane id starts from ego position on path to the path end inline bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, const std::shared_ptr & planner_data) { - using motion_utils::findNearestIndex; const geometry_msgs::msg::Pose current_pose = planner_data->current_pose.pose; const double v0 = planner_data->current_velocity->twist.linear.x; const double a0 = planner_data->current_accel.get(); const auto & external_v_limit = planner_data->external_velocity_limit; const auto & smoother = planner_data->velocity_smoother_; - const double max = std::numeric_limits::max(); auto trajectory = convertPathToTrajectoryPoints(in_path); if (external_v_limit) { @@ -148,23 +106,22 @@ inline bool smoothPath( // Resample trajectory with ego-velocity based interval distances auto traj_resampled = smoother->resampleTrajectory( *traj_lateral_acc_filtered, v0, current_pose, std::numeric_limits::max()); - const auto traj_resampled_closest = findNearestIndex(*traj_resampled, current_pose, max, M_PI_4); - if (!traj_resampled_closest) { - return false; - } + const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( + *traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); std::vector debug_trajectories; // Clip trajectory from closest point TrajectoryPoints clipped; TrajectoryPoints traj_smoothed; clipped.insert( - clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end()); + clipped.end(), traj_resampled->begin() + traj_resampled_closest, traj_resampled->end()); if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; return false; } traj_smoothed.insert( traj_smoothed.begin(), traj_resampled->begin(), - traj_resampled->begin() + *traj_resampled_closest); + traj_resampled->begin() + traj_resampled_closest); out_path = convertTrajectoryPointsToPath(traj_smoothed); return true; diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 0a3f410aa0021..c05a8819e9267 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -90,8 +90,6 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; using motion_utils::validateNonEmpty; using tier4_autoware_utils::calcAzimuthAngle; using tier4_autoware_utils::calcDistance2d; @@ -104,10 +102,48 @@ using tier4_planning_msgs::msg::StopReason; namespace planning_utils { +template +size_t calcPointIndexFromSegmentIndex( + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) +{ + const size_t prev_point_idx = seg_idx; + const size_t next_point_idx = seg_idx + 1; + + const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx)); + const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx)); + + if (prev_dist < next_dist) { + return prev_point_idx; + } + return next_point_idx; +} + +template +size_t calcSegmentIndexFromPointIndex( + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) +{ + if (idx == 0) { + return 0; + } + if (idx == points.size() - 1) { + return idx - 1; + } + if (points.size() < 3) { + return 0; + } + + const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); + if (0 < offset_to_seg) { + return idx; + } + return idx - 1; +} + // create detection area from given range return false if creation failure bool createDetectionAreaPolygons( - Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & current_pose, - const DetectionRange & da_range, const double obstacle_vel_mps, const double min_velocity = 1.0); + Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, + const double min_velocity = 1.0); PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio); Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y); void extractClosePartition( @@ -125,89 +161,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( geometry_msgs::msg::Pose transformAbsCoordinate2D( const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin); SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id); -/** - * @brief find nearest segment index with search range - */ -template -size_t findNearestSegmentIndex(const T & points, const PointWithSearchRangeIndex & point_with_index) -{ - const auto & index = point_with_index.index; - const auto point = point_with_index.point; - - validateNonEmpty(points); - - double min_dist = std::numeric_limits::max(); - size_t nearest_idx = 0; - for (size_t i = index.min_idx; i <= index.max_idx; ++i) { - const auto dist = calcSquaredDistance2d(points.at(i), point); - if (dist < min_dist) { - min_dist = dist; - nearest_idx = i; - } - } - - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == points.size() - 1) { - return points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} -/** - * @brief find nearest segment index within distance threshold - */ -template -PointWithSearchRangeIndex findFirstNearSearchRangeIndex( - const T & points, const geometry_msgs::msg::Point & point, const double distance_thresh = 9.0) -{ - validateNonEmpty(points); - - bool min_idx_found = false; - bool max_idx_found = false; - PointWithSearchRangeIndex point_with_range = {point, {static_cast(0), points.size() - 1}}; - for (size_t i = 0; i < points.size(); i++) { - const auto & p = points.at(i).point.pose.position; - const double dist = std::hypot(point.x - p.x, point.y - p.y); - if (dist < distance_thresh) { - if (!min_idx_found) { - point_with_range.index.min_idx = i; - min_idx_found = true; - } - if (!max_idx_found) point_with_range.index.max_idx = i; - } else if (min_idx_found) { - // found close index and farther than distance_thresh, stop update max index - max_idx_found = true; - } - } - return point_with_range; -} -/** - * @brief calcSignedArcLength from point to point with search range - */ -template -double calcSignedArcLengthWithSearchIndex( - const T & points, const PointWithSearchRangeIndex & src_point_with_range, - const PointWithSearchRangeIndex & dst_point_with_range) -{ - validateNonEmpty(points); - const size_t src_idx = planning_utils::findNearestSegmentIndex(points, src_point_with_range); - const size_t dst_idx = planning_utils::findNearestSegmentIndex(points, dst_point_with_range); - const double signed_length = calcSignedArcLength(points, src_idx, dst_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_idx, src_point_with_range.point); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_idx, dst_point_with_range.point); - return signed_length - signed_length_src_offset + signed_length_dst_offset; -} Polygon2d toFootprintPolygon(const PredictedObject & object); bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( @@ -254,7 +208,7 @@ std::vector concatVector(const std::vector & vec1, const std::vector & boost::optional getNearestLaneId( const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, - const geometry_msgs::msg::Pose & current_pose, boost::optional & nearest_segment_idx); + const geometry_msgs::msg::Pose & current_pose); std::vector getSortedLaneIdsFromPath(const PathWithLaneId & path); @@ -270,9 +224,7 @@ std::unordered_map, lanelet::ConstLanelet> get std::unordered_map, lanelet::ConstLanelet> reg_elem_map_on_path; // Add current lane id - boost::optional nearest_segment_idx; - const auto nearest_lane_id = - getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx); + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { @@ -338,6 +290,8 @@ bool isOverLine( boost::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); +boost::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output); } // namespace planning_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 587716670e62d..2ac0e6eb67f71 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -163,6 +163,12 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_data_.accel_lowpass_gain_ = this->declare_parameter("lowpass_gain", 0.5); planner_data_.stop_line_extend_length = this->declare_parameter("stop_line_extend_length", 5.0); + // nearest search + planner_data_.ego_nearest_dist_threshold = + this->declare_parameter("ego_nearest_dist_threshold"); + planner_data_.ego_nearest_yaw_threshold = + this->declare_parameter("ego_nearest_yaw_threshold"); + // Initialize PlannerManager if (this->declare_parameter("launch_crosswalk", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index 200cf873ad4b4..956655d38aecf 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -30,12 +30,9 @@ std::vector getCrosswalksOnPath( { std::vector crosswalks; - auto nearest_segment_idx = motion_utils::findNearestSegmentIndex( - path.points, current_pose, std::numeric_limits::max(), M_PI_2); - // Add current lane id - const auto nearest_lane_id = behavior_velocity_planner::planning_utils::getNearestLaneId( - path, lanelet_map, current_pose, nearest_segment_idx); + const auto nearest_lane_id = + behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index f9bee714d7aff..0d6c838a4fcae 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -49,10 +49,11 @@ void DetectionAreaModuleManager::launchNewModules( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose)) { // Use lanelet_id to unregister module when the route is changed + const auto lane_id = detection_area_with_lane_id.second.id(); const auto module_id = detection_area_with_lane_id.first->id(); if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *detection_area_with_lane_id.first, planner_param_, + module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, logger_.get_child("detection_area_module"), clock_)); generateUUID(module_id); } diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 1573f5f576b6f..d89bb16b8b3ce 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -34,14 +34,14 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; -using motion_utils::findNearestSegmentIndex; -using motion_utils::insertTargetPoint; DetectionAreaModule::DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), planner_param_(planner_param) @@ -77,19 +77,27 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Get self pose const auto & self_pose = planner_data_->current_pose.pose; + const size_t current_seg_idx = findEgoSegmentIndex(path->points); // Get stop point const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, planner_param_.stop_margin, + original_path, stop_line, lane_id_, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { return true; } - const auto is_stopped = planner_data_->isVehicleStopped(0.0); + const auto & stop_point_idx = stop_point->first; + const auto & stop_pose = stop_point->second; + const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, stop_pose.position, stop_point_idx); + + auto modified_stop_pose = stop_pose; + size_t modified_stop_line_seg_idx = stop_line_seg_idx; - auto stop_pose = stop_point->second; - const auto stop_dist = calcSignedArcLength(path->points, self_pose.position, stop_pose.position); + const auto is_stopped = planner_data_->isVehicleStopped(0.0); + const auto stop_dist = calcSignedArcLength( + path->points, self_pose.position, current_seg_idx, stop_pose.position, stop_line_seg_idx); // Don't re-approach when the ego stops closer to the stop point than hold_stop_margin_distance if (is_stopped && stop_dist < planner_param_.hold_stop_margin_distance) { @@ -100,7 +108,8 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } - stop_pose = ego_pos_on_path.get(); + modified_stop_pose = ego_pos_on_path.get(); + modified_stop_line_seg_idx = current_seg_idx; } setDistance(stop_dist); @@ -117,14 +126,22 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (planner_param_.use_dead_line) { // Use '-' for margin because it's the backward distance from stop line const auto dead_line_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, -planner_param_.dead_line_margin, + original_path, stop_line, lane_id_, -planner_param_.dead_line_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (dead_line_point) { + const size_t dead_line_point_idx = dead_line_point->first; const auto & dead_line_pose = dead_line_point->second; + + const size_t dead_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, dead_line_pose.position, dead_line_point_idx); + debug_data_.dead_line_poses.push_back(dead_line_pose); - if (planning_utils::isOverLine(original_path, self_pose, dead_line_pose)) { + const double dist_from_ego_to_dead_line = calcSignedArcLength( + original_path.points, self_pose.position, current_seg_idx, dead_line_pose.position, + dead_line_seg_idx); + if (dist_from_ego_to_dead_line < 0.0) { RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line"); setSafe(true); return true; @@ -133,9 +150,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } // Ignore objects detected after stop_line if not in STOP state - if ( - state_ != State::STOP && - planning_utils::isOverLine(original_path, self_pose, stop_point->second)) { + const double dist_from_ego_to_stop = calcSignedArcLength( + original_path.points, self_pose.position, current_seg_idx, stop_pose.position, + stop_line_seg_idx); + if (state_ != State::STOP && dist_from_ego_to_stop < 0.0) { setSafe(true); return true; } @@ -153,7 +171,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Insert stop point state_ = State::STOP; - planning_utils::insertStopPoint(stop_pose.position, *path); + planning_utils::insertStopPoint(modified_stop_pose.position, modified_stop_line_seg_idx, *path); // For virtual wall debug_data_.stop_poses.push_back(stop_point->second); diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp index 27b96148d8747..ad8b526d9ab42 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp @@ -52,11 +52,14 @@ void NoStoppingAreaModuleManager::launchNewModules( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose)) { // Use lanelet_id to unregister module when the route is changed - const auto module_id = m.first->id(); + const int64_t module_id = m.first->id(); + const int64_t lane_id = m.second.id(); + if (!isModuleRegistered(module_id)) { // assign 1 no stopping area for each module registerModule(std::make_shared( - module_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), clock_)); + module_id, lane_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), + clock_)); generateUUID(module_id); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 5a0f83cac7b78..b753fd36b8b5d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -40,10 +40,12 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; NoStoppingAreaModule::NoStoppingAreaModule( - const int64_t module_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param) { @@ -128,7 +130,7 @@ bool NoStoppingAreaModule::modifyPathVelocity( return true; } const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line.value(), planner_param_.stop_margin, + original_path, stop_line.value(), lane_id_, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { setSafe(true); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 75404879f331d..58cc2d2c6f813 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -61,8 +61,8 @@ PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0) } bool buildDetectionAreaPolygon( - Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & pose, - const PlannerParam & param) + Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const PlannerParam & param) { const auto & p = param; DetectionRange da_range; @@ -76,7 +76,7 @@ bool buildDetectionAreaPolygon( da_range.max_lateral_distance = p.detection_area.max_lateral_distance; slices.clear(); return planning_utils::createDetectionAreaPolygons( - slices, path, pose, da_range, p.pedestrian_vel); + slices, path, target_pose, target_seg_idx, da_range, p.pedestrian_vel); } void calcSlowDownPointsForPossibleCollision( diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index c4df98bb9a0b0..c9c062110c1d1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -122,8 +122,9 @@ bool OcclusionSpotModule::modifyPathVelocity( } } DEBUG_PRINT(show_time, "apply velocity [ms]: ", stop_watch_.toc("processing_time", true)); + const size_t ego_seg_idx = findEgoSegmentIndex(predicted_path.points); if (!utils::buildDetectionAreaPolygon( - debug_data_.detection_area_polygons, predicted_path, ego_pose, param_)) { + debug_data_.detection_area_polygons, predicted_path, ego_pose, ego_seg_idx, param_)) { return true; // path point is not enough } DEBUG_PRINT(show_time, "generate poly[ms]: ", stop_watch_.toc("processing_time", true)); diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index d398bb42fdac4..cf8c78d9e8930 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -156,8 +156,9 @@ Polygons2d RunOutModule::createDetectionAreaPolygon(const PathWithLaneId & smoot da_range.min_lateral_distance = p.vehicle_param.width / 2.0; da_range.max_lateral_distance = obstacle_vel_mps * p.dynamic_obstacle.max_prediction_time; Polygons2d detection_area_poly; + const size_t ego_seg_idx = findEgoSegmentIndex(smoothed_path.points); planning_utils::createDetectionAreaPolygons( - detection_area_poly, smoothed_path, planner_data_->current_pose.pose, da_range, + detection_area_poly, smoothed_path, planner_data_->current_pose.pose, ego_seg_idx, da_range, p.dynamic_obstacle.max_vel_kmph / 3.6); for (const auto & poly : detection_area_poly) { diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index 3cd08f1f5b6e2..3f8a098f4fe1d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -23,184 +24,18 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -namespace -{ -double calcYawFromPoints( - const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back) -{ - return std::atan2(p_back.y - p_front.y, p_back.x - p_front.x); -} - -geometry_msgs::msg::Pose calcInterpolatedPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithOffset & offset_segment) -{ - // Get segment points - const auto & p_front = path.points.at(offset_segment.index).point.pose.position; - const auto & p_back = path.points.at(offset_segment.index + 1).point.pose.position; - - // To Eigen point - const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); - const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); - - // Calculate interpolation ratio - const auto interpolate_ratio = offset_segment.offset / (p_eigen_back - p_eigen_front).norm(); - - // Add offset to front point - const auto interpolated_point_2d = - p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); - const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); - - // Calculate orientation so that X-axis would be along the trajectory - tf2::Quaternion quat; - quat.setRPY(0, 0, calcYawFromPoints(p_front, p_back)); - - // To Pose - geometry_msgs::msg::Pose interpolated_pose; - interpolated_pose.position.x = interpolated_point_2d.x(); - interpolated_pose.position.y = interpolated_point_2d.y(); - interpolated_pose.position.z = interpolated_z; - interpolated_pose.orientation = tf2::toMsg(quat); - - return interpolated_pose; -} - -boost::optional findBackwardOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, - const double offset_length) -{ - double sum_length = 0.0; - const auto start = static_cast(base_idx) - 1; - for (std::int32_t i = start; i >= 0; --i) { - const auto p_front = to_bg2d(path.points.at(i).point.pose.position); - const auto p_back = to_bg2d(path.points.at(i + 1).point.pose.position); - - sum_length += bg::distance(p_front, p_back); - - // If it's over offset point, return front index and remain offset length - if (sum_length >= offset_length) { - const auto k = static_cast(i); - return StopLineModule::SegmentIndexWithOffset{k, sum_length - offset_length}; - } - } - - // No enough path length - return {}; -} - -} // namespace - StopLineModule::StopLineModule( const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - module_id_(module_id), - stop_line_(stop_line), lane_id_(lane_id), + stop_line_(stop_line), state_(State::APPROACH) { planner_param_ = planner_param; } -boost::optional StopLineModule::findCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const SearchRangeIndex & search_index) -{ - const size_t min_search_index = std::max(static_cast(0), search_index.min_idx); - const size_t max_search_index = std::min(search_index.max_idx, path.points.size() - 1); - - // for creating debug marker - if (planner_param_.show_stopline_collision_check) { - debug_data_.search_stopline = stop_line; - for (size_t i = min_search_index; i < max_search_index; ++i) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - debug_data_.search_segments.push_back(path_segment); - } - } - - for (size_t i = min_search_index; i < max_search_index; ++i) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - - // Find intersection - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - std::vector collision_points; - bg::intersection(stop_line, path_segment, collision_points); - - // Ignore if no collision found - if (collision_points.empty()) { - continue; - } - - // Select first collision - const auto & collision_point = collision_points.at(0); - - return StopLineModule::SegmentIndexWithPoint2d{i, collision_point}; - } - - return {}; -} - -boost::optional StopLineModule::findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPoint2d & collision) -{ - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto base_backward_length = planner_param_.stop_margin + base_link2front; - - const auto & p_back = to_bg2d(path.points.at(collision.index + 1).point.pose.position); - - return findBackwardOffsetSegment( - path, collision.index + 1, base_backward_length + bg::distance(p_back, collision.point)); -} - -boost::optional StopLineModule::calcStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const boost::optional & offset_segment) -{ - // If no stop point found due to out of range, use front point on path - if (!offset_segment) { - return StopLineModule::SegmentIndexWithPose{0, path.points.front().point.pose}; - } - - return StopLineModule::SegmentIndexWithPose{ - offset_segment->index, calcInterpolatedPose(path, *offset_segment)}; -} - -autoware_auto_planning_msgs::msg::PathWithLaneId StopLineModule::insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPose & stop_pose_with_index, - tier4_planning_msgs::msg::StopReason * stop_reason) -{ - auto modified_path = path; - - // Insert stop pose to between segment start and end - size_t insert_index = static_cast(stop_pose_with_index.index + 1); - auto stop_point_with_lane_id = modified_path.points.at(insert_index); - stop_point_with_lane_id.point.pose = stop_pose_with_index.pose; - stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - - // Update first stop index - first_stop_path_point_index_ = static_cast(insert_index); - debug_data_.stop_pose = stop_point_with_lane_id.point.pose; - - // Insert stop point or replace with zero velocity - planning_utils::insertVelocity(modified_path, stop_point_with_lane_id, 0.0, insert_index); - - // Get stop point and stop factor - { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_point_with_lane_id.point.pose; - stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); - planning_utils::appendStopReason(stop_factor, stop_reason); - } - - return modified_path; -} - bool StopLineModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason) @@ -215,49 +50,52 @@ bool StopLineModule::modifyPathVelocity( const LineString2d stop_line = planning_utils::extendLine( stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); - const auto & current_position = planner_data_->current_pose.pose.position; - const PointWithSearchRangeIndex src_point_with_search_range_index = - planning_utils::findFirstNearSearchRangeIndex(path->points, current_position); - SearchRangeIndex dst_search_range = - planning_utils::getPathIndexRangeIncludeLaneId(*path, lane_id_); - - // extend following and previous search range to avoid no collision - if (dst_search_range.max_idx < path->points.size() - 1) dst_search_range.max_idx++; - if (dst_search_range.min_idx > 0) dst_search_range.min_idx--; - // Find collision - const auto collision = findCollision(*path, stop_line, dst_search_range); + // Calculate stop pose and insert index + const auto stop_point = arc_lane_utils::createTargetPoint( + *path, stop_line, lane_id_, planner_param_.stop_margin, + planner_data_->vehicle_info_.max_longitudinal_offset_m); // If no collision found, do nothing - if (!collision) { + if (!stop_point) { RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision"); return true; } - const double center_line_z = (stop_line_[0].z() + stop_line_[1].z()) / 2.0; - const auto stop_line_position = planning_utils::toRosPoint(collision->point, center_line_z); - - // Find offset segment - const auto offset_segment = findOffsetSegment(*path, *collision); - // Calculate stop pose and insert index - const auto stop_pose_with_index = calcStopPose(*path, offset_segment); + const auto stop_point_idx = stop_point->first; + auto stop_pose = stop_point->second; + stop_pose.position.z = (stop_line_[0].z() + stop_line_[1].z()) / 2.0; - const PointWithSearchRangeIndex dst_point_with_search_range_index = { - stop_line_position, dst_search_range}; - const double stop_line_margin = base_link2front + planner_param_.stop_margin; /** * @brief : calculate signed arc length consider stop margin from stop line * * |----------------------------| * s---ego----------x--|--------g */ + const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, stop_pose.position, stop_point_idx); + const double stop_line_margin = base_link2front + planner_param_.stop_margin; + const size_t current_seg_idx = findEgoSegmentIndex(path->points); const double signed_arc_dist_to_stop_point = - planning_utils::calcSignedArcLengthWithSearchIndex( - path->points, src_point_with_search_range_index, dst_point_with_search_range_index) - + motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, current_seg_idx, stop_pose.position, + stop_line_seg_idx) - stop_line_margin; if (state_ == State::APPROACH) { // Insert stop pose - *path = insertStopPose(*path, *stop_pose_with_index, stop_reason); + planning_utils::insertStopPoint(stop_pose.position, stop_line_seg_idx, *path); + + // Update first stop index + first_stop_path_point_index_ = static_cast(stop_point_idx); + debug_data_.stop_pose = stop_pose; + + // Get stop point and stop factor + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); + planning_utils::appendStopReason(stop_factor, stop_reason); + } // Move to stopped state if stopped if ( diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index d00cd239b8c71..c5fb9956ff18b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -108,11 +108,12 @@ void TrafficLightModuleManager::launchNewModules( } // Use lanelet_id to unregister module when the route is changed - const auto module_id = traffic_light_reg_elem.second.id(); + const auto lane_id = traffic_light_reg_elem.second.id(); + const auto module_id = lane_id; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, - logger_.get_child("traffic_light_module"), clock_)); + module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, + planner_param_, logger_.get_child("traffic_light_module"), clock_)); generateUUID(module_id); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index 516704d47be22..ed5ba00e18899 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -187,10 +187,12 @@ autoware_auto_perception_msgs::msg::LookingTrafficSignal initializeTrafficSignal } // namespace TrafficLightModule::TrafficLightModule( - const int64_t module_id, const lanelet::TrafficLight & traffic_light_reg_elem, - lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const int64_t lane_id, + const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, + const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), state_(State::APPROACH), diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index f509e8d5e0133..f8ae83dc6f9a7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -50,10 +50,11 @@ void VirtualTrafficLightModuleManager::launchNewModules( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose)) { // Use lanelet_id to unregister module when the route is changed - const auto module_id = m.second.id(); + const auto lane_id = m.second.id(); + const auto module_id = lane_id; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *m.first, m.second, planner_param_, + module_id, lane_id, *m.first, m.second, planner_param_, logger_.get_child("virtual_traffic_light_module"), clock_)); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index f8bad074bc1ab..af88095a97085 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -76,7 +76,8 @@ std::vector toAutowarePoints( return output; } -tier4_autoware_utils::LineString2d to_2d(const tier4_autoware_utils::LineString3d & line_string) +[[maybe_unused]] tier4_autoware_utils::LineString2d to_2d( + const tier4_autoware_utils::LineString3d & line_string) { tier4_autoware_utils::LineString2d output; for (const auto & p : line_string) { @@ -99,52 +100,45 @@ geometry_msgs::msg::Pose calcHeadPose( return tier4_autoware_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); } -template -boost::optional findCollision( - const T & points, const tier4_autoware_utils::LineString3d & line) +geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point3d & p) { - for (size_t i = 0; i < points.size() - 1; ++i) { - // Create path segment - const auto & p_front = tier4_autoware_utils::getPoint(points.at(i)); - const auto & p_back = tier4_autoware_utils::getPoint(points.at(i + 1)); - const tier4_autoware_utils::LineString2d path_segment{ - {p_front.x, p_front.y}, {p_back.x, p_back.y}}; - - // Find intersection - std::vector collision_points; - bg::intersection(to_2d(line), path_segment, collision_points); - - // Ignore if no collision found - if (collision_points.empty()) { - continue; - } - - // Select first collision if found - const auto collision_point_2d = collision_points.front(); + geometry_msgs::msg::Point geom_p; + geom_p.x = p.x(); + geom_p.y = p.y(); - // Calculate interpolation ratio - const auto interpolate_ratio = - calcDistance2d(p_front, toMsg(collision_point_2d.to_3d(0))) / calcDistance2d(p_front, p_back); - - // Interpolate z - const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); - - // To point - const auto collision_point = tier4_autoware_utils::createPoint( - collision_point_2d.x(), collision_point_2d.y(), interpolated_z); + return geom_p; +} - return SegmentIndexWithPoint{i, collision_point}; +template +boost::optional findLastCollisionBeforeEndLine( + const T & points, const tier4_autoware_utils::LineString3d & target_line, + const size_t end_line_idx) +{ + const auto target_line_p1 = convertToGeomPoint(target_line.at(0)); + const auto target_line_p2 = convertToGeomPoint(target_line.at(1)); + + for (size_t i = end_line_idx; 0 < i; + --i) { // NOTE: size_t can be used since it will not be negative. + const auto & p1 = tier4_autoware_utils::getPoint(points.at(i)); + const auto & p2 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto collision_point = + arc_lane_utils::checkCollision(p1, p2, target_line_p1, target_line_p2); + + if (collision_point) { + return SegmentIndexWithPoint{i, collision_point.get()}; + } } return {}; } template -boost::optional findCollision( - const T & points, const std::vector & lines) +boost::optional findLastCollisionBeforeEndLine( + const T & points, const std::vector & lines, + const size_t end_line_idx) { for (const auto & line : lines) { - const auto collision = findCollision(points, line); + const auto collision = findLastCollisionBeforeEndLine(points, line, end_line_idx); if (collision) { return collision; } @@ -190,11 +184,12 @@ boost::optional insertStopVelocityAtCollision( } // namespace VirtualTrafficLightModule::VirtualTrafficLightModule( - const int64_t module_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, - lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, + const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - module_id_(module_id), + lane_id_(lane_id), reg_elem_(reg_elem), lane_(lane), planner_param_(planner_param) @@ -264,8 +259,17 @@ bool VirtualTrafficLightModule::modifyPathVelocity( planner_data_->current_pose.pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); module_data_.path = *path; + // Calculate path index of end line + // NOTE: In order to deal with u-turn or self-crossing path, only start/stop lines before the end + // line are used when whether the ego is before/after the start/stop/end lines is calculated. + const auto opt_end_line_idx = getPathIndexOfFirstEndLine(); + if (!opt_end_line_idx) { + return true; + } + const size_t end_line_idx = opt_end_line_idx.get(); + // Do nothing if vehicle is before start line - if (isBeforeStartLine()) { + if (isBeforeStartLine(end_line_idx)) { RCLCPP_DEBUG(logger_, "before start_line"); state_ = State::NONE; updateInfrastructureCommand(); @@ -273,7 +277,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity( } // Do nothing if vehicle is after any end line - if (isAfterAnyEndLine() || state_ == State::FINALIZED) { + if (isAfterAnyEndLine(end_line_idx) || state_ == State::FINALIZED) { RCLCPP_DEBUG(logger_, "after end_line"); state_ = State::FINALIZED; updateInfrastructureCommand(); @@ -295,7 +299,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity( // Stop at stop_line if no message received if (!virtual_traffic_light_state) { RCLCPP_DEBUG(logger_, "no message received"); - insertStopVelocityAtStopLine(path, stop_reason); + insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); updateInfrastructureCommand(); return true; } @@ -303,16 +307,16 @@ bool VirtualTrafficLightModule::modifyPathVelocity( // Stop at stop_line if no right is given if (!hasRightOfWay(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "no right is given"); - insertStopVelocityAtStopLine(path, stop_reason); + insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); updateInfrastructureCommand(); return true; } // Stop at stop_line if state is timeout before stop_line - if (isBeforeStopLine()) { + if (isBeforeStopLine(end_line_idx)) { if (isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout before stop line"); - insertStopVelocityAtStopLine(path, stop_reason); + insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); } updateInfrastructureCommand(); @@ -326,7 +330,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity( if ( planner_param_.check_timeout_after_stop_line && isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout after stop line"); - insertStopVelocityAtStopLine(path, stop_reason); + insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); updateInfrastructureCommand(); return true; } @@ -334,9 +338,9 @@ bool VirtualTrafficLightModule::modifyPathVelocity( // Stop at stop_line if finalization isn't completed if (!virtual_traffic_light_state->is_finalized) { RCLCPP_DEBUG(logger_, "finalization isn't completed"); - insertStopVelocityAtEndLine(path, stop_reason); + insertStopVelocityAtEndLine(path, stop_reason, end_line_idx); - if (isNearAnyEndLine() && planner_data_->isVehicleStopped()) { + if (isNearAnyEndLine(end_line_idx) && planner_data_->isVehicleStopped()) { state_ = State::FINALIZING; } } @@ -361,79 +365,128 @@ void VirtualTrafficLightModule::setStopReason( planning_utils::appendStopReason(stop_factor, stop_reason); } -bool VirtualTrafficLightModule::isBeforeStartLine() +boost::optional VirtualTrafficLightModule::getPathIndexOfFirstEndLine() { - const auto collision = findCollision(module_data_.path.points, map_data_.start_line); + boost::optional min_seg_idx; + for (const auto & end_line : map_data_.end_lines) { + geometry_msgs::msg::Point end_line_p1; + end_line_p1.x = end_line.front().x(); + end_line_p1.y = end_line.front().y(); + + geometry_msgs::msg::Point end_line_p2; + end_line_p2.x = end_line.back().x(); + end_line_p2.y = end_line.back().y(); + + const auto collision = + arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, lane_id_); + if (!collision) { + continue; + } + + const size_t collision_seg_idx = collision->first; + + if (!min_seg_idx || collision_seg_idx < min_seg_idx.get()) { + min_seg_idx = + collision_seg_idx + 1; // NOTE: In order that min_seg_idx will be after the end line + } + } + + return min_seg_idx; +} + +bool VirtualTrafficLightModule::isBeforeStartLine(const size_t end_line_idx) +{ + const auto collision = + findLastCollisionBeforeEndLine(module_data_.path.points, map_data_.start_line, end_line_idx); // Since the module is registered, a collision should be detected usually. // Therefore if no collision found, vehicle's path is fully after the line. if (!collision) { return false; } + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + module_data_.path.points, collision->point, collision->index); - const double max_dist = std::numeric_limits::max(); + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose, collision->point, max_dist, - planner_param_.max_yaw_deviation_rad); + module_data_.path.points, ego_pose.position, ego_seg_idx, + collision->point, collision_seg_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; - return *signed_arc_length > 0; + return signed_arc_length > 0; } -bool VirtualTrafficLightModule::isBeforeStopLine() +bool VirtualTrafficLightModule::isBeforeStopLine(const size_t end_line_idx) { - const auto collision = findCollision(module_data_.path.points, *map_data_.stop_line); + const auto collision = + findLastCollisionBeforeEndLine(module_data_.path.points, *map_data_.stop_line, end_line_idx); // Since the module is registered, a collision should be detected usually. // Therefore if no collision found, vehicle's path is fully after the line. if (!collision) { return false; } + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + module_data_.path.points, collision->point, collision->index); - const double max_dist = std::numeric_limits::max(); + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose, collision->point, max_dist, - planner_param_.max_yaw_deviation_rad); + module_data_.path.points, ego_pose.position, ego_seg_idx, + collision->point, collision_seg_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; - return *signed_arc_length > -planner_param_.dead_line_margin; + return signed_arc_length > -planner_param_.dead_line_margin; } -bool VirtualTrafficLightModule::isAfterAnyEndLine() +bool VirtualTrafficLightModule::isAfterAnyEndLine(const size_t end_line_idx) { // Assume stop line is before end lines - if (isBeforeStopLine()) { + if (isBeforeStopLine(end_line_idx)) { return false; } - const auto collision = findCollision(module_data_.path.points, map_data_.end_lines); + const auto collision = + findLastCollisionBeforeEndLine(module_data_.path.points, map_data_.end_lines, end_line_idx); // If the goal is set before the end line, collision will not be detected. // Therefore if there is no collision, the ego vehicle is assumed to be before the end line. if (!collision) { return false; } + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + module_data_.path.points, collision->point, collision->index); - const double max_dist = std::numeric_limits::max(); + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose, collision->point, max_dist, - planner_param_.max_yaw_deviation_rad); + module_data_.path.points, ego_pose.position, ego_seg_idx, + collision->point, collision_seg_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; - return *signed_arc_length < -planner_param_.dead_line_margin; + return signed_arc_length < -planner_param_.dead_line_margin; } -bool VirtualTrafficLightModule::isNearAnyEndLine() +bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) { - const auto collision = findCollision(module_data_.path.points, map_data_.end_lines); + const auto collision = + findLastCollisionBeforeEndLine(module_data_.path.points, map_data_.end_lines, end_line_idx); if (!collision) { return false; } + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + module_data_.path.points, collision->point, collision->index); - const double max_dist = std::numeric_limits::max(); + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose, collision->point, max_dist, - planner_param_.max_yaw_deviation_rad); + module_data_.path.points, ego_pose.position, ego_seg_idx, + collision->point, collision_seg_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; - return std::abs(*signed_arc_length) < planner_param_.near_line_distance; + return std::abs(signed_arc_length) < planner_param_.near_line_distance; } boost::optional @@ -473,9 +526,10 @@ bool VirtualTrafficLightModule::hasRightOfWay( void VirtualTrafficLightModule::insertStopVelocityAtStopLine( autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) + tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) { - const auto collision = findCollision(path->points, *map_data_.stop_line); + const auto collision = + findLastCollisionBeforeEndLine(path->points, *map_data_.stop_line, end_line_idx); const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m; geometry_msgs::msg::Pose stop_pose{}; @@ -483,19 +537,25 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( insertStopVelocityFromStart(path); stop_pose = planner_data_->current_pose.pose; } else { - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(path->points); + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, collision->point, collision->index); + const auto stop_distance = - motion_utils::calcSignedArcLength(path->points, ego_pos, collision.get().point) + offset; + motion_utils::calcSignedArcLength( + path->points, ego_pose.position, ego_seg_idx, collision.get().point, collision_seg_idx) + + offset; const auto is_stopped = planner_data_->isVehicleStopped(); if (stop_distance < planner_param_.hold_stop_margin_distance && is_stopped) { SegmentIndexWithPoint new_collision; const auto ego_pos_on_path = - motion_utils::calcLongitudinalOffsetPoint(path->points, ego_pos, 0.0); + motion_utils::calcLongitudinalOffsetPoint(path->points, ego_pose.position, 0.0); if (ego_pos_on_path) { new_collision.point = ego_pos_on_path.get(); - new_collision.index = motion_utils::findNearestSegmentIndex(path->points, ego_pos); + new_collision.index = ego_seg_idx; insertStopVelocityAtCollision(new_collision, 0.0, path); } @@ -526,14 +586,15 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( void VirtualTrafficLightModule::insertStopVelocityAtEndLine( autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) + tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) { - const auto collision = findCollision(path->points, map_data_.end_lines); + const auto collision = + findLastCollisionBeforeEndLine(path->points, map_data_.end_lines, end_line_idx); geometry_msgs::msg::Pose stop_pose{}; if (!collision) { // No enough length - if (isBeforeStopLine()) { + if (isBeforeStopLine(end_line_idx)) { return; } diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 0256c4ba09d49..f3e9c0591b7e2 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -47,8 +47,9 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con } bool createDetectionAreaPolygons( - Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & pose, - const DetectionRange & da_range, const double obstacle_vel_mps, const double min_velocity) + Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, + const double min_velocity) { /** * @brief relationships for interpolated polygon @@ -67,28 +68,27 @@ bool createDetectionAreaPolygons( const size_t max_index = static_cast(path.points.size() - 1); //! avoid bug with same point polygon const double eps = 1e-3; - auto nearest_idx = findNearestIndex(path.points, pose.position); + auto nearest_idx = + calcPointIndexFromSegmentIndex(path.points, target_pose.position, target_seg_idx); if (max_index == nearest_idx) return false; // case of path point is not enough size auto p0 = path.points.at(nearest_idx).point; auto first_idx = nearest_idx + 1; // use ego point as start point if same point as ego is not in the path const auto dist_to_nearest = - std::fabs(calcSignedArcLength(path.points, pose.position, nearest_idx)); + std::fabs(calcSignedArcLength(path.points, target_pose.position, target_seg_idx, nearest_idx)); if (dist_to_nearest > eps) { - const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(path.points, pose.position); - // interpolate ego point const auto & pp = path.points; - const double ds = calcDistance2d(pp.at(nearest_seg_idx), pp.at(nearest_seg_idx + 1)); - const double dist_to_nearest_seg = - calcSignedArcLength(path.points, nearest_seg_idx, pose.position); - const double ratio = dist_to_nearest_seg / ds; + const double ds = calcDistance2d(pp.at(target_seg_idx), pp.at(target_seg_idx + 1)); + const double dist_to_target_seg = + calcSignedArcLength(path.points, target_seg_idx, target_pose.position, target_seg_idx); + const double ratio = dist_to_target_seg / ds; p0 = getLerpPathPointWithLaneId( - pp.at(nearest_seg_idx).point, pp.at(nearest_seg_idx + 1).point, ratio); + pp.at(target_seg_idx).point, pp.at(target_seg_idx + 1).point, ratio); // new first index should be ahead of p0 - first_idx = nearest_seg_idx + 1; + first_idx = target_seg_idx + 1; } double ttc = 0.0; @@ -523,15 +523,8 @@ LineString2d extendLine( boost::optional getNearestLaneId( const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, - const geometry_msgs::msg::Pose & current_pose, boost::optional & nearest_segment_idx) + const geometry_msgs::msg::Pose & current_pose) { - nearest_segment_idx = motion_utils::findNearestSegmentIndex( - path.points, current_pose, std::numeric_limits::max(), M_PI_2); - - if (!nearest_segment_idx) { - return boost::none; - } - lanelet::ConstLanelets lanes; const auto lane_ids = getSortedLaneIdsFromPath(path); for (const auto & lane_id : lane_ids) { @@ -549,9 +542,7 @@ std::vector getLaneletsOnPath( const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) { - boost::optional nearest_segment_idx; - const auto nearest_lane_id = - getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx); + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { @@ -614,6 +605,7 @@ std::vector getSubsequentLaneIdsSetOnPath( return subsequent_lane_ids; } +// TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex bool isOverLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, @@ -630,6 +622,7 @@ boost::optional insertDecelPoint( { // TODO(tanaka): consider proper overlap threshold for inserting decel point const double overlap_threshold = 5e-2; + // TODO(murooka): remove this function for u-turn and crossing-path const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point); const auto insert_idx = motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold); @@ -646,6 +639,7 @@ boost::optional insertDecelPoint( return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); } +// TODO(murooka): remove this function for u-turn and crossing-path boost::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output) { @@ -662,5 +656,21 @@ boost::optional insertStopPoint( return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); } + +boost::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output) +{ + const auto insert_idx = motion_utils::insertTargetPoint(stop_seg_idx, stop_point, output.points); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.get(); i < output.points.size(); ++i) { + output.points.at(i).point.longitudinal_velocity_mps = 0.0; + } + + return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); +} } // namespace planning_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp index d39b4ccb40d1b..ac5a5c82d6730 100644 --- a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp @@ -26,6 +26,18 @@ using LineString2d = behavior_velocity_planner::LineString2d; using Point2d = behavior_velocity_planner::Point2d; namespace arc_lane_utils = behavior_velocity_planner::arc_lane_utils; +namespace +{ +geometry_msgs::msg::Point createPoint(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + p.z = z; + return p; +} +} // namespace + TEST(findCollisionSegment, nominal) { /** @@ -36,13 +48,17 @@ TEST(findCollisionSegment, nominal) * **/ auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + for (auto & point : path.points) { + point.lane_ids.push_back(100); + } + LineString2d stop_line; stop_line.emplace_back(Point2d(3.5, 3.0)); stop_line.emplace_back(Point2d(3.5, -3.0)); - auto segment = arc_lane_utils::findCollisionSegment(path, stop_line); + auto segment = arc_lane_utils::findCollisionSegment(path, stop_line, 100); EXPECT_EQ(segment->first, static_cast(3)); - EXPECT_DOUBLE_EQ(segment->second.x(), 3.5); - EXPECT_DOUBLE_EQ(segment->second.y(), 0.0); + EXPECT_DOUBLE_EQ(segment->second.x, 3.5); + EXPECT_DOUBLE_EQ(segment->second.y, 0.0); } TEST(findOffsetSegment, case_forward_offset_segment) @@ -54,9 +70,8 @@ TEST(findOffsetSegment, case_forward_offset_segment) * 0 1 2 3 4 5 **/ const int collision_segment_idx = 3; - Point2d collision_point(3.5, 0.0); - const PathIndexWithPoint2d & collision_segment = - std::make_pair(collision_segment_idx, collision_point); + const auto collision_point = createPoint(3.5, 0.0, 0.0); + const auto & collision_segment = std::make_pair(collision_segment_idx, collision_point); // nominal { double offset_length = 1.0; @@ -84,9 +99,8 @@ TEST(findOffsetSegment, case_backward_offset_segment) * 0 1 2 3 4 5 **/ const int collision_segment_idx = 3; - Point2d collision_point(3.5, 0.0); - const PathIndexWithPoint2d & collision_segment = - std::make_pair(collision_segment_idx, collision_point); + const auto collision_point = createPoint(3.5, 0.0, 0.0); + const auto & collision_segment = std::make_pair(collision_segment_idx, collision_point); // nominal { double offset_length = -1.0; @@ -104,3 +118,127 @@ TEST(findOffsetSegment, case_backward_offset_segment) EXPECT_FALSE(offset_segment); } } + +TEST(checkCollision, various_cases) +{ + using behavior_velocity_planner::arc_lane_utils::checkCollision; + constexpr double epsilon = 1e-6; + + { // normal case with collision + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // normal case without collision + const auto p1 = createPoint(1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, boost::none); + } + + { // normal case without collision + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, boost::none); + } + + { // line and point + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, boost::none); + } + + { // point and line + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(-1.0, 0.0, 0.0); + const auto p4 = createPoint(2.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, boost::none); + } + + { // collision with edges + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } +}