Skip to content

Commit 22cbf83

Browse files
isamu-takagiisamu-takagi
authored
feat(autoware_utils_geometry): split package (#48)
* feat(autoware_utils_geometry): split package Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * compatibility header Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * rename namespace Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix namespace Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix path Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix include path Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 4e6922e commit 22cbf83

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

41 files changed

+1731
-1316
lines changed

‎autoware_utils/README.md

-97
Original file line numberDiff line numberDiff line change
@@ -6,105 +6,8 @@ The **autoware_utils** library is a comprehensive toolkit designed to facilitate
66

77
### Design
88

9-
#### Geometry Module
10-
11-
The geometry module provides classes and functions for handling 2D and 3D points, vectors, polygons, and performing geometric operations:
12-
13-
- **`boost_geometry.hpp`**: Integrates Boost.Geometry for advanced geometric computations, defining point, segment, box, linestring, ring, and polygon types.
14-
- **`alt_geometry.hpp`**: Implements alternative geometric types and operations for 2D vectors and polygons, including vector arithmetic, polygon creation, and various geometric predicates.
15-
- **`ear_clipping.hpp`**: Provides algorithms for triangulating polygons using the ear clipping method.
16-
- **`gjk_2d.hpp`**: Implements the GJK algorithm for fast intersection detection between convex polygons.
17-
- **`sat_2d.hpp`**: Implements the SAT (Separating Axis Theorem) algorithm for detecting intersections between convex polygons.
18-
- **`random_concave_polygon.hpp` and `random_convex_polygon.hpp`**: Generate random concave and convex polygons for testing purposes.
19-
- **`pose_deviation.hpp`**: Calculates deviations between poses in terms of lateral, longitudinal, and yaw angles.
20-
- **`boost_polygon_utils.hpp`**: Utility functions for manipulating polygons, including:
21-
- Checking if a polygon is clockwise.
22-
- Rotating polygons around the origin.
23-
- Converting poses and shapes to polygons.
24-
- Expanding polygons by an offset.
25-
- **`geometry.hpp`**: Comprehensive geometric operations, including:
26-
- Distance calculations between points and segments.
27-
- Curvature computation.
28-
- Pose transformations and interpolations.
29-
- Intersection checks for convex polygons using GJK.
30-
- Conversion between different coordinate systems.
31-
329
#### ROS Module
3310

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

36-
- **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages.
37-
- **`msg_operation.hpp`**: Overloaded operators for quaternion messages.
3813
- **`self_pose_listener.hpp`**: Listens to the self-pose of the vehicle.
39-
40-
## Usage
41-
42-
### Including Headers
43-
44-
To use the Autoware Utils library in your project, include the necessary headers at the top of your source files:
45-
46-
```cpp
47-
#include "autoware_utils/geometry/boost_geometry.hpp"
48-
#include "autoware_utils/math/accumulator.hpp"
49-
#include "autoware_utils/ros/debug_publisher.hpp"
50-
```
51-
52-
or you can include `autoware_utils/autoware_utils.hpp` for all features:
53-
54-
```cpp
55-
#include "autoware_utils/autoware_utils.hpp"
56-
```
57-
58-
### Example Code Snippets
59-
60-
#### Using Vector2d from alt_geometry.hpp
61-
62-
```cpp
63-
#include "autoware_utils/geometry/alt_geometry.hpp"
64-
65-
using namespace autoware_utils::alt;
66-
67-
int main() {
68-
Vector2d vec1(3.0, 4.0);
69-
Vector2d vec2(1.0, 2.0);
70-
71-
// Compute the dot product
72-
double dot_product = vec1.dot(vec2);
73-
74-
// Compute the norm
75-
double norm = vec1.norm();
76-
77-
return 0;
78-
}
79-
```
80-
81-
### Detailed Usage Examples
82-
83-
#### Manipulating Polygons with boost_polygon_utils.hpp
84-
85-
```cpp
86-
#include "autoware_utils/geometry/boost_polygon_utils.hpp"
87-
#include "autoware_utils/geometry/boost_geometry.hpp"
88-
#include <rclcpp/rclcpp.hpp>
89-
90-
int main(int argc, char * argv[]) {
91-
rclcpp::init(argc, argv);
92-
auto node = rclcpp::Node::make_shared("polygon_node");
93-
94-
// Create a polygon
95-
autoware_utils::Polygon2d polygon;
96-
// Assume polygon is populated with points
97-
98-
// Rotate the polygon by 90 degrees
99-
autoware_utils::Polygon2d rotated_polygon = autoware_utils::rotate_polygon(polygon, M_PI / 2);
100-
101-
// Expand the polygon by an offset
102-
autoware_utils::Polygon2d expanded_polygon = autoware_utils::expand_polygon(polygon, 1.0);
103-
104-
// Check if the polygon is clockwise
105-
bool is_clockwise = autoware_utils::is_clockwise(polygon);
106-
107-
rclcpp::shutdown();
108-
return 0;
109-
}
110-
```

‎autoware_utils/include/autoware_utils/geometry/alt_geometry.hpp

+7-180
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2020-2024 Tier IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,186 +15,13 @@
1515
#ifndef AUTOWARE_UTILS__GEOMETRY__ALT_GEOMETRY_HPP_
1616
#define AUTOWARE_UTILS__GEOMETRY__ALT_GEOMETRY_HPP_
1717

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

20-
#include <list>
21-
#include <optional>
22-
#include <utility>
23-
#include <vector>
21+
#include <autoware_utils_geometry/alt_geometry.hpp>
22+
namespace autoware_utils { using namespace autoware_utils_geometry; }
2423

25-
namespace autoware_utils
26-
{
27-
// Alternatives for Boost.Geometry ----------------------------------------------------------------
28-
// TODO(mitukou1109): remove namespace
29-
namespace alt
30-
{
31-
class Vector2d
32-
{
33-
public:
34-
Vector2d() : x_(0.0), y_(0.0) {}
35-
36-
Vector2d(const double x, const double y) : x_(x), y_(y) {}
37-
38-
explicit Vector2d(const autoware_utils::Point2d & point) : x_(point.x()), y_(point.y()) {}
39-
40-
double cross(const Vector2d & other) const { return x_ * other.y() - y_ * other.x(); }
41-
42-
double dot(const Vector2d & other) const { return x_ * other.x() + y_ * other.y(); }
43-
44-
double norm2() const { return x_ * x_ + y_ * y_; }
45-
46-
double norm() const { return std::sqrt(norm2()); }
47-
48-
Vector2d vector_triple(const Vector2d & v1, const Vector2d & v2) const
49-
{
50-
const auto tmp = this->cross(v1);
51-
return {-v2.y() * tmp, v2.x() * tmp};
52-
}
53-
54-
const double & x() const { return x_; }
55-
56-
double & x() { return x_; }
57-
58-
const double & y() const { return y_; }
59-
60-
double & y() { return y_; }
61-
62-
private:
63-
double x_;
64-
double y_;
65-
};
66-
67-
inline Vector2d operator+(const Vector2d & v1, const Vector2d & v2)
68-
{
69-
return {v1.x() + v2.x(), v1.y() + v2.y()};
70-
}
71-
72-
inline Vector2d operator-(const Vector2d & v1, const Vector2d & v2)
73-
{
74-
return {v1.x() - v2.x(), v1.y() - v2.y()};
75-
}
76-
77-
inline Vector2d operator-(const Vector2d & v)
78-
{
79-
return {-v.x(), -v.y()};
80-
}
81-
82-
inline Vector2d operator*(const double & s, const Vector2d & v)
83-
{
84-
return {s * v.x(), s * v.y()};
85-
}
86-
87-
// We use Vector2d to represent points, but we do not name the class Point2d directly
88-
// as it has some vector operation functions.
89-
using Point2d = Vector2d;
90-
using Points2d = std::vector<Point2d>;
91-
using PointList2d = std::list<Point2d>;
92-
93-
class Polygon2d
94-
{
95-
public:
96-
static std::optional<Polygon2d> create(
97-
const PointList2d & outer, const std::vector<PointList2d> & inners) noexcept;
98-
99-
static std::optional<Polygon2d> create(
100-
PointList2d && outer, std::vector<PointList2d> && inners) noexcept;
101-
102-
static std::optional<Polygon2d> create(const autoware_utils::Polygon2d & polygon) noexcept;
103-
104-
const PointList2d & outer() const noexcept { return outer_; }
105-
106-
PointList2d & outer() noexcept { return outer_; }
107-
108-
const std::vector<PointList2d> & inners() const noexcept { return inners_; }
109-
110-
std::vector<PointList2d> & inners() noexcept { return inners_; }
111-
112-
autoware_utils::Polygon2d to_boost() const;
113-
114-
protected:
115-
Polygon2d(const PointList2d & outer, const std::vector<PointList2d> & inners)
116-
: outer_(outer), inners_(inners)
117-
{
118-
}
119-
120-
Polygon2d(PointList2d && outer, std::vector<PointList2d> && inners)
121-
: outer_(std::move(outer)), inners_(std::move(inners))
122-
{
123-
}
124-
125-
PointList2d outer_;
126-
127-
std::vector<PointList2d> inners_;
128-
};
129-
130-
class ConvexPolygon2d : public Polygon2d
131-
{
132-
public:
133-
static std::optional<ConvexPolygon2d> create(const PointList2d & vertices) noexcept;
134-
135-
static std::optional<ConvexPolygon2d> create(PointList2d && vertices) noexcept;
136-
137-
static std::optional<ConvexPolygon2d> create(const autoware_utils::Polygon2d & polygon) noexcept;
138-
139-
const PointList2d & vertices() const noexcept { return outer(); }
140-
141-
PointList2d & vertices() noexcept { return outer(); }
142-
143-
private:
144-
explicit ConvexPolygon2d(const PointList2d & vertices) : Polygon2d(vertices, {}) {}
145-
146-
explicit ConvexPolygon2d(PointList2d && vertices) : Polygon2d(std::move(vertices), {}) {}
147-
};
148-
} // namespace alt
149-
150-
double area(const alt::ConvexPolygon2d & poly);
151-
152-
std::optional<alt::ConvexPolygon2d> convex_hull(const alt::Points2d & points);
153-
154-
void correct(alt::Polygon2d & poly);
155-
156-
bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
157-
158-
bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);
159-
160-
double distance(
161-
const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
162-
163-
double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
164-
165-
std::optional<alt::ConvexPolygon2d> envelope(const alt::Polygon2d & poly);
166-
167-
bool equals(const alt::Point2d & point1, const alt::Point2d & point2);
168-
169-
bool equals(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2);
170-
171-
alt::Points2d::const_iterator find_farthest(
172-
const alt::Points2d & points, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
173-
174-
bool intersects(
175-
const alt::Point2d & seg1_start, const alt::Point2d & seg1_end, const alt::Point2d & seg2_start,
176-
const alt::Point2d & seg2_end);
177-
178-
bool intersects(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);
179-
180-
bool is_above(
181-
const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
182-
183-
bool is_clockwise(const alt::PointList2d & vertices);
184-
185-
bool is_convex(const alt::Polygon2d & poly);
186-
187-
alt::PointList2d simplify(const alt::PointList2d & line, const double max_distance);
188-
189-
bool touches(
190-
const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
191-
192-
bool touches(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
193-
194-
bool within(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
195-
196-
bool within(
197-
const alt::ConvexPolygon2d & poly_contained, const alt::ConvexPolygon2d & poly_containing);
198-
} // namespace autoware_utils
24+
// clang-format on
25+
// NOLINTEND
19926

20027
#endif // AUTOWARE_UTILS__GEOMETRY__ALT_GEOMETRY_HPP_

0 commit comments

Comments
 (0)