scenario_simulator_v2 C++ API
bounding_box.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef GEOMETRY__BOUNDING_BOX_HPP_
16 #define GEOMETRY__BOUNDING_BOX_HPP_
17 
18 #include <boost/assert.hpp>
19 #include <boost/assign/list_of.hpp>
20 #include <boost/geometry.hpp>
21 #include <boost/geometry/algorithms/disjoint.hpp>
22 #include <boost/geometry/geometries/linestring.hpp>
23 #include <boost/geometry/geometries/point_xy.hpp>
24 #include <boost/numeric/conversion/bounds.hpp>
25 #include <geometry/transform.hpp>
26 #include <geometry_msgs/msg/pose.hpp>
27 #include <optional>
28 #include <traffic_simulator_msgs/msg/bounding_box.hpp>
29 #include <vector>
30 
31 namespace math
32 {
33 namespace geometry
34 {
35 
36 using boost_point = boost::geometry::model::d2::point_xy<double>;
37 using boost_polygon = boost::geometry::model::polygon<boost_point>;
38 
40 {
41  double front;
42  double rear;
43  double right;
44  double left;
45  double up;
46  double down;
47 };
48 
49 std::optional<double> getPolygonDistance(
50  const geometry_msgs::msg::Pose & pose0, const traffic_simulator_msgs::msg::BoundingBox & bbox0,
51  const geometry_msgs::msg::Pose & pose1, const traffic_simulator_msgs::msg::BoundingBox & bbox1);
52 std::optional<std::pair<geometry_msgs::msg::Pose, geometry_msgs::msg::Pose>> getClosestPoses(
53  const geometry_msgs::msg::Pose & pose0, const traffic_simulator_msgs::msg::BoundingBox & bbox0,
54  const geometry_msgs::msg::Pose & pose1, const traffic_simulator_msgs::msg::BoundingBox & bbox1);
56  const boost_point & p, const boost_point & p1, const boost_point & p2);
57 boost_point toBoostPoint(const geometry_msgs::msg::Point & point);
58 boost_polygon toBoostPolygon(const std::vector<geometry_msgs::msg::Point> & points);
59 geometry_msgs::msg::Pose toPose(const boost_point & point);
60 geometry_msgs::msg::Pose subtractPoses(
61  const geometry_msgs::msg::Pose & pose1, const geometry_msgs::msg::Pose & pose2);
62 auto toPolygon2D(const traffic_simulator_msgs::msg::BoundingBox & bounding_box)
63  -> std::vector<geometry_msgs::msg::Point>;
64 auto toPolygon2D(
65  const geometry_msgs::msg::Pose & pose,
66  const traffic_simulator_msgs::msg::BoundingBox & bounding_box) -> boost_polygon;
67 std::vector<geometry_msgs::msg::Point> getPointsFromBbox(
68  const traffic_simulator_msgs::msg::BoundingBox & bounding_box,
69  const double width_extension_right = 0.0, const double width_extension_left = 0.0,
70  const double length_extension_front = 0.0, const double length_extension_rear = 0.0);
72  const traffic_simulator_msgs::msg::BoundingBox & bounding_box);
73 
74 } // namespace geometry
75 } // namespace math
76 
77 #endif // GEOMETRY__BOUNDING_BOX_HPP_
boost_polygon toBoostPolygon(const std::vector< geometry_msgs::msg::Point > &points)
Definition: bounding_box.cpp:220
auto toPolygon2D(const traffic_simulator_msgs::msg::BoundingBox &bounding_box) -> std::vector< geometry_msgs::msg::Point >
Definition: bounding_box.cpp:159
boost::geometry::model::polygon< boost_point > boost_polygon
Definition: bounding_box.hpp:37
std::optional< std::pair< geometry_msgs::msg::Pose, geometry_msgs::msg::Pose > > getClosestPoses(const geometry_msgs::msg::Pose &pose0, const traffic_simulator_msgs::msg::BoundingBox &bbox0, const geometry_msgs::msg::Pose &pose1, const traffic_simulator_msgs::msg::BoundingBox &bbox1)
Definition: bounding_box.cpp:58
boost_point pointToSegmentProjection(const boost_point &p, const boost_point &p1, const boost_point &p2)
Definition: bounding_box.cpp:101
DistancesFromCenterToEdge getDistancesFromCenterToEdge(const traffic_simulator_msgs::msg::BoundingBox &bounding_box)
Definition: bounding_box.cpp:251
boost_point toBoostPoint(const geometry_msgs::msg::Point &point)
Definition: bounding_box.cpp:215
std::vector< geometry_msgs::msg::Point > getPointsFromBbox(const traffic_simulator_msgs::msg::BoundingBox &bounding_box, const double width_extension_right=0.0, const double width_extension_left=0.0, const double length_extension_front=0.0, const double length_extension_rear=0.0)
Definition: bounding_box.cpp:129
geometry_msgs::msg::Pose toPose(const boost_point &point)
Definition: bounding_box.cpp:231
geometry_msgs::msg::Pose subtractPoses(const geometry_msgs::msg::Pose &pose1, const geometry_msgs::msg::Pose &pose2)
Definition: bounding_box.cpp:240
boost::geometry::model::d2::point_xy< double > boost_point
Definition: bounding_box.hpp:36
std::optional< double > getPolygonDistance(const geometry_msgs::msg::Pose &pose0, const traffic_simulator_msgs::msg::BoundingBox &bbox0, const geometry_msgs::msg::Pose &pose1, const traffic_simulator_msgs::msg::BoundingBox &bbox1)
Get the Polygon Distance object.
Definition: bounding_box.cpp:38
Definition: bounding_box.hpp:32
Definition: bounding_box.hpp:40
double right
Definition: bounding_box.hpp:43
double left
Definition: bounding_box.hpp:44
double up
Definition: bounding_box.hpp:45
double front
Definition: bounding_box.hpp:41
double rear
Definition: bounding_box.hpp:42
double down
Definition: bounding_box.hpp:46