Skip to content

Commit

Permalink
refactor: use tier4_autoware_utils::intersect (autowarefoundation#4313)
Browse files Browse the repository at this point in the history 
* use tier4_autoware_utils::intersect in motion

Signed-off-by: Takayuki Murooka <[email protected]>

* use tier4_autoware_utils::intersect in behavior

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168
takayuki5168 authored Jul 23, 2023
1 parent bb3a3c3 commit ff8c637
Show file tree
Hide file tree
Showing 3 changed files with 13 additions and 81 deletions.
26 changes: 2 additions & 24 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,28 +69,6 @@ geometry_msgs::msg::Polygon toMsg(const tier4_autoware_utils::Polygon2d & polygo
}
return ret;
}

boost::optional<Point> intersect(
const Point & p1, const Point & p2, const Point & p3, const Point & p4)
{
// calculate intersection point
const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y);
if (det == 0.0) {
return {};
}

const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det;
const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det;
if (t < 0 || 1 < t || s < 0 || 1 < s) {
return {};
}

Point intersect_point;
intersect_point.x = t * p1.x + (1.0 - t) * p2.x;
intersect_point.y = t * p1.y + (1.0 - t) * p2.y;
intersect_point.z = t * p1.z + (1.0 - t) * p2.z;
return intersect_point;
}
} // namespace

bool isOnRight(const ObjectData & obj)
Expand Down Expand Up @@ -1222,8 +1200,8 @@ double extendToRoadShoulderDistanceWithPolygon(
.y(polygon[(i + 1) % polygon.size()].y())
.z(0.0);

const auto intersect_pos =
intersect(overhang_pos, lat_offset_overhang_pos, polygon_current_point, polygon_next_point);
const auto intersect_pos = tier4_autoware_utils::intersect(
overhang_pos, lat_offset_overhang_pos, polygon_current_point, polygon_next_point);
if (intersect_pos) {
intersect_dist_vec.push_back(calcDistance2d(*intersect_pos, overhang_pos));
}
Expand Down
41 changes: 9 additions & 32 deletions planning/behavior_path_planner/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,29 +125,6 @@ using tier4_autoware_utils::Point2d;

namespace drivable_area_processing
{
boost::optional<geometry_msgs::msg::Point> intersect(
const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4)
{
// calculate intersection point
const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y);
if (det == 0.0) {
return {};
}

const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det;
const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det;
if (t < 0 || 1 < t || s < 0 || 1 < s) {
return {};
}

geometry_msgs::msg::Point intersect_point;
intersect_point.x = t * p1.x + (1.0 - t) * p2.x;
intersect_point.y = t * p1.y + (1.0 - t) * p2.y;
intersect_point.z = t * p1.z + (1.0 - t) * p2.z;
return intersect_point;
}

boost::optional<std::pair<size_t, geometry_msgs::msg::Point>> intersectBound(
const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
const std::vector<geometry_msgs::msg::Point> & bound, const size_t seg_idx1,
Expand All @@ -158,11 +135,12 @@ boost::optional<std::pair<size_t, geometry_msgs::msg::Point>> intersectBound(
const size_t end_idx = static_cast<size_t>(std::min(
static_cast<int>(bound.size()) - 1, static_cast<int>(std::max(seg_idx1, seg_idx2)) + 1 + 5));
for (int i = start_idx; i < static_cast<int>(end_idx); ++i) {
const auto intersect_point = intersect(p1, p2, bound.at(i), bound.at(i + 1));
const auto intersect_point =
tier4_autoware_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1));
if (intersect_point) {
std::pair<size_t, geometry_msgs::msg::Point> result;
result.first = static_cast<size_t>(i);
result.second = intersect_point.get();
result.second = *intersect_point;
return result;
}
}
Expand Down Expand Up @@ -329,11 +307,11 @@ std::vector<PolygonPoint> concatenateTwoPolygons(
const size_t next_idx = outside_idx + 1;

for (size_t i = 0; i < get_in_poly().size() - 1; ++i) {
const auto intersection = intersect(
const auto intersection = tier4_autoware_utils::intersect(
get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point,
get_in_poly().at(i).point, get_in_poly().at(i + 1).point);
if (intersection) {
const auto intersect_point = PolygonPoint{intersection.get(), 0, 0.0, 0.0};
const auto intersect_point = PolygonPoint{*intersection, 0, 0.0, 0.0};
concatenated_polygon.push_back(intersect_point);

is_front_polygon_outside = !is_front_polygon_outside;
Expand Down Expand Up @@ -1714,14 +1692,14 @@ void makeBoundLongitudinallyMonotonic(
PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
const bool is_bound_left)
{
using drivable_area_processing::intersect;
using motion_utils::findNearestSegmentIndex;
using tier4_autoware_utils::calcAzimuthAngle;
using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
using tier4_autoware_utils::createQuaternionFromRPY;
using tier4_autoware_utils::getPoint;
using tier4_autoware_utils::getPose;
using tier4_autoware_utils::intersect;
using tier4_autoware_utils::normalizeRadian;

const auto set_orientation = [](
Expand All @@ -1741,7 +1719,7 @@ void makeBoundLongitudinallyMonotonic(
continue;
}

intersects.emplace_back(i, opt_intersect.get());
intersects.emplace_back(i, *opt_intersect);
}

if (intersects.empty()) {
Expand Down Expand Up @@ -1834,9 +1812,8 @@ void makeBoundLongitudinallyMonotonic(

if (intersect_point) {
Pose pose;
pose.position = intersect_point.get();
const auto yaw =
calcAzimuthAngle(intersect_point.get(), bound_with_pose.at(i + 1).position);
pose.position = *intersect_point;
const auto yaw = calcAzimuthAngle(*intersect_point, bound_with_pose.at(i + 1).position);
pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw);
monotonic_bound.push_back(pose);
bound_idx = i;
Expand Down
27 changes: 2 additions & 25 deletions planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,29 +115,6 @@ std::vector<double> toStdVector(const Eigen::VectorXd & eigen_vec)
return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()};
}

// NOTE: much faster than boost::geometry::intersection()
std::optional<geometry_msgs::msg::Point> intersect(
const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4)
{
// calculate intersection point
const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y);
if (det == 0.0) {
return std::nullopt;
}

const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det;
const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det;
if (t < 0 || 1 < t || s < 0 || 1 < s) {
return std::nullopt;
}

geometry_msgs::msg::Point intersect_point;
intersect_point.x = t * p1.x + (1.0 - t) * p2.x;
intersect_point.y = t * p1.y + (1.0 - t) * p2.y;
return intersect_point;
}

bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos)
{
const double base_theta = tf2::getYaw(pose.orientation);
Expand All @@ -163,8 +140,8 @@ double calcLateralDistToBounds(

double closest_dist_to_bound = max_lat_offset;
for (size_t i = 0; i < bound.size() - 1; ++i) {
const auto intersect_point =
intersect(min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1));
const auto intersect_point = tier4_autoware_utils::intersect(
min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1));
if (intersect_point) {
const bool is_point_left = isLeft(pose, *intersect_point);
const double dist_to_bound =
Expand Down

0 comments on commit ff8c637

Please sign in to comment.