Skip to content

Commit fd9677c

Browse files
authored
feat(behavior_velocity_planner): replace Polygon2d with common's one (#1767)
* feat(behavior_velocity_planner): replace Polygon2d with common's one Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent 728875e commit fd9677c

File tree

15 files changed

+56
-196
lines changed

15 files changed

+56
-196
lines changed

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,6 @@
1818
#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
1919

2020
#include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
21-
// #include "geometry_msgs/msg/point32.hpp"
2221
#include "geometry_msgs/msg/pose.hpp"
2322

2423
#include <boost/geometry.hpp>
@@ -35,6 +34,7 @@ geometry_msgs::msg::Polygon rotatePolygon(
3534
const geometry_msgs::msg::Polygon & polygon, const double & angle);
3635
Polygon2d toPolygon2d(
3736
const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape);
37+
Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object);
3838
double getArea(const autoware_auto_perception_msgs::msg::Shape & shape);
3939

4040
} // namespace tier4_autoware_utils

common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp

+7
Original file line numberDiff line numberDiff line change
@@ -167,6 +167,13 @@ Polygon2d toPolygon2d(
167167
return isClockwise(polygon) ? polygon : inverseClockwise(polygon);
168168
}
169169

170+
tier4_autoware_utils::Polygon2d toPolygon2d(
171+
const autoware_auto_perception_msgs::msg::PredictedObject & object)
172+
{
173+
return tier4_autoware_utils::toPolygon2d(
174+
object.kinematics.initial_pose_with_covariance.pose, object.shape);
175+
}
176+
170177
double getArea(const autoware_auto_perception_msgs::msg::Shape & shape)
171178
{
172179
if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

-18
Original file line numberDiff line numberDiff line change
@@ -239,24 +239,6 @@ class IntersectionModule : public SceneModuleInterface
239239
bool isTargetStuckVehicleType(
240240
const autoware_auto_perception_msgs::msg::PredictedObject & object) const;
241241

242-
/**
243-
* @brief convert object to footprint polygon
244-
* @param object detected object
245-
* @return 2d polygon of the object footprint
246-
*/
247-
Polygon2d toFootprintPolygon(
248-
const autoware_auto_perception_msgs::msg::PredictedObject & object) const;
249-
250-
/**
251-
* @brief convert predicted object to footprint polygon
252-
* @param object detected object
253-
* @param predicted_pose predicted object pose
254-
* @return 2d polygon of the object footprint
255-
*/
256-
Polygon2d toPredictedFootprintPolygon(
257-
const autoware_auto_perception_msgs::msg::PredictedObject & object,
258-
const geometry_msgs::msg::Pose & predicted_pose) const;
259-
260242
/**
261243
* @brief Whether target tier4_api_msgs::Intersection::status is valid or not
262244
* @param target_status target tier4_api_msgs::Intersection::status

planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -90,15 +90,15 @@ class RunOutModule : public SceneModuleInterface
9090
const float velocity_mps) const;
9191

9292
bool checkCollisionWithShape(
93-
const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range,
94-
const Shape & shape, std::vector<geometry_msgs::msg::Point> & collision_points) const;
93+
const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape,
94+
std::vector<geometry_msgs::msg::Point> & collision_points) const;
9595

9696
bool checkCollisionWithCylinder(
97-
const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range,
98-
const float radius, std::vector<geometry_msgs::msg::Point> & collision_points) const;
97+
const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius,
98+
std::vector<geometry_msgs::msg::Point> & collision_points) const;
9999

100100
bool checkCollisionWithBoundingBox(
101-
const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range,
101+
const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range,
102102
const geometry_msgs::msg::Vector3 & dimension,
103103
std::vector<geometry_msgs::msg::Point> & collision_points) const;
104104

planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp

+10-108
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,8 @@
1515
#ifndef UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
1616
#define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
1717

18+
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
19+
1820
#include <autoware_auto_planning_msgs/msg/path_point.hpp>
1921
#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
2022
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
@@ -33,7 +35,6 @@
3335
#include <boost/geometry/geometries/point_xy.hpp>
3436
#include <boost/geometry/geometries/polygon.hpp>
3537
#include <boost/geometry/geometries/register/point.hpp>
36-
#include <boost/geometry/geometries/segment.hpp>
3738

3839
#include <lanelet2_core/primitives/Polygon.h>
3940
#include <tf2/utils.h>
@@ -60,16 +61,16 @@ BOOST_GEOMETRY_REGISTER_POINT_3D(
6061

6162
namespace behavior_velocity_planner
6263
{
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;
6869

6970
template <class T>
7071
Point2d to_bg2d(const T & p)
7172
{
72-
return Point2d(boost::geometry::get<0>(p), boost::geometry::get<1>(p));
73+
return Point2d(bg::get<0>(p), bg::get<1>(p));
7374
}
7475

7576
template <class T>
@@ -82,17 +83,6 @@ LineString2d to_bg2d(const std::vector<T> & vec)
8283
return ps;
8384
}
8485

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-
9686
inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line)
9787
{
9888
Polygon2d polygon;
@@ -107,78 +97,11 @@ inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2
10797
polygon.outer().push_back(*itr);
10898
}
10999

100+
bg::correct(polygon);
110101
return polygon;
111102
}
112103

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)
182105
{
183106
geometry_msgs::msg::Polygon polygon_msg;
184107
geometry_msgs::msg::Point32 point_msg;
@@ -189,27 +112,6 @@ inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon)
189112
}
190113
return polygon_msg;
191114
}
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-
}
213115
} // namespace behavior_velocity_planner
214116

215117
#endif // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_

planning/behavior_velocity_planner/include/utilization/util.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,6 @@ struct PointWithSearchRangeIndex
7979
using geometry_msgs::msg::Pose;
8080
using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
8181
using Polygons2d = std::vector<Polygon2d>;
82-
using Point2d = boost::geometry::model::d2::point_xy<double>;
8382
using autoware_auto_perception_msgs::msg::PredictedObject;
8483
using autoware_auto_perception_msgs::msg::PredictedObjects;
8584
using autoware_auto_perception_msgs::msg::Shape;
@@ -162,7 +161,6 @@ geometry_msgs::msg::Pose transformAbsCoordinate2D(
162161
const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin);
163162
SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id);
164163

165-
Polygon2d toFootprintPolygon(const PredictedObject & object);
166164
bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
167165
geometry_msgs::msg::Pose getAheadPose(
168166
const size_t start_idx, const double ahead_dist,

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

+10-28
Original file line numberDiff line numberDiff line change
@@ -293,7 +293,7 @@ bool IntersectionModule::checkCollision(
293293
ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point),
294294
&closest_lanelet);
295295

296-
debug_data_.ego_lane_polygon = toGeomMsg(ego_poly);
296+
debug_data_.ego_lane_polygon = toGeomPoly(ego_poly);
297297

298298
/* extract target objects */
299299
autoware_auto_perception_msgs::msg::PredictedObjects target_objects;
@@ -403,12 +403,14 @@ bool IntersectionModule::checkCollision(
403403

404404
polygon.outer().emplace_back(polygon.outer().front());
405405

406-
debug_data_.candidate_collision_ego_lane_polygon = toGeomMsg(polygon);
406+
bg::correct(polygon);
407+
408+
debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon);
407409

408410
for (auto itr = first_itr; itr != last_itr.base(); ++itr) {
409-
const auto footprint_polygon = toPredictedFootprintPolygon(object, *itr);
411+
const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape);
410412
debug_data_.candidate_collision_object_polygons.emplace_back(
411-
toGeomMsg(footprint_polygon));
413+
toGeomPoly(footprint_polygon));
412414
if (bg::intersects(polygon, footprint_polygon)) {
413415
collision_detected = true;
414416
break;
@@ -464,6 +466,7 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon(
464466
}
465467

466468
polygon.outer().emplace_back(polygon.outer().front());
469+
bg::correct(polygon);
467470

468471
return polygon;
469472
}
@@ -538,7 +541,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(
538541
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
539542
const Polygon2d & stuck_vehicle_detect_area) const
540543
{
541-
debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area);
544+
debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area);
542545

