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.

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
Changes from 1 commit
Commits
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
Prev Previous commit
Next Next commit
rename namespace
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
  • Loading branch information
isamu-takagi committed Mar 7, 2025
commit e0054df35438cf455d042382e681699ad2c38ef4
2 changes: 1 addition & 1 deletion autoware_utils_geometry/README.md
Original file line number Diff line number Diff line change
@@ -41,7 +41,7 @@ The geometry module provides classes and functions for handling 2D and 3D points
```cpp
#include "autoware_utils/geometry/alt_geometry.hpp"

using namespace autoware_utils::alt;
using namespace autoware_utils_geometry::alt;

int main() {
Vector2d vec1(3.0, 4.0);
Original file line number Diff line number Diff line change
@@ -22,7 +22,7 @@
#include <utility>
#include <vector>

namespace autoware_utils
namespace autoware_utils_geometry
{
// Alternatives for Boost.Geometry ----------------------------------------------------------------
// TODO(mitukou1109): remove namespace
@@ -195,6 +195,6 @@ 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
} // namespace autoware_utils_geometry

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__ALT_GEOMETRY_HPP_
Original file line number Diff line number Diff line change
@@ -25,7 +25,7 @@

#include <geometry_msgs/msg/point.hpp>

namespace autoware_utils
namespace autoware_utils_geometry
{
// 2D
struct Point2d;
@@ -93,7 +93,7 @@ inline Point3d from_msg(const geometry_msgs::msg::Point & msg)
point.z() = msg.z;
return point;
}
} // namespace autoware_utils
} // namespace autoware_utils_geometry

BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT
autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT
Original file line number Diff line number Diff line change
@@ -24,7 +24,7 @@

#include <vector>

namespace autoware_utils
namespace autoware_utils_geometry
{
bool is_clockwise(const Polygon2d & polygon);
Polygon2d inverse_clockwise(const Polygon2d & polygon);
@@ -47,6 +47,6 @@ Polygon2d to_footprint(
const double base_to_rear, const double width);
double get_area(const autoware_perception_msgs::msg::Shape & shape);
Polygon2d expand_polygon(const Polygon2d & input_polygon, const double offset);
} // namespace autoware_utils
} // namespace autoware_utils_geometry

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__BOOST_POLYGON_UTILS_HPP_
Original file line number Diff line number Diff line change
@@ -20,7 +20,7 @@
#include <optional>
#include <vector>

namespace autoware_utils
namespace autoware_utils_geometry
{
struct LinkedPoint
{
@@ -101,6 +101,6 @@ std::vector<alt::ConvexPolygon2d> triangulate(const alt::Polygon2d & polygon);
* @return A vector of convex triangles representing the triangulated polygon.
*/
std::vector<Polygon2d> triangulate(const Polygon2d & polygon);
} // namespace autoware_utils
} // namespace autoware_utils_geometry

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__EAR_CLIPPING_HPP_
Original file line number Diff line number Diff line change
@@ -96,7 +96,7 @@ inline void doTransform(
#endif
} // namespace tf2

namespace autoware_utils
namespace autoware_utils_geometry
{

// TODO(Takagi, Isamu): Move to each function.
@@ -593,6 +593,6 @@ std::optional<geometry_msgs::msg::Point> intersect(
*/
bool intersects_convex(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);

} // namespace autoware_utils
} // namespace autoware_utils_geometry

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__GEOMETRY_HPP_
Original file line number Diff line number Diff line change
@@ -17,13 +17,13 @@

#include "autoware_utils_geometry/geometry/boost_geometry.hpp"

namespace autoware_utils::gjk
namespace autoware_utils_geometry::gjk
{
/**
* @brief Check if 2 convex polygons intersect using the GJK algorithm
* @details much faster than boost::geometry::overlaps() but limited to convex polygons
*/
bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);
} // namespace autoware_utils::gjk
} // namespace autoware_utils_geometry::gjk

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__GJK_2D_HPP_
Original file line number Diff line number Diff line change
@@ -18,7 +18,7 @@
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>

namespace autoware_utils
namespace autoware_utils_geometry
{
struct PoseDeviation
{
@@ -39,6 +39,6 @@ double calc_yaw_deviation(
PoseDeviation calc_pose_deviation(
const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose);

} // namespace autoware_utils
} // namespace autoware_utils_geometry

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__POSE_DEVIATION_HPP_
Original file line number Diff line number Diff line change
@@ -20,7 +20,7 @@
#include <optional>
#include <vector>

namespace autoware_utils
namespace autoware_utils_geometry
{
/// @brief generate a random non-convex polygon
/// @param vertices number of vertices for the desired polygon
@@ -42,6 +42,6 @@ bool test_intersection(
const std::function<
bool(const autoware_utils::Polygon2d &, const autoware_utils::Polygon2d &)> &);

} // namespace autoware_utils
} // namespace autoware_utils_geometry

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__RANDOM_CONCAVE_POLYGON_HPP_
Original file line number Diff line number Diff line change
@@ -17,13 +17,13 @@

#include <autoware_utils_geometry/geometry/geometry.hpp>

namespace autoware_utils
namespace autoware_utils_geometry
{
/// @brief generate a random convex polygon
/// @param vertices number of vertices for the desired polygon
/// @param max points will be generated in the range [-max,max]
/// @details algorithm from https://cglab.ca/~sander/misc/ConvexGeneration/convex.html
Polygon2d random_convex_polygon(const size_t vertices, const double max);
} // namespace autoware_utils
} // namespace autoware_utils_geometry

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__RANDOM_CONVEX_POLYGON_HPP_
Original file line number Diff line number Diff line change
@@ -17,14 +17,14 @@

#include "autoware_utils_geometry/geometry/boost_geometry.hpp"

namespace autoware_utils::sat
namespace autoware_utils_geometry::sat
{
/**
* @brief Check if 2 convex polygons intersect using the SAT algorithm
* @details faster than boost::geometry::overlap() but speed decline sharply as vertices increase
*/
bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);

} // namespace autoware_utils::sat
} // namespace autoware_utils_geometry::sat

#endif // AUTOWARE_UTILS_GEOMETRY__GEOMETRY__SAT_2D_HPP_
Original file line number Diff line number Diff line change
@@ -15,7 +15,7 @@
#ifndef AUTOWARE_UTILS_GEOMETRY__MSG__COVARIANCE_HPP_
#define AUTOWARE_UTILS_GEOMETRY__MSG__COVARIANCE_HPP_

namespace autoware_utils
namespace autoware_utils_geometry
{
namespace xyz_covariance_index
{
@@ -115,6 +115,6 @@ enum XYZ_UPPER_COV_IDX {
Z_Z = 5,
};
} // namespace xyz_upper_covariance_index
} // namespace autoware_utils
} // namespace autoware_utils_geometry

#endif // AUTOWARE_UTILS_GEOMETRY__MSG__COVARIANCE_HPP_
1 change: 1 addition & 0 deletions autoware_utils_geometry/package.xml
Original file line number Diff line number Diff line change
@@ -16,6 +16,7 @@

<depend>autoware_internal_planning_msgs</depend>
<depend>autoware_utils_math</depend>
<depend>libboost-system-dev</depend>
<depend>tf2</depend>
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>
4 changes: 2 additions & 2 deletions autoware_utils_geometry/src/geometry/alt_geometry.cpp
Original file line number Diff line number Diff line change
@@ -19,7 +19,7 @@
#include <utility>
#include <vector>

namespace autoware_utils
namespace autoware_utils_geometry
{
// Alternatives for Boost.Geometry ----------------------------------------------------------------
namespace alt
@@ -644,4 +644,4 @@ bool within(

return true;
}
} // namespace autoware_utils
} // namespace autoware_utils_geometry
Original file line number Diff line number Diff line change
@@ -68,7 +68,7 @@ double get_circle_area(const geometry_msgs::msg::Vector3 & dimensions)
}
} // namespace

namespace autoware_utils
namespace autoware_utils_geometry
{
bool is_clockwise(const Polygon2d & polygon)
{
@@ -264,4 +264,4 @@ Polygon2d expand_polygon(const Polygon2d & input_polygon, const double offset)
boost::geometry::correct(expanded_polygon);
return expanded_polygon;
}
} // namespace autoware_utils
} // namespace autoware_utils_geometry
4 changes: 2 additions & 2 deletions autoware_utils_geometry/src/geometry/ear_clipping.cpp
Original file line number Diff line number Diff line change
@@ -18,7 +18,7 @@
#include <limits>
#include <vector>

namespace autoware_utils
namespace autoware_utils_geometry
{

void remove_point(const std::size_t p_index, std::vector<LinkedPoint> & points)
@@ -631,4 +631,4 @@ std::vector<Polygon2d> triangulate(const Polygon2d & poly)
}
return triangles;
}
} // namespace autoware_utils
} // namespace autoware_utils_geometry
4 changes: 2 additions & 2 deletions autoware_utils_geometry/src/geometry/geometry.cpp
Original file line number Diff line number Diff line change
@@ -36,7 +36,7 @@ void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<tf2::Tran
}
} // namespace tf2

namespace autoware_utils
namespace autoware_utils_geometry
{
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Quaternion & quat)
{
@@ -394,4 +394,4 @@ bool intersects_convex(const Polygon2d & convex_polygon1, const Polygon2d & conv
return gjk::intersects(convex_polygon1, convex_polygon2);
}

} // namespace autoware_utils
} // namespace autoware_utils_geometry
4 changes: 2 additions & 2 deletions autoware_utils_geometry/src/geometry/gjk_2d.cpp
Original file line number Diff line number Diff line change
@@ -18,7 +18,7 @@

#include <boost/geometry/algorithms/equals.hpp>

namespace autoware_utils::gjk
namespace autoware_utils_geometry::gjk
{

namespace
@@ -147,4 +147,4 @@ bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_poly
}
return true;
}
} // namespace autoware_utils::gjk
} // namespace autoware_utils_geometry::gjk
4 changes: 2 additions & 2 deletions autoware_utils_geometry/src/geometry/pose_deviation.cpp
Original file line number Diff line number Diff line change
@@ -28,7 +28,7 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

namespace autoware_utils
namespace autoware_utils_geometry
{

double calc_lateral_deviation(
@@ -82,4 +82,4 @@ PoseDeviation calc_pose_deviation(

return deviation;
}
} // namespace autoware_utils
} // namespace autoware_utils_geometry
Original file line number Diff line number Diff line change
@@ -29,7 +29,7 @@
#include <utility>
#include <vector>

namespace autoware_utils
namespace autoware_utils_geometry
{
namespace
{
@@ -404,4 +404,4 @@ std::optional<Polygon2d> random_concave_polygon(const size_t vertices, const dou
}
return poly;
}
} // namespace autoware_utils
} // namespace autoware_utils_geometry
Original file line number Diff line number Diff line change
@@ -20,7 +20,7 @@
#include <random>
#include <vector>

namespace autoware_utils
namespace autoware_utils_geometry
{
namespace
{
@@ -110,4 +110,4 @@ Polygon2d random_convex_polygon(const size_t vertices, const double max)
boost::geometry::correct(poly);
return poly;
}
} // namespace autoware_utils
} // namespace autoware_utils_geometry
4 changes: 2 additions & 2 deletions autoware_utils_geometry/src/geometry/sat_2d.cpp
Original file line number Diff line number Diff line change
@@ -16,7 +16,7 @@

#include <utility>

namespace autoware_utils::sat
namespace autoware_utils_geometry::sat
{

namespace
@@ -79,4 +79,4 @@ bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_poly
has_no_separating_axis(convex_polygon2, convex_polygon1);
}

} // namespace autoware_utils::sat
} // namespace autoware_utils_geometry::sat