Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(run_out)!: ignore the collision points on crosswalk #5862

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
run_out:
detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points
use_partition_lanelet: true # [-] whether to use partition lanelet map data
suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet:
specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates
stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin
passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_run_out_module/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>behavior_velocity_crosswalk_module</depend>
<depend>behavior_velocity_planner_common</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_run_out_module/src/manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
auto & p = planner_param_.run_out;
p.detection_method = getOrDeclareParameter<std::string>(node, ns + ".detection_method");
p.use_partition_lanelet = getOrDeclareParameter<bool>(node, ns + ".use_partition_lanelet");
p.suppress_on_crosswalk = getOrDeclareParameter<bool>(node, ns + ".suppress_on_crosswalk");
p.specify_decel_jerk = getOrDeclareParameter<bool>(node, ns + ".specify_decel_jerk");
p.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
p.passing_margin = getOrDeclareParameter<double>(node, ns + ".passing_margin");
Expand Down
43 changes: 37 additions & 6 deletions planning/behavior_velocity_run_out_module/src/scene.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check warning on line 1 in planning/behavior_velocity_run_out_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Overall Code Complexity

This module has a mean cyclomatic complexity of 4.18 across 22 functions. The mean complexity threshold is 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -14,6 +14,7 @@

#include "scene.hpp"

#include "behavior_velocity_crosswalk_module/util.hpp"
#include "path_utils.hpp"

#include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
Expand All @@ -23,6 +24,10 @@
#include <tier4_autoware_utils/geometry/geometry.hpp>

#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <lanelet2_core/geometry/Point.h>
#include <lanelet2_core/geometry/Polygon.h>

#include <algorithm>
#include <limits>
Expand Down Expand Up @@ -110,8 +115,14 @@
elapsed_obstacle_creation.count() / 1000.0);

// detect collision with dynamic obstacles using velocity planning of ego
const auto crosswalk_lanelets = planner_param_.run_out.suppress_on_crosswalk
? getCrosswalksOnPath(
planner_data_->current_odometry->pose, *path,
planner_data_->route_handler_->getLaneletMapPtr(),
planner_data_->route_handler_->getOverallGraphPtr())
: std::vector<std::pair<int64_t, lanelet::ConstLanelet>>();
const auto dynamic_obstacle =
detectCollision(partition_excluded_obstacles, extended_smoothed_path);
detectCollision(partition_excluded_obstacles, extended_smoothed_path, crosswalk_lanelets);

// record time for collision check
const auto t_collision_check = std::chrono::system_clock::now();
Expand Down Expand Up @@ -155,7 +166,8 @@
}

std::optional<DynamicObstacle> RunOutModule::detectCollision(
const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path)
const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path,
const std::vector<std::pair<int64_t, lanelet::ConstLanelet>> & crosswalk_lanelets)
{
if (path.points.size() < 2) {
RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points.");
Expand Down Expand Up @@ -189,7 +201,7 @@
debug_ptr_->pushTravelTimeTexts(travel_time, p2.pose, /* lateral_offset */ 3.0);

auto obstacles_collision =
checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time);
checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, crosswalk_lanelets);
if (obstacles_collision.empty()) {
continue;
}
Expand Down Expand Up @@ -309,7 +321,8 @@

std::vector<DynamicObstacle> RunOutModule::checkCollisionWithObstacles(
const std::vector<DynamicObstacle> & dynamic_obstacles,
std::vector<geometry_msgs::msg::Point> poly, const float travel_time) const
std::vector<geometry_msgs::msg::Point> poly, const float travel_time,
const std::vector<std::pair<int64_t, lanelet::ConstLanelet>> & crosswalk_lanelets) const
{
const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly);

Expand Down Expand Up @@ -338,8 +351,8 @@
*predicted_obstacle_pose_min_vel, *predicted_obstacle_pose_max_vel};