543546
for (const auto & object : objects_ptr->objects) {
544547
if (!isTargetStuckVehicleType(object)) {
@@ -550,7 +553,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(
550553
}
551554

552555
// check if the footprint is in the stuck detect area
553-
const Polygon2d obj_footprint = toFootprintPolygon(object);
556+
const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object);
554557
const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area);
555558
if (is_in_stuck_area) {
556559
RCLCPP_DEBUG(logger_, "stuck vehicle found.");
@@ -561,27 +564,6 @@ bool IntersectionModule::checkStuckVehicleInIntersection(
561564
return false;
562565
}
563566

564-
Polygon2d IntersectionModule::toFootprintPolygon(
565-
const autoware_auto_perception_msgs::msg::PredictedObject & object) const
566-
{
567-
Polygon2d obj_footprint;
568-
if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) {
569-
obj_footprint = toBoostPoly(object.shape.footprint);
570-
} else {
571-
// cylinder type is treated as square-polygon
572-
obj_footprint =
573-
obj2polygon(object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions);
574-
}
575-
return obj_footprint;
576-
}
577-
578-
Polygon2d IntersectionModule::toPredictedFootprintPolygon(
579-
const autoware_auto_perception_msgs::msg::PredictedObject & object,
580-
const geometry_msgs::msg::Pose & predicted_pose) const
581-
{
582-
return obj2polygon(predicted_pose, object.shape.dimensions);
583-
}
584-
585567
bool IntersectionModule::isTargetCollisionVehicleType(
586568
const autoware_auto_perception_msgs::msg::PredictedObject & object) const
587569
{
@@ -794,7 +776,7 @@ bool IntersectionModule::checkFrontVehicleDeceleration(
794776
predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point;
795777
predicted_object.kinematics.initial_pose_with_covariance.pose.orientation =
796778
tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw);
797-
Polygon2d predicted_obj_footprint = toFootprintPolygon(predicted_object);
779+
auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object);
798780
const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area);
799781
debug_data_.predicted_obj_pose.position = stopping_point;
800782
debug_data_.predicted_obj_pose.orientation =

planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -162,8 +162,8 @@ bool NoStoppingAreaModule::modifyPathVelocity(
162162
setSafe(true);
163163
return true;
164164
}
165-
debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area);
166-
debug_data_.stop_line_detect_area = toGeomMsg(stop_line_detect_area);
165+
debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area);
166+
debug_data_.stop_line_detect_area = toGeomPoly(stop_line_detect_area);
167167
// Find stuck vehicle in no stopping area
168168
const bool is_entry_prohibited_by_stuck_vehicle =
169169
checkStuckVehiclesInNoStoppingArea(stuck_vehicle_detect_area, predicted_obj_arr_ptr);
@@ -230,11 +230,11 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea(
230230
continue; // not stop vehicle
231231
}
232232
// check if the footprint is in the stuck detect area
233-
const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object);
233+
const Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object);
234234
const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly);
235235
if (is_in_stuck_area) {
236236
RCLCPP_DEBUG(logger_, "stuck vehicle found.");
237-
for (const auto p : obj_footprint.outer()) {
237+
for (const auto & p : obj_footprint.outer()) {
238238
geometry_msgs::msg::Point point;
239239
point.x = p.x();
240240
point.y = p.y();

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius)
3636
line_poly.outer().emplace_back(p0.x() - r * sin(angle), p0.y() + r * cos(angle));
3737
// std::cout << boost::geometry::wkt(line_poly) << std::endl;
3838
// std::cout << boost::geometry::wkt(line) << std::endl;
39+
40+
bg::correct(line_poly);
3941
return line_poly;
4042
}
4143

@@ -173,6 +175,8 @@ Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, cons
173175
poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, 0, 0).position));
174176
poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position));
175177
poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position));
178+
179+
bg::correct(poly);
176180
return poly;
177181
}
178182

0 commit comments

Comments
 (0)