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.

Sign up for GitHub

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(autoware_utils_geometry): split package #48

Merged
merged 6 commits into from
Mar 18, 2025
Merged

feat(autoware_utils_geometry): split package #48

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
b0220b3
feat(autoware_utils_geometry): split package
isamu-takagi Mar 7, 2025
d28ac4c
compatibility header
isamu-takagi Mar 7, 2025
e0054df
rename namespace
isamu-takagi Mar 7, 2025
7162fa2
fix namespace
isamu-takagi Mar 11, 2025
f092412
fix path
isamu-takagi Mar 13, 2025
15eb6ef
fix include path
isamu-takagi Mar 13, 2025
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
97 changes: 0 additions & 97 deletions autoware_utils/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,105 +6,8 @@ The **autoware_utils** library is a comprehensive toolkit designed to facilitate

### Design

#### Geometry Module

The geometry module provides classes and functions for handling 2D and 3D points, vectors, polygons, and performing geometric operations:

- **`boost_geometry.hpp`**: Integrates Boost.Geometry for advanced geometric computations, defining point, segment, box, linestring, ring, and polygon types.
- **`alt_geometry.hpp`**: Implements alternative geometric types and operations for 2D vectors and polygons, including vector arithmetic, polygon creation, and various geometric predicates.
- **`ear_clipping.hpp`**: Provides algorithms for triangulating polygons using the ear clipping method.
- **`gjk_2d.hpp`**: Implements the GJK algorithm for fast intersection detection between convex polygons.
- **`sat_2d.hpp`**: Implements the SAT (Separating Axis Theorem) algorithm for detecting intersections between convex polygons.
- **`random_concave_polygon.hpp` and `random_convex_polygon.hpp`**: Generate random concave and convex polygons for testing purposes.
- **`pose_deviation.hpp`**: Calculates deviations between poses in terms of lateral, longitudinal, and yaw angles.
- **`boost_polygon_utils.hpp`**: Utility functions for manipulating polygons, including:
- Checking if a polygon is clockwise.
- Rotating polygons around the origin.
- Converting poses and shapes to polygons.
- Expanding polygons by an offset.
- **`geometry.hpp`**: Comprehensive geometric operations, including:
- Distance calculations between points and segments.
- Curvature computation.
- Pose transformations and interpolations.
- Intersection checks for convex polygons using GJK.
- Conversion between different coordinate systems.

#### ROS Module

The ROS module provides utilities for working with ROS messages and nodes:

- **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages.
- **`msg_operation.hpp`**: Overloaded operators for quaternion messages.
- **`self_pose_listener.hpp`**: Listens to the self-pose of the vehicle.

## Usage

### Including Headers

To use the Autoware Utils library in your project, include the necessary headers at the top of your source files:

```cpp
#include "autoware_utils/geometry/boost_geometry.hpp"
#include "autoware_utils/math/accumulator.hpp"
#include "autoware_utils/ros/debug_publisher.hpp"
```

or you can include `autoware_utils/autoware_utils.hpp` for all features:

```cpp
#include "autoware_utils/autoware_utils.hpp"
```

### Example Code Snippets

#### Using Vector2d from alt_geometry.hpp

```cpp
#include "autoware_utils/geometry/alt_geometry.hpp"

using namespace autoware_utils::alt;

int main() {
Vector2d vec1(3.0, 4.0);
Vector2d vec2(1.0, 2.0);

// Compute the dot product
double dot_product = vec1.dot(vec2);

// Compute the norm
double norm = vec1.norm();

return 0;
}
```

### Detailed Usage Examples

#### Manipulating Polygons with boost_polygon_utils.hpp

```cpp
#include "autoware_utils/geometry/boost_polygon_utils.hpp"
#include "autoware_utils/geometry/boost_geometry.hpp"
#include <rclcpp/rclcpp.hpp>

int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("polygon_node");

// Create a polygon
autoware_utils::Polygon2d polygon;
// Assume polygon is populated with points

// Rotate the polygon by 90 degrees
autoware_utils::Polygon2d rotated_polygon = autoware_utils::rotate_polygon(polygon, M_PI / 2);

// Expand the polygon by an offset
autoware_utils::Polygon2d expanded_polygon = autoware_utils::expand_polygon(polygon, 1.0);

// Check if the polygon is clockwise
bool is_clockwise = autoware_utils::is_clockwise(polygon);

rclcpp::shutdown();
return 0;
}
```
187 changes: 7 additions & 180 deletions autoware_utils/include/autoware_utils/geometry/alt_geometry.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020-2024 Tier IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -15,186 +15,13 @@
#ifndef AUTOWARE_UTILS__GEOMETRY__ALT_GEOMETRY_HPP_
#define AUTOWARE_UTILS__GEOMETRY__ALT_GEOMETRY_HPP_

#include "autoware_utils/geometry/boost_geometry.hpp"
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
// clang-format off

#include <list>
#include <optional>
#include <utility>
#include <vector>
#include <autoware_utils_geometry/alt_geometry.hpp>
namespace autoware_utils { using namespace autoware_utils_geometry; }

