Submission for review

.gitignore

@@ -0,0 +1,3 @@
+build/
+install/
+log/

common/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.22)
+
+project(common)
+
+find_package(ament_cmake REQUIRED)
+
+add_library(common SHARED
+    src/format.cpp
+)
+
+target_include_directories(common
+    PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+ament_export_targets(commonTargets HAS_LIBRARY_TARGET)
+
+install(
+    TARGETS common
+    EXPORT commonTargets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+ament_package()

common/include/common/exception.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <exception>
+#include <sstream>
+#include <utility>
+
+#include "common/format.h"
+
+namespace common {
+
+class Exception : public std::exception {
+  public:
+    Exception() = default;
+
+    Exception(std::string message) : message_(std::move(message)) {}
+
+    Exception(const char* message) : message_(message) {}
+
+    template<typename... Args>
+    Exception(const char* f, const Args&... args) : message_(fmt(f, args...)) {}
+
+    template<typename T>
+    Exception& operator<<(const T& value) {
+        std::ostringstream stream;
+        stream << value;
+        message_ += stream.str();
+        return *this;
+    }
+
+    const char* what() const noexcept override { return message_.c_str(); }
+
+  private:
+    std::string message_;
+};
+
+#define THROW_EXCEPTION() throw Exception() << __FILE__ << ":" << __LINE__
+
+#define VERIFY(expr)                             \
+    [&](auto&& arg) -> decltype(arg) {           \
+        if (!static_cast<bool>(arg)) {           \
+            THROW_EXCEPTION() << ": '" << #expr; \
+        }                                        \
+        return std::forward<decltype(arg)>(arg); \
+    }(expr)
+
+#define VERIFY_STREAM(condition, messages)                           \
+    if (!(condition)) {                                              \
+        THROW_EXCEPTION() << ": " << #condition << "\n" << messages; \
+    }
+
+#define VERIFY_FMT(condition, ...)                                          \
+    if (!(condition)) {                                                     \
+        THROW_EXCEPTION() << ":" << #condition << "\n" << fmt(__VA_ARGS__); \
+    }
+
+#define FALL(...) THROW_EXCEPTION() << ": " << fmt(__VA_ARGS__);
+
+}  // namespace sim2d
+// namespace common = truck;

common/include/common/format.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <string>
+
+namespace common {
+
+std::string fmt(const char* s, ...);
+
+}  // namespace sim2d

common/include/common/math.h

@@ -0,0 +1,75 @@
+#pragma once
+
+#include "common/exception.h"
+
+#include <algorithm>
+#include <cmath>
+
+namespace common {
+
+template<typename T>
+inline T abs(const T& x) {
+    return std::abs(x);
+}
+
+template<typename T>
+inline double squared(const T& x) {
+    return x * x;
+}
+
+template<typename T>
+T clamp(const T& val, const T& low, const T& high) {
+    return std::clamp(val, low, high);
+}
+
+template<typename T>
+T clamp(const T& val, const T& abs_limit) {
+    return clamp(val, -abs_limit, abs_limit);
+}
+
+template<typename T>
+inline constexpr int sign(const T& x) {
+    return (T(0) < x) - (x < T(0));
+}
+
+template<class T>
+struct Limits {
+    Limits() = default;
+
+    Limits(const T& min, const T& max) : min(min), max(max) { VERIFY(min <= max); }
+
+    bool isMet(const T& x) const { return min <= x && x <= max; }
+
+    bool isStrictlyMet(const T& x) const { return min < x && x < max; }
+
+    T clamp(const T& t) const { return common::clamp(t, min, max); }
+
+    double ratio(const T& t) const { return (clamp(t) - min) / (max - min); }
+
+    T min, max;
+};
+
+template<class Int, class Float>
+Int ceil(Float value) {
+    return static_cast<Int>(std::ceil(value));
+}
+
+template<class Int, class Float>
+Int floor(Float value) {
+    return static_cast<Int>(std::floor(value));
+}
+
+template<class Int, class Float>
+Int round(Float value) {
+    return static_cast<Int>(std::round(value));
+}
+
+template<typename T, typename = std::enable_if_t<std::is_integral_v<T>>>
+inline constexpr size_t fls(T value) {
+    size_t i = 0;
+    for (; (value >> i) > 0; ++i) {
+    }
+    return i;
+}
+
+}  // namespace sim2d

common/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>common</name>
+  <version>0.0.0</version>
+  <description>Common utilities - math, exceptions, formatting.</description>
+  <maintainer email="aitsyganok@yandex.ru">Andrey Tsyganok</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/src/format.cpp

@@ -0,0 +1,20 @@
+#include "common/format.h"
+
+#include <cstdarg>
+#include <cstdio>
+
+namespace common {
+
+std::string fmt(const char* s, ...) {
+    va_list args;
+    va_start(args, s);
+
+    char buffer[4096];
+    vsnprintf(buffer, sizeof(buffer), s, args);
+
+    va_end(args);
+
+    return std::string(buffer);
+}
+
+}  // namespace sim2d

geom/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.5)
+project(geom)
+
+find_package(ament_cmake REQUIRED)
+find_package(common REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(Boost REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+    src/angle.cpp
+    src/angle_vector.cpp
+    #src/arc.cpp
+    src/bezier.cpp
+    src/bounding_box.cpp
+    src/circle.cpp
+    src/complex_polygon.cpp
+    src/distance.cpp
+    src/intersection.cpp
+    src/line.cpp
+    src/motion_state.cpp
+    src/msg.cpp
+    src/polygon.cpp
+    #src/polyline.cpp
+    src/pose.cpp
+    src/segment.cpp
+    src/transform.cpp
+    src/vector.cpp
+    src/vector3.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+    common
+    geometry_msgs
+    nav_msgs
+    tf2
+    Boost
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET)
+ament_export_dependencies(common geometry_msgs nav_msgs tf2 Boost)
+
+install(
+    TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}Targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+ament_package()