Skip to content

Commit

Permalink
feat: revert namespace back to autoware_utils, new namespace will be …
Browse files Browse the repository at this point in the history
…updated in further version

Signed-off-by: JianKangEgon <[email protected]>
  • Loading branch information
JianKangEgon committed Jan 22, 2025
1 parent 483ad63 commit c04c422
Show file tree
Hide file tree
Showing 61 changed files with 196 additions and 349 deletions.
22 changes: 11 additions & 11 deletions autoware_utils/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ or you can include `autoware_utils/autoware_utils.hpp` for all features:
```cpp
#include "autoware_utils/geometry/alt_geometry.hpp"

using namespace autoware::utils::alt;
using namespace autoware_utils::alt;

int main() {
Vector2d vec1(3.0, 4.0);
Expand All @@ -123,7 +123,7 @@ int main() {
#include "autoware_utils/math/accumulator.hpp"
int main() {
autoware::utils::Accumulator<double> acc;
autoware_utils::Accumulator<double> acc;
acc.add(1.0);
acc.add(2.0);
Expand Down Expand Up @@ -152,7 +152,7 @@ int main(int argc, char * argv[]) {
auto node = rclcpp::Node::make_shared("transform_node");

// Initialize ManagedTransformBuffer
autoware::utils::ManagedTransformBuffer transform_buffer(node, false);
autoware_utils::ManagedTransformBuffer transform_buffer(node, false);

// Load point cloud data
sensor_msgs::msg::PointCloud2 cloud_in; // Assume this is populated with data
Expand Down Expand Up @@ -183,7 +183,7 @@ int main(int argc, char * argv[]) {
double param_value = 0.0;
std::vector<rclcpp::Parameter> params = node->get_parameters({"my_param"});
if (autoware::utils::update_param(params, "my_param", param_value)) {
if (autoware_utils::update_param(params, "my_param", param_value)) {
RCLCPP_INFO(node->get_logger(), "Updated parameter value: %f", param_value);
} else {
RCLCPP_WARN(node->get_logger(), "Parameter 'my_param' not found.");
Expand All @@ -206,7 +206,7 @@ int main(int argc, char * argv[]) {
auto node = rclcpp::Node::make_shared("processing_time_node");

// Initialize ProcessingTimePublisher
autoware::utils::ProcessingTimePublisher processing_time_pub(node.get(), "~/debug/processing_time_ms");
autoware_utils::ProcessingTimePublisher processing_time_pub(node.get(), "~/debug/processing_time_ms");

// Simulate some processing times
std::map<std::string, double> processing_times = {
Expand All @@ -233,17 +233,17 @@ int main(int argc, char * argv[]) {
auto node = rclcpp::Node::make_shared("polygon_node");
// Create a polygon
autoware::utils::Polygon2d polygon;
autoware_utils::Polygon2d polygon;
// Assume polygon is populated with points
// Rotate the polygon by 90 degrees
autoware::utils::Polygon2d rotated_polygon = autoware::utils::rotate_polygon(polygon, M_PI / 2);
autoware_utils::Polygon2d rotated_polygon = autoware_utils::rotate_polygon(polygon, M_PI / 2);
// Expand the polygon by an offset
autoware::utils::Polygon2d expanded_polygon = autoware::utils::expand_polygon(polygon, 1.0);
autoware_utils::Polygon2d expanded_polygon = autoware_utils::expand_polygon(polygon, 1.0);
// Check if the polygon is clockwise
bool is_clockwise = autoware::utils::is_clockwise(polygon);
bool is_clockwise = autoware_utils::is_clockwise(polygon);
rclcpp::shutdown();
return 0;
Expand Down Expand Up @@ -273,7 +273,7 @@ int main(int argc, char * argv[]) {
// Populate transform matrix with actual values

// Convert and transform point cloud
autoware::utils::transform_point_cloud_from_ros_msg(cloud_in, pcl_cloud, transform);
autoware_utils::transform_point_cloud_from_ros_msg(cloud_in, pcl_cloud, transform);

rclcpp::shutdown();
return 0;
Expand All @@ -292,7 +292,7 @@ int main(int argc, char * argv[]) {
auto node = rclcpp::Node::make_shared("debug_node");
// Initialize DebugPublisher
autoware::utils::DebugPublisher debug_pub(node, "/debug");
autoware_utils::DebugPublisher debug_pub(node, "/debug");
// Publish a debug message with custom type
float debug_data = 42.0;
Expand Down
15 changes: 6 additions & 9 deletions autoware_utils/include/autoware_utils/geometry/alt_geometry.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,7 @@
#include <utility>
#include <vector>

namespace autoware
{
namespace utils
namespace autoware_utils
{
// Alternatives for Boost.Geometry ----------------------------------------------------------------
// TODO(mitukou1109): remove namespace
Expand All @@ -37,7 +35,7 @@ class Vector2d

Vector2d(const double x, const double y) : x_(x), y_(y) {}

explicit Vector2d(const autoware::utils::Point2d & point) : x_(point.x()), y_(point.y()) {}
explicit Vector2d(const autoware_utils::Point2d & point) : x_(point.x()), y_(point.y()) {}

double cross(const Vector2d & other) const { return x_ * other.y() - y_ * other.x(); }

Expand Down Expand Up @@ -101,7 +99,7 @@ class Polygon2d
static std::optional<Polygon2d> create(
PointList2d && outer, std::vector<PointList2d> && inners) noexcept;

static std::optional<Polygon2d> create(const autoware::utils::Polygon2d & polygon) noexcept;
static std::optional<Polygon2d> create(const autoware_utils::Polygon2d & polygon) noexcept;

const PointList2d & outer() const noexcept { return outer_; }

Expand All @@ -111,7 +109,7 @@ class Polygon2d

std::vector<PointList2d> & inners() noexcept { return inners_; }

autoware::utils::Polygon2d to_boost() const;
autoware_utils::Polygon2d to_boost() const;

protected:
Polygon2d(const PointList2d & outer, const std::vector<PointList2d> & inners)
Expand All @@ -136,7 +134,7 @@ class ConvexPolygon2d : public Polygon2d

static std::optional<ConvexPolygon2d> create(PointList2d && vertices) noexcept;

static std::optional<ConvexPolygon2d> create(const autoware::utils::Polygon2d & polygon) noexcept;
static std::optional<ConvexPolygon2d> create(const autoware_utils::Polygon2d & polygon) noexcept;

const PointList2d & vertices() const noexcept { return outer(); }

Expand Down Expand Up @@ -197,7 +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 utils
} // namespace autoware
} // namespace autoware_utils

#endif // AUTOWARE_UTILS__GEOMETRY__ALT_GEOMETRY_HPP_
20 changes: 8 additions & 12 deletions autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,7 @@

#include <geometry_msgs/msg/point.hpp>

namespace autoware
{

namespace utils
namespace autoware_utils
{
// 2D
struct Point2d;
Expand Down Expand Up @@ -96,13 +93,12 @@ inline Point3d from_msg(const geometry_msgs::msg::Point & msg)
point.z() = msg.z;
return point;
}
} // namespace utils
} // namespace autoware

BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT
autoware::utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT
BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT
autoware::utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT
BOOST_GEOMETRY_REGISTER_RING(autoware::utils::LinearRing2d) // NOLINT
} // namespace autoware_utils

BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT
autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT
BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT
autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT
BOOST_GEOMETRY_REGISTER_RING(autoware_utils::LinearRing2d) // NOLINT

#endif // AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,7 @@

#include <vector>

namespace autoware
{
namespace utils
namespace autoware_utils
{
bool is_clockwise(const Polygon2d & polygon);
Polygon2d inverse_clockwise(const Polygon2d & polygon);
Expand All @@ -49,7 +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 utils
} // namespace autoware
} // namespace autoware_utils

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

namespace autoware
{
namespace utils
namespace autoware_utils
{
struct LinkedPoint
{
Expand Down Expand Up @@ -103,7 +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 utils
} // namespace autoware
} // namespace autoware_utils

#endif // AUTOWARE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_
7 changes: 2 additions & 5 deletions autoware_utils/include/autoware_utils/geometry/geometry.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -96,9 +96,7 @@ inline void doTransform(
#endif
} // namespace tf2

namespace autoware
{
namespace utils
namespace autoware_utils
{
template <class T>
geometry_msgs::msg::Point get_point(const T & p)
Expand Down Expand Up @@ -590,7 +588,6 @@ std::optional<geometry_msgs::msg::Point> intersect(
*/
bool intersects_convex(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);

} // namespace utils
} // namespace autoware
} // namespace autoware_utils

#endif // AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_
4 changes: 2 additions & 2 deletions autoware_utils/include/autoware_utils/geometry/gjk_2d.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,13 @@

#include "autoware_utils/geometry/boost_geometry.hpp"

namespace autoware::utils::gjk
namespace autoware_utils::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::gjk

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

namespace autoware
{
namespace utils
namespace autoware_utils
{
struct PoseDeviation
{
Expand All @@ -41,7 +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 utils
} // namespace autoware
} // namespace autoware_utils

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

namespace autoware
{
namespace utils
namespace autoware_utils
{
/// @brief generate a random non-convex polygon
/// @param vertices number of vertices for the desired polygon
Expand All @@ -39,12 +37,11 @@ std::optional<Polygon2d> random_concave_polygon(const size_t vertices, const dou
/// otherwise false.
/// @return True if at least one pair of polygons intersects, otherwise false.
bool test_intersection(
const std::vector<autoware::utils::Polygon2d> & polygons1,
const std::vector<autoware::utils::Polygon2d> & polygons2,
const std::vector<autoware_utils::Polygon2d> & polygons1,
const std::vector<autoware_utils::Polygon2d> & polygons2,
const std::function<
bool(const autoware::utils::Polygon2d &, const autoware::utils::Polygon2d &)> &);
bool(const autoware_utils::Polygon2d &, const autoware_utils::Polygon2d &)> &);

} // namespace utils
} // namespace autoware
} // namespace autoware_utils

#endif // AUTOWARE_UTILS__GEOMETRY__RANDOM_CONCAVE_POLYGON_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -17,16 +17,13 @@

#include <autoware_utils/geometry/geometry.hpp>

namespace autoware
{
namespace utils
namespace autoware_utils
{
/// @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 utils
} // namespace autoware
} // namespace autoware_utils

#endif // AUTOWARE_UTILS__GEOMETRY__RANDOM_CONVEX_POLYGON_HPP_
4 changes: 2 additions & 2 deletions autoware_utils/include/autoware_utils/geometry/sat_2d.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,14 @@

#include "autoware_utils/geometry/boost_geometry.hpp"

namespace autoware::utils::sat
namespace autoware_utils::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::sat

#endif // AUTOWARE_UTILS__GEOMETRY__SAT_2D_HPP_
7 changes: 2 additions & 5 deletions autoware_utils/include/autoware_utils/math/accumulator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,7 @@
#ifndef AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_
#define AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_

namespace autoware
{
namespace utils
namespace autoware_utils
{
/**
* @brief class to accumulate statistical data, supporting min, max and mean.
Expand Down Expand Up @@ -90,7 +88,6 @@ std::ostream & operator<<(std::ostream & os, const Accumulator<T> & accumulator)
return os;
}

} // namespace utils
} // namespace autoware
} // namespace autoware_utils

#endif // AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_
7 changes: 2 additions & 5 deletions autoware_utils/include/autoware_utils/math/constants.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,10 @@
#ifndef AUTOWARE_UTILS__MATH__CONSTANTS_HPP_
#define AUTOWARE_UTILS__MATH__CONSTANTS_HPP_

namespace autoware
{
namespace utils
namespace autoware_utils
{
constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20
constexpr double gravity = 9.80665;
} // namespace utils
} // namespace autoware
} // namespace autoware_utils

#endif // AUTOWARE_UTILS__MATH__CONSTANTS_HPP_
7 changes: 2 additions & 5 deletions autoware_utils/include/autoware_utils/math/normalization.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,7 @@

#include <cmath>

namespace autoware
{
namespace utils
namespace autoware_utils
{
inline double normalize_degree(const double deg, const double min_deg = -180)
{
Expand All @@ -47,7 +45,6 @@ inline double normalize_radian(const double rad, const double min_rad = -pi)
return value - std::copysign(2 * pi, value);
}

} // namespace utils
} // namespace autoware
} // namespace autoware_utils

#endif // AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_
Loading

0 comments on commit c04c422

Please sign in to comment.