15
15
#ifndef UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
16
16
#define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
17
17
18
+ #include < tier4_autoware_utils/geometry/boost_geometry.hpp>
19
+
18
20
#include < autoware_auto_planning_msgs/msg/path_point.hpp>
19
21
#include < autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
20
22
#include < autoware_auto_planning_msgs/msg/trajectory_point.hpp>
33
35
#include < boost/geometry/geometries/point_xy.hpp>
34
36
#include < boost/geometry/geometries/polygon.hpp>
35
37
#include < boost/geometry/geometries/register/point.hpp>
36
- #include < boost/geometry/geometries/segment.hpp>
37
38
38
39
#include < lanelet2_core/primitives/Polygon.h>
39
40
#include < tf2/utils.h>
@@ -60,16 +61,16 @@ BOOST_GEOMETRY_REGISTER_POINT_3D(
60
61
61
62
namespace behavior_velocity_planner
62
63
{
63
- using Point2d = boost::geometry::model::d2::point_xy< double > ;
64
- using Segment2d = boost::geometry::model::segment<Point2d>;
65
- using LineString2d = boost::geometry::model::linestring< Point2d> ;
66
- using Polygon2d =
67
- boost::geometry::model::polygon<Point2d, false , false >; // counter-clockwise, open
64
+ namespace bg = boost::geometry;
65
+
66
+ using Point2d = tier4_autoware_utils:: Point2d;
67
+ using LineString2d = tier4_autoware_utils::LineString2d;
68
+ using Polygon2d = tier4_autoware_utils::Polygon2d;
68
69
69
70
template <class T >
70
71
Point2d to_bg2d (const T & p)
71
72
{
72
- return Point2d (boost::geometry:: get<0 >(p), boost::geometry ::get<1 >(p));
73
+ return Point2d (bg:: get<0 >(p), bg ::get<1 >(p));
73
74
}
74
75
75
76
template <class T >
@@ -82,17 +83,6 @@ LineString2d to_bg2d(const std::vector<T> & vec)
82
83
return ps;
83
84
}
84
85
85
- inline Polygon2d linestring2polygon (const LineString2d & line_string)
86
- {
87
- Polygon2d polygon;
88
-
89
- for (const auto & p : line_string) {
90
- polygon.outer ().push_back (p);
91
- }
92
-
93
- return polygon;
94
- }
95
-
96
86
inline Polygon2d lines2polygon (const LineString2d & left_line, const LineString2d & right_line)
97
87
{
98
88
Polygon2d polygon;
@@ -107,78 +97,11 @@ inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2
107
97
polygon.outer ().push_back (*itr);
108
98
}
109
99
100
+ bg::correct (polygon);
110
101
return polygon;
111
102
}
112
103
113
- inline Polygon2d obj2polygon (
114
- const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & shape)
115
- {
116
- // rename
117
- const double x = pose.position .x ;
118
- const double y = pose.position .y ;
119
- const double h = shape.x ;
120
- const double w = shape.y ;
121
- const double yaw = tf2::getYaw (pose.orientation );
122
-
123
- // create base polygon
124
- Polygon2d obj_poly;
125
- boost::geometry::exterior_ring (obj_poly) = boost::assign::list_of<Point2d>(h / 2.0 , w / 2.0 )(
126
- -h / 2.0 , w / 2.0 )(-h / 2.0 , -w / 2.0 )(h / 2.0 , -w / 2.0 )(h / 2.0 , w / 2.0 );
127
-
128
- // rotate polygon(yaw)
129
- boost::geometry::strategy::transform::rotate_transformer<boost::geometry::radian, double , 2 , 2 >
130
- rotate (-yaw); // anti-clockwise -> :clockwise rotation
131
- Polygon2d rotate_obj_poly;
132
- boost::geometry::transform (obj_poly, rotate_obj_poly, rotate);
133
-
134
- // translate polygon(x, y)
135
- boost::geometry::strategy::transform::translate_transformer<double , 2 , 2 > translate (x, y);
136
- Polygon2d translate_obj_poly;
137
- boost::geometry::transform (rotate_obj_poly, translate_obj_poly, translate);
138
- return translate_obj_poly;
139
- }
140
-
141
- inline double calcOverlapAreaRate (const Polygon2d & target, const Polygon2d & base)
142
- {
143
- /* OverlapAreaRate: common area(target && base) / target area */
144
-
145
- if (boost::geometry::within (target, base)) {
146
- // target is within base, common area = target area
147
- return 1.0 ;
148
- }
149
-
150
- if (!boost::geometry::intersects (target, base)) {
151
- // target and base has not intersect area
152
- return 0.0 ;
153
- }
154
-
155
- // calculate intersect polygon
156
- std::vector<Polygon2d> intersects;
157
- boost::geometry::intersection (target, base, intersects);
158
-
159
- // calculate area of polygon
160
- double intersect_area = 0.0 ;
161
- for (const auto & intersect : intersects) {
162
- intersect_area += boost::geometry::area (intersect);
163
- }
164
- const double target_area = boost::geometry::area (target);
165
- // specification of boost1.65
166
- // common area is not intersect area
167
- const double common_area = target_area - intersect_area;
168
-
169
- return common_area / target_area;
170
- }
171
-
172
- inline std::vector<Segment2d> makeSegments (const LineString2d & ls)
173
- {
174
- std::vector<Segment2d> segments;
175
- for (size_t i = 0 ; i < ls.size (); ++i) {
176
- segments.emplace_back (ls.at (i), ls.at (i + 1 ));
177
- }
178
- return segments;
179
- }
180
-
181
- inline geometry_msgs::msg::Polygon toGeomMsg (const Polygon2d & polygon)
104
+ inline geometry_msgs::msg::Polygon toGeomPoly (const Polygon2d & polygon)
182
105
{
183
106
geometry_msgs::msg::Polygon polygon_msg;
184
107
geometry_msgs::msg::Point32 point_msg;
@@ -189,27 +112,6 @@ inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon)
189
112
}
190
113
return polygon_msg;
191
114
}
192
-
193
- inline Polygon2d toBoostPoly (const geometry_msgs::msg::Polygon & polygon)
194
- {
195
- Polygon2d boost_poly;
196
- for (const auto & point : polygon.points ) {
197
- const Point2d point2d (point.x , point.y );
198
- boost_poly.outer ().push_back (point2d);
199
- }
200
- return boost_poly;
201
- }
202
-
203
- inline Polygon2d toBoostPoly (const lanelet::BasicPolygon2d & polygon)
204
- {
205
- Polygon2d boost_poly;
206
- for (const auto & vec : polygon) {
207
- const Point2d point2d (vec.x (), vec.y ());
208
- boost_poly.outer ().push_back (point2d);
209
- }
210
-
211
- return boost_poly;
212
- }
213
115
} // namespace behavior_velocity_planner
214
116
215
117
#endif // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
0 commit comments