Skip to content

Commit

Permalink
test(detection_area): refactor and add unit tests (#9087)
Browse files Browse the repository at this point in the history 
Signed-off-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
@maxime-clem
maxime-clem authored Oct 20, 2024
1 parent ae24913 commit 6ff2121
Show file tree
Hide file tree
Showing 7 changed files with 448 additions and 166 deletions.
13 changes: 10 additions & 3 deletions ...behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
1 change: 1 addition & 0 deletions ...ng/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
<depend>tf2_geometry_msgs</depend>
<depend>visualization_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

Expand Down
169 changes: 17 additions & 152 deletions .../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,32 +14,28 @@

#include "scene.hpp"

#include "utils.hpp"

#include <autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#ifdef ROS_DISTRO_GALACTIC
#include <tf2_eigen/tf2_eigen.h>
#else
#include <tf2_eigen/tf2_eigen.hpp>
#endif

#include <lanelet2_core/geometry/Polygon.h>

#include <algorithm>
#include <memory>
#include <utility>
#include <vector>

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),
Expand All @@ -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
Expand All @@ -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<const rclcpp::Time>(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;
Expand Down Expand Up @@ -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_ = {};
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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<lanelet::BasicPoint2d, double> 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<geometry_msgs::msg::Point> DetectionAreaModule::getObstaclePoints() const
{
std::vector<geometry_msgs::msg::Point> 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
12 changes: 1 addition & 11 deletions .../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -76,15 +75,6 @@ class DetectionAreaModule : public SceneModuleInterface
autoware::motion_utils::VirtualWalls createVirtualWalls() override;

private:
LineString2d getStopLineGeometry2d() const;

std::vector<geometry_msgs::msg::Point> 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_;

Expand Down
Loading

0 comments on commit 6ff2121

Please sign in to comment.