std::vector<geometry_msgs::msg::Point> collision_points;
const bool collision_detected =
checkCollisionWithShape(bg_poly_vehicle, pose_with_range, obstacle.shape, collision_points);
const bool collision_detected = checkCollisionWithShape(

Check warning on line 354 in planning/behavior_velocity_run_out_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_run_out_module/src/scene.cpp#L354

Added line #L354 was not covered by tests
bg_poly_vehicle, pose_with_range, obstacle.shape, crosswalk_lanelets, collision_points);

if (!collision_detected) {
continue;
Expand Down Expand Up @@ -398,28 +411,46 @@

bool RunOutModule::checkCollisionWithShape(
const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape,
const std::vector<std::pair<int64_t, lanelet::ConstLanelet>> & crosswalk_lanelets,
std::vector<geometry_msgs::msg::Point> & collision_points) const
{
bool collision_detected = false;
switch (shape.type) {
case Shape::CYLINDER:
collision_detected = checkCollisionWithCylinder(
vehicle_polygon, pose_with_range, shape.dimensions.x / 2.0, collision_points);
break;

case Shape::BOUNDING_BOX:
collision_detected = checkCollisionWithBoundingBox(
vehicle_polygon, pose_with_range, shape.dimensions, collision_points);
break;

case Shape::POLYGON:
collision_detected = checkCollisionWithPolygon();
break;

default:
break;
}

if (!collision_points.empty()) {
for (const auto & crosswalk : crosswalk_lanelets) {
const auto poly = crosswalk.second.polygon2d().basicPolygon();
for (auto it = collision_points.begin(); it != collision_points.end();) {
if (boost::geometry::within(lanelet::BasicPoint2d(it->x, it->y), poly)) {
it = collision_points.erase(it);
} else {
++it;
}
}
if (collision_points.empty()) {
break;
}
}
collision_detected = !collision_points.empty();

Check warning on line 451 in planning/behavior_velocity_run_out_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_run_out_module/src/scene.cpp#L451

Added line #L451 was not covered by tests
}

Check warning on line 453 in planning/behavior_velocity_run_out_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

RunOutModule::checkCollisionWithShape has a cyclomatic complexity of 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 453 in planning/behavior_velocity_run_out_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Deep, Nested Complexity

RunOutModule::checkCollisionWithShape has a nested complexity depth of 4, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
return collision_detected;
}

Expand Down
8 changes: 6 additions & 2 deletions planning/behavior_velocity_run_out_module/src/scene.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

#include <memory>
#include <optional>
#include <utility>
#include <vector>

namespace behavior_velocity_planner
Expand Down Expand Up @@ -67,7 +68,8 @@ class RunOutModule : public SceneModuleInterface
Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const;

std::optional<DynamicObstacle> detectCollision(
const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path_points);
const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path,
const std::vector<std::pair<int64_t, lanelet::ConstLanelet>> & crosswalk_lanelets);

float calcCollisionPositionOfVehicleSide(
const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const;
Expand All @@ -77,7 +79,8 @@ class RunOutModule : public SceneModuleInterface

std::vector<DynamicObstacle> checkCollisionWithObstacles(
const std::vector<DynamicObstacle> & dynamic_obstacles,
std::vector<geometry_msgs::msg::Point> poly, const float travel_time) const;
std::vector<geometry_msgs::msg::Point> poly, const float travel_time,
const std::vector<std::pair<int64_t, lanelet::ConstLanelet>> & crosswalk_lanelets) const;

std::optional<DynamicObstacle> findNearestCollisionObstacle(
const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose,
Expand All @@ -89,6 +92,7 @@ class RunOutModule : public SceneModuleInterface

bool checkCollisionWithShape(
const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape,
const std::vector<std::pair<int64_t, lanelet::ConstLanelet>> & crosswalk_lanelets,
std::vector<geometry_msgs::msg::Point> & collision_points) const;

bool checkCollisionWithCylinder(
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_run_out_module/src/utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ struct RunOutParam
{
std::string detection_method;
bool use_partition_lanelet;
bool suppress_on_crosswalk;
bool specify_decel_jerk;
double stop_margin;
double passing_margin;
Expand Down
Loading