namespace autoware_utils
{
// Alternatives for Boost.Geometry ----------------------------------------------------------------
// TODO(mitukou1109): remove namespace
namespace alt
{
class Vector2d
{
public:
Vector2d() : x_(0.0), y_(0.0) {}

Vector2d(const double x, const double y) : x_(x), y_(y) {}

explicit Vector2d(const autoware_utils::Point2d & point) : x_(point.x()), y_(point.y()) {}

double cross(const Vector2d & other) const { return x_ * other.y() - y_ * other.x(); }

double dot(const Vector2d & other) const { return x_ * other.x() + y_ * other.y(); }

double norm2() const { return x_ * x_ + y_ * y_; }

double norm() const { return std::sqrt(norm2()); }

Vector2d vector_triple(const Vector2d & v1, const Vector2d & v2) const
{
const auto tmp = this->cross(v1);
return {-v2.y() * tmp, v2.x() * tmp};
}

const double & x() const { return x_; }

double & x() { return x_; }

const double & y() const { return y_; }

double & y() { return y_; }

private:
double x_;
double y_;
};

inline Vector2d operator+(const Vector2d & v1, const Vector2d & v2)
{
return {v1.x() + v2.x(), v1.y() + v2.y()};
}

inline Vector2d operator-(const Vector2d & v1, const Vector2d & v2)
{
return {v1.x() - v2.x(), v1.y() - v2.y()};
}

inline Vector2d operator-(const Vector2d & v)
{
return {-v.x(), -v.y()};
}

inline Vector2d operator*(const double & s, const Vector2d & v)
{
return {s * v.x(), s * v.y()};
}

// We use Vector2d to represent points, but we do not name the class Point2d directly
// as it has some vector operation functions.
using Point2d = Vector2d;
using Points2d = std::vector<Point2d>;
using PointList2d = std::list<Point2d>;

class Polygon2d
{
public:
static std::optional<Polygon2d> create(
const PointList2d & outer, const std::vector<PointList2d> & inners) noexcept;

static std::optional<Polygon2d> create(
PointList2d && outer, std::vector<PointList2d> && inners) noexcept;

static std::optional<Polygon2d> create(const autoware_utils::Polygon2d & polygon) noexcept;

const PointList2d & outer() const noexcept { return outer_; }

PointList2d & outer() noexcept { return outer_; }

const std::vector<PointList2d> & inners() const noexcept { return inners_; }

std::vector<PointList2d> & inners() noexcept { return inners_; }

autoware_utils::Polygon2d to_boost() const;

protected:
Polygon2d(const PointList2d & outer, const std::vector<PointList2d> & inners)
: outer_(outer), inners_(inners)
{
}

Polygon2d(PointList2d && outer, std::vector<PointList2d> && inners)
: outer_(std::move(outer)), inners_(std::move(inners))
{
}

PointList2d outer_;

std::vector<PointList2d> inners_;
};

class ConvexPolygon2d : public Polygon2d
{
public:
static std::optional<ConvexPolygon2d> create(const PointList2d & vertices) noexcept;

static std::optional<ConvexPolygon2d> create(PointList2d && vertices) noexcept;

static std::optional<ConvexPolygon2d> create(const autoware_utils::Polygon2d & polygon) noexcept;

const PointList2d & vertices() const noexcept { return outer(); }

PointList2d & vertices() noexcept { return outer(); }

private:
explicit ConvexPolygon2d(const PointList2d & vertices) : Polygon2d(vertices, {}) {}

explicit ConvexPolygon2d(PointList2d && vertices) : Polygon2d(std::move(vertices), {}) {}
};
} // namespace alt

double area(const alt::ConvexPolygon2d & poly);

std::optional<alt::ConvexPolygon2d> convex_hull(const alt::Points2d & points);

void correct(alt::Polygon2d & poly);

bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);

bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);

double distance(
const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);

double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);

std::optional<alt::ConvexPolygon2d> envelope(const alt::Polygon2d & poly);

bool equals(const alt::Point2d & point1, const alt::Point2d & point2);

bool equals(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2);

alt::Points2d::const_iterator find_farthest(
const alt::Points2d & points, const alt::Point2d & seg_start, const alt::Point2d & seg_end);

bool intersects(
const alt::Point2d & seg1_start, const alt::Point2d & seg1_end, const alt::Point2d & seg2_start,
const alt::Point2d & seg2_end);

bool intersects(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);

bool is_above(
const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);

bool is_clockwise(const alt::PointList2d & vertices);

bool is_convex(const alt::Polygon2d & poly);

alt::PointList2d simplify(const alt::PointList2d & line, const double max_distance);

bool touches(
const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);

bool touches(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);

bool within(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);

bool within(
const alt::ConvexPolygon2d & poly_contained, const alt::ConvexPolygon2d & poly_containing);
} // namespace autoware_utils
// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__GEOMETRY__ALT_GEOMETRY_HPP_
Loading
Loading