diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt index 57eb826927663..3aff4a524ffdd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt @@ -6,9 +6,16 @@ autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/manager.cpp - src/scene.cpp + DIRECTORY src ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index c707e2fbfdbde..8669aa16b7252 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -33,6 +33,7 @@ tf2_geometry_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index d6a1747259962..ad3ba986733e1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -14,32 +14,28 @@ #include "scene.hpp" +#include "utils.hpp" + #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else #include -#endif #include -#include #include #include #include namespace autoware::behavior_velocity_planner { -namespace bg = boost::geometry; using autoware::motion_utils::calcLongitudinalOffsetPose; using autoware::motion_utils::calcSignedArcLength; DetectionAreaModule::DetectionAreaModule( 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 PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), @@ -51,13 +47,6 @@ DetectionAreaModule::DetectionAreaModule( velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } -LineString2d DetectionAreaModule::getStopLineGeometry2d() const -{ - const lanelet::ConstLineString3d stop_line = detection_area_reg_elem_.stopLine(); - return planning_utils::extendLine( - stop_line[0], stop_line[1], planner_data_->stop_line_extend_length); -} - bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { // Store original path @@ -69,14 +58,16 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * *stop_reason = planning_utils::initializeStopReason(StopReason::DETECTION_AREA); // Find obstacles in detection area - const auto obstacle_points = getObstaclePoints(); + const auto obstacle_points = detection_area::get_obstacle_points( + detection_area_reg_elem_.detectionAreas(), *planner_data_->no_ground_pointcloud); debug_data_.obstacle_points = obstacle_points; if (!obstacle_points.empty()) { last_obstacle_found_time_ = std::make_shared(clock_->now()); } // Get stop line geometry - const auto stop_line = getStopLineGeometry2d(); + const auto stop_line = detection_area::get_stop_line_geometry2d( + detection_area_reg_elem_, planner_data_->stop_line_extend_length); // Get self pose const auto & self_pose = planner_data_->current_odometry->pose; @@ -118,7 +109,8 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * setDistance(stop_dist); // Check state - setSafe(canClearStopState()); + setSafe(detection_area::can_clear_stop_state( + last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time)); if (isActivated()) { state_ = State::GO; last_obstacle_found_time_ = {}; @@ -165,7 +157,14 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Ignore objects if braking distance is not enough if (planner_param_.use_pass_judge_line) { - if (state_ != State::STOP && !hasEnoughBrakingDistance(self_pose, stop_point->second)) { + const auto current_velocity = planner_data_->current_velocity->twist.linear.x; + const double pass_judge_line_distance = planning_utils::calcJudgeLineDistWithAccLimit( + current_velocity, planner_data_->current_acceleration->accel.accel.linear.x, + planner_data_->delay_response_time); + if ( + state_ != State::STOP && + !detection_area::has_enough_braking_distance( + self_pose, stop_point->second, pass_judge_line_distance, current_velocity)) { RCLCPP_WARN_THROTTLE( logger_, *clock_, std::chrono::milliseconds(1000).count(), "[detection_area] vehicle is over stop border"); @@ -206,138 +205,4 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } - -// calc smallest enclosing circle with average O(N) algorithm -// reference: -// https://erickimphotography.com/blog/wp-content/uploads/2018/09/Computational-Geometry-Algorithms-and-Applications-3rd-Ed.pdf -std::pair calcSmallestEnclosingCircle( - const lanelet::ConstPolygon2d & poly) -{ - // The `eps` is used to avoid precision bugs in circle inclusion checks. - // If the value of `eps` is too small, this function doesn't work well. More than 1e-10 is - // recommended. - const double eps = 1e-5; - lanelet::BasicPoint2d center(0.0, 0.0); - double radius_squared = 0.0; - - auto cross = [](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> double { - return p1.x() * p2.y() - p1.y() * p2.x(); - }; - - auto make_circle_3 = [&]( - const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, - const lanelet::BasicPoint2d & p3) -> void { - // reference for circumcenter vector https://en.wikipedia.org/wiki/Circumscribed_circle - const double A = (p2 - p3).squaredNorm(); - const double B = (p3 - p1).squaredNorm(); - const double C = (p1 - p2).squaredNorm(); - const double S = cross(p2 - p1, p3 - p1); - if (std::abs(S) < eps) return; - center = (A * (B + C - A) * p1 + B * (C + A - B) * p2 + C * (A + B - C) * p3) / (4 * S * S); - radius_squared = (center - p1).squaredNorm() + eps; - }; - - auto make_circle_2 = - [&](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> void { - center = (p1 + p2) * 0.5; - radius_squared = (center - p1).squaredNorm() + eps; - }; - - auto in_circle = [&](const lanelet::BasicPoint2d & p) -> bool { - return (center - p).squaredNorm() <= radius_squared; - }; - - // mini disc - for (size_t i = 1; i < poly.size(); i++) { - const auto p1 = poly[i].basicPoint2d(); - if (in_circle(p1)) continue; - - // mini disc with point - const auto p0 = poly[0].basicPoint2d(); - make_circle_2(p0, p1); - for (size_t j = 0; j < i; j++) { - const auto p2 = poly[j].basicPoint2d(); - if (in_circle(p2)) continue; - - // mini disc with two points - make_circle_2(p1, p2); - for (size_t k = 0; k < j; k++) { - const auto p3 = poly[k].basicPoint2d(); - if (in_circle(p3)) continue; - - // mini disc with tree points - make_circle_3(p1, p2, p3); - } - } - } - - return std::make_pair(center, radius_squared); -} - -std::vector DetectionAreaModule::getObstaclePoints() const -{ - std::vector obstacle_points; - - const auto detection_areas = detection_area_reg_elem_.detectionAreas(); - const auto & points = *(planner_data_->no_ground_pointcloud); - - for (const auto & detection_area : detection_areas) { - const auto poly = lanelet::utils::to2D(detection_area); - const auto circle = calcSmallestEnclosingCircle(poly); - for (const auto p : points) { - const double squared_dist = (circle.first.x() - p.x) * (circle.first.x() - p.x) + - (circle.first.y() - p.y) * (circle.first.y() - p.y); - if (squared_dist <= circle.second) { - if (bg::within(Point2d{p.x, p.y}, poly.basicPolygon())) { - obstacle_points.push_back(autoware::universe_utils::createPoint(p.x, p.y, p.z)); - // get all obstacle point becomes high computation cost so skip if any point is found - break; - } - } - } - } - - return obstacle_points; -} - -bool DetectionAreaModule::canClearStopState() const -{ - // vehicle can clear stop state if the obstacle has never appeared in detection area - if (!last_obstacle_found_time_) { - return true; - } - - // vehicle can clear stop state if the certain time has passed since the obstacle disappeared - const auto elapsed_time = clock_->now() - *last_obstacle_found_time_; - if (elapsed_time.seconds() >= planner_param_.state_clear_time) { - return true; - } - - // rollback in simulation mode - if (elapsed_time.seconds() < 0.0) { - return true; - } - - return false; -} - -bool DetectionAreaModule::hasEnoughBrakingDistance( - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const -{ - // get vehicle info and compute pass_judge_line_distance - const auto current_velocity = planner_data_->current_velocity->twist.linear.x; - const double max_acc = planner_data_->max_stop_acceleration_threshold; - const double delay_response_time = planner_data_->delay_response_time; - const double pass_judge_line_distance = - planning_utils::calcJudgeLineDistWithAccLimit(current_velocity, max_acc, delay_response_time); - - // prevent from being judged as not having enough distance when the current velocity is zero - // and the vehicle crosses the stop line - if (current_velocity < 1e-3) { - return true; - } - - return arc_lane_utils::calcSignedDistance(self_pose, line_pose.position) > - pass_judge_line_distance; -} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index efc1c1809302a..d756ea39a7f92 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -63,11 +63,10 @@ class DetectionAreaModule : public SceneModuleInterface double distance_to_judge_over_stop_line; }; -public: DetectionAreaModule( 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 PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -76,15 +75,6 @@ class DetectionAreaModule : public SceneModuleInterface autoware::motion_utils::VirtualWalls createVirtualWalls() override; private: - LineString2d getStopLineGeometry2d() const; - - std::vector getObstaclePoints() const; - - bool canClearStopState() const; - - bool hasEnoughBrakingDistance( - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; - // Lane id int64_t lane_id_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp new file mode 100644 index 0000000000000..6b3bf416e6d99 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp @@ -0,0 +1,165 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utils.hpp" + +#include +#include +#include +#include + +#include +#include + +#include + +namespace +{ +// calc smallest enclosing circle with average O(N) algorithm +// reference: +// https://erickimphotography.com/blog/wp-content/uploads/2018/09/Computational-Geometry-Algorithms-and-Applications-3rd-Ed.pdf +std::pair get_smallest_enclosing_circle( + const lanelet::ConstPolygon2d & poly) +{ + // The `eps` is used to avoid precision bugs in circle inclusion checks. + // If the value of `eps` is too small, this function doesn't work well. More than 1e-10 is + // recommended. + const double eps = 1e-5; + lanelet::BasicPoint2d center(0.0, 0.0); + double radius_squared = 0.0; + + auto cross = [](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> double { + return p1.x() * p2.y() - p1.y() * p2.x(); + }; + + auto make_circle_3 = [&]( + const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, + const lanelet::BasicPoint2d & p3) -> void { + // reference for circumcenter vector https://en.wikipedia.org/wiki/Circumscribed_circle + const double a = (p2 - p3).squaredNorm(); + const double b = (p3 - p1).squaredNorm(); + const double c = (p1 - p2).squaredNorm(); + const double s = cross(p2 - p1, p3 - p1); + if (std::abs(s) < eps) return; + center = (a * (b + c - a) * p1 + b * (c + a - b) * p2 + c * (a + b - c) * p3) / (4 * s * s); + radius_squared = (center - p1).squaredNorm() + eps; + }; + + auto make_circle_2 = + [&](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> void { + center = (p1 + p2) * 0.5; + radius_squared = (center - p1).squaredNorm() + eps; + }; + + auto in_circle = [&](const lanelet::BasicPoint2d & p) -> bool { + return (center - p).squaredNorm() <= radius_squared; + }; + + // mini disc + for (size_t i = 1; i < poly.size(); i++) { + const auto p1 = poly[i].basicPoint2d(); + if (in_circle(p1)) continue; + + // mini disc with point + const auto p0 = poly[0].basicPoint2d(); + make_circle_2(p0, p1); + for (size_t j = 0; j < i; j++) { + const auto p2 = poly[j].basicPoint2d(); + if (in_circle(p2)) continue; + + // mini disc with two points + make_circle_2(p1, p2); + for (size_t k = 0; k < j; k++) { + const auto p3 = poly[k].basicPoint2d(); + if (in_circle(p3)) continue; + + // mini disc with tree points + make_circle_3(p1, p2, p3); + } + } + } + + return std::make_pair(center, radius_squared); +} +} // namespace + +namespace autoware::behavior_velocity_planner::detection_area +{ +universe_utils::LineString2d get_stop_line_geometry2d( + const lanelet::autoware::DetectionArea & detection_area, const double extend_length) +{ + const auto stop_line = detection_area.stopLine(); + return planning_utils::extendLine(stop_line[0], stop_line[1], extend_length); +} + +std::vector get_obstacle_points( + const lanelet::ConstPolygons3d & detection_areas, const pcl::PointCloud & points) +{ + std::vector obstacle_points; + for (const auto & detection_area : detection_areas) { + const auto poly = lanelet::utils::to2D(detection_area); + const auto circle = get_smallest_enclosing_circle(poly); + for (const auto p : points) { + const double squared_dist = (circle.first.x() - p.x) * (circle.first.x() - p.x) + + (circle.first.y() - p.y) * (circle.first.y() - p.y); + if (squared_dist <= circle.second) { + if (boost::geometry::within(Point2d{p.x, p.y}, poly.basicPolygon())) { + obstacle_points.push_back(autoware::universe_utils::createPoint(p.x, p.y, p.z)); + // get all obstacle point becomes high computation cost so skip if any point is found + break; + } + } + } + } + return obstacle_points; +} + +bool can_clear_stop_state( + const std::shared_ptr & last_obstacle_found_time, const rclcpp::Time & now, + const double state_clear_time) +{ + // vehicle can clear stop state if the obstacle has never appeared in detection area + if (!last_obstacle_found_time) { + return true; + } + + // vehicle can clear stop state if the certain time has passed since the obstacle disappeared + const auto elapsed_time = now - *last_obstacle_found_time; + if (elapsed_time.seconds() >= state_clear_time) { + return true; + } + + // rollback in simulation mode + if (elapsed_time.seconds() < 0.0) { + return true; + } + + return false; +} + +bool has_enough_braking_distance( + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double pass_judge_line_distance, const double current_velocity) +{ + // prevent from being judged as not having enough distance when the current velocity is zero + // and the vehicle crosses the stop line + if (current_velocity < 1e-3) { + return true; + } + + return arc_lane_utils::calcSignedDistance(self_pose, line_pose.position) > + pass_judge_line_distance; +} + +} // namespace autoware::behavior_velocity_planner::detection_area diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.hpp new file mode 100644 index 0000000000000..3a0ba0172cbd1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +namespace autoware::behavior_velocity_planner::detection_area +{ + +/// @brief get the extended stop line of the given detection area +/// @param [in] detection_area detection area +/// @param [in] extend_length [m] extension length to add on each edge of the stop line +/// @return extended stop line +universe_utils::LineString2d get_stop_line_geometry2d( + const lanelet::autoware::DetectionArea & detection_area, const double extend_length); + +/// @brief get the obstacle points found inside a detection area +/// @param [in] detection_areas detection area polygons +/// @param [in] points obstacle points +/// @return the first obstacle point found in each detection area +std::vector get_obstacle_points( + const lanelet::ConstPolygons3d & detection_areas, const pcl::PointCloud & points); + +/// @brief return true if the stop state can be cleared +/// @details can be cleared if enough time passed since last detecting an obstacle +/// @param [in] last_obstacle_found_time pointer to the time when an obstacle was last detected +/// @param [in] now current time +/// @param [in] state_clear_time [s] minimum duration since last obstacle detection to clear the +/// stop state +/// @return true if the stop state can be cleared +bool can_clear_stop_state( + const std::shared_ptr & last_obstacle_found_time, const rclcpp::Time & now, + const double state_clear_time); + +/// @brief return true if distance to brake is enough +/// @param self_pose current ego pose +/// @param line_pose stop pose +/// @param pass_judge_line_distance braking distance +/// @param current_velocity current ego velocity +/// @return true if the distance to brake is enough +bool has_enough_braking_distance( + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double pass_judge_line_distance, const double current_velocity); +} // namespace autoware::behavior_velocity_planner::detection_area + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_utils.cpp new file mode 100644 index 0000000000000..9b4dc5193f655 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_utils.cpp @@ -0,0 +1,183 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/utils.hpp" + +#include +#include +#include +#include + +#include +#include + +TEST(TestUtils, getStopLine) +{ + using autoware::behavior_velocity_planner::detection_area::get_stop_line_geometry2d; + lanelet::LineString3d line; + line.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, -1.0)); + line.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 1.0)); + lanelet::Polygons3d detection_areas; + lanelet::Polygon3d area; + area.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, -1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, 1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + detection_areas.push_back(area); + auto detection_area = + lanelet::autoware::DetectionArea::make(lanelet::InvalId, {}, detection_areas, line); + { + const double extend_length = 0.0; + const auto stop_line = get_stop_line_geometry2d(*detection_area, extend_length); + ASSERT_EQ(stop_line.size(), 2UL); + EXPECT_EQ(stop_line[0].x(), line[0].x()); + EXPECT_EQ(stop_line[0].y(), line[0].y()); + EXPECT_EQ(stop_line[1].x(), line[1].x()); + EXPECT_EQ(stop_line[1].y(), line[1].y()); + } + // extended line + for (auto extend_length = -2.0; extend_length < 2.0; extend_length += 0.1) { + const auto stop_line = get_stop_line_geometry2d(*detection_area, extend_length); + ASSERT_EQ(stop_line.size(), 2UL); + EXPECT_EQ(stop_line[0].x(), line[0].x()); + EXPECT_EQ(stop_line[0].y(), line[0].y() - extend_length); + EXPECT_EQ(stop_line[1].x(), line[1].x()); + EXPECT_EQ(stop_line[1].y(), line[1].y() + extend_length); + } +} + +TEST(TestUtils, getObstaclePoints) +{ + using autoware::behavior_velocity_planner::detection_area::get_obstacle_points; + lanelet::ConstPolygons3d detection_areas; + lanelet::Polygon3d area; + area.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, -1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, 1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + detection_areas.push_back(area); + pcl::PointCloud points; + // empty points + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + EXPECT_TRUE(obstacle_points.empty()); + } + // add points outside the detection area + points.emplace_back(0.0, 0.0, 0.0); + points.emplace_back(4.0, 4.0, 0.0); + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + EXPECT_TRUE(obstacle_points.empty()); + } + // add point on the edge of the detection area (will not be found) + points.emplace_back(1.0, 1.0, 0.0); + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + EXPECT_TRUE(obstacle_points.empty()); + } + // add point inside the detection area (will be found) + points.emplace_back(2.0, 0.0, 0.0); + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + ASSERT_EQ(obstacle_points.size(), 1UL); + EXPECT_EQ(obstacle_points[0].x, points[3].x); + EXPECT_EQ(obstacle_points[0].y, points[3].y); + } + // add a detection area that covers all points + lanelet::Polygon3d full_area; + full_area.push_back(lanelet::Point3d(lanelet::InvalId, -10.0, -10.0)); + full_area.push_back(lanelet::Point3d(lanelet::InvalId, -10.0, 10.0)); + full_area.push_back(lanelet::Point3d(lanelet::InvalId, 10.0, 10.0)); + full_area.push_back(lanelet::Point3d(lanelet::InvalId, 10.0, -10.0)); + detection_areas.push_back(full_area); + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + ASSERT_EQ(obstacle_points.size(), 2UL); // only the 1st point found for each area are returned + EXPECT_EQ(obstacle_points[0].x, points[3].x); + EXPECT_EQ(obstacle_points[0].y, points[3].y); + EXPECT_EQ(obstacle_points[1].x, points[0].x); + EXPECT_EQ(obstacle_points[1].y, points[0].y); + } +} + +TEST(TestUtils, canClearStopState) +{ + using autoware::behavior_velocity_planner::detection_area::can_clear_stop_state; + std::shared_ptr last_obstacle_found_time = nullptr; + // can clear if we never found an obstacle + for (auto now_s = 0; now_s <= 10; now_s += 1) { + for (auto now_ns = 0; now_ns <= 1e9; now_ns += 1e8) { + for (double state_clear_time = 0.0; state_clear_time <= 10.0; state_clear_time += 0.1) { + const auto can_clear = can_clear_stop_state( + last_obstacle_found_time, rclcpp::Time(now_s, now_ns, RCL_CLOCK_UNINITIALIZED), + state_clear_time); + EXPECT_TRUE(can_clear); + } + } + } + last_obstacle_found_time = std::make_shared(1.0, 0.0); + const auto state_clear_time = 1.0; + // special case for negative time difference which may occur with simulated time + EXPECT_TRUE(can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(0, 0), state_clear_time)); + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(0, 9 * 1e8), state_clear_time)); + // cannot clear before the time has passed since the obstacle disappeared + EXPECT_FALSE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(1, 1), state_clear_time)); + EXPECT_FALSE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(1, 1e9 - 1), state_clear_time)); + // can clear after the time has passed + EXPECT_TRUE(can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(2, 1), state_clear_time)); + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(100, 0), state_clear_time)); + // negative time + const auto negative_state_clear_time = -1.0; + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(0, 0), negative_state_clear_time)); + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(1, 0), negative_state_clear_time)); + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(2, 0), negative_state_clear_time)); +} + +TEST(TestUtils, hasEnoughBrakingDistance) +{ + using autoware::behavior_velocity_planner::detection_area::has_enough_braking_distance; + // prepare a stop pose 10m away from the self pose + geometry_msgs::msg::Pose self_pose; + self_pose.position.x = 0.0; + self_pose.position.y = 0.0; + geometry_msgs::msg::Pose line_pose; + line_pose.position.x = 10.0; + line_pose.position.y = 0.0; + // can always brake at zero velocity + for (auto pass_judge_line_distance = 0.0; pass_judge_line_distance <= 20.0; + pass_judge_line_distance += 0.1) { + double current_velocity = 0.0; + EXPECT_TRUE(has_enough_braking_distance( + self_pose, line_pose, pass_judge_line_distance, current_velocity)); + } + // if velocity is not zero, can brake if the pass judge line distance is lower than 10m + const double current_velocity = 5.0; + for (auto pass_judge_line_distance = 0.0; pass_judge_line_distance < 10.0; + pass_judge_line_distance += 0.1) { + EXPECT_TRUE(has_enough_braking_distance( + self_pose, line_pose, pass_judge_line_distance, current_velocity)); + } + for (auto pass_judge_line_distance = 10.0; pass_judge_line_distance <= 20.0; + pass_judge_line_distance += 0.1) { + EXPECT_FALSE(has_enough_braking_distance( + self_pose, line_pose, pass_judge_line_distance, current_velocity)); + } +}