diff --git a/src/modelec_utils/CMakeLists.txt b/src/modelec_utils/CMakeLists.txt
index 8cf6e42..a084c8d 100644
--- a/src/modelec_utils/CMakeLists.txt
+++ b/src/modelec_utils/CMakeLists.txt
@@ -7,6 +7,7 @@ endif()
 
 # Dependencies
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_gtest REQUIRED)
 
 find_package(tinyxml2 REQUIRED)
 
@@ -33,9 +34,6 @@ target_include_directories(utils PUBLIC
         $<INSTALL_INTERFACE:include>
 )
 
-# Si utils dépend de config, décommente :
-# target_link_libraries(${PROJECT_NAME} PUBLIC modelec_config)
-
 # === Install headers ===
 install(DIRECTORY include/
         DESTINATION include/
@@ -69,6 +67,28 @@ install(EXPORT utilsTargets
         DESTINATION lib/cmake/utils
 )
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(${PROJECT_NAME}_test
+          test/point.test.cpp
+          test/utils.test.cpp
+          test/config.test.cpp
+  )
+
+  target_include_directories(${PROJECT_NAME}_test PUBLIC
+          $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}_test
+          utils
+          config
+          tinyxml2::tinyxml2
+  )
+endif()
+
 # === Export metadata for ament ===
 ament_export_targets(utilsTargets)
 ament_export_targets(configTargets)
diff --git a/src/modelec_utils/package.xml b/src/modelec_utils/package.xml
index b0f43d0..d5494a2 100644
--- a/src/modelec_utils/package.xml
+++ b/src/modelec_utils/package.xml
@@ -11,6 +11,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/src/modelec_utils/test/config.test.cpp b/src/modelec_utils/test/config.test.cpp
new file mode 100644
index 0000000..f856ca3
--- /dev/null
+++ b/src/modelec_utils/test/config.test.cpp
@@ -0,0 +1,50 @@
+#include <gtest/gtest.h>
+#include <fstream>
+#include <string>
+#include "modelec_utils/config.hpp"
+
+TEST(ConfigTest, LoadValidXML)
+{
+    // Create temporary XML file
+    const std::string filepath = "test_config.xml";
+
+    std::ofstream file(filepath);
+    file <<
+        "<root attr1=\"A\" attr2=\"B\">"
+        "   <child1>123</child1>"
+        "   <child2 flag=\"true\">3.14</child2>"
+        "   <nested>"
+        "       <deep>hello</deep>"
+        "   </nested>"
+        "</root>";
+    file.close();
+
+    EXPECT_TRUE(Modelec::Config::load(filepath));
+
+    // Root attributes
+    EXPECT_EQ(Modelec::Config::get<std::string>("root@attr1", ""), "A");
+    EXPECT_EQ(Modelec::Config::get<std::string>("root@attr2", ""), "B");
+
+    // Simple child element
+    EXPECT_EQ(Modelec::Config::get<int>("root.child1", 0), 123);
+
+    // Child with attribute
+    EXPECT_DOUBLE_EQ(Modelec::Config::get<double>("root.child2", 0.0), 3.14);
+    EXPECT_TRUE(Modelec::Config::get<bool>("root.child2@flag", false));
+
+    // Deep nested node
+    EXPECT_EQ(Modelec::Config::get<std::string>("root.nested.deep", ""), "hello");
+}
+
+TEST(ConfigTest, LoadInvalidXML)
+{
+    EXPECT_FALSE(Modelec::Config::load("no_such_file.xml"));
+}
+
+TEST(ConfigTest, DefaultValues)
+{
+    // Key does not exist → default returned
+    EXPECT_EQ(Modelec::Config::get<int>("missing.int", 42), 42);
+    EXPECT_EQ(Modelec::Config::get<std::string>("missing.string", "default"), "default");
+    EXPECT_FALSE(Modelec::Config::get<bool>("missing.bool", false));
+}
diff --git a/src/modelec_utils/test/point.test.cpp b/src/modelec_utils/test/point.test.cpp
new file mode 100644
index 0000000..5c2dbbb
--- /dev/null
+++ b/src/modelec_utils/test/point.test.cpp
@@ -0,0 +1,45 @@
+#include <gtest/gtest.h>
+#include <modelec_utils/point.hpp>
+#include <cmath>
+
+TEST(PointTest, Distance) {
+    Modelec::Point p1(0, 0, 0);
+    Modelec::Point p2(3, 4, 0);
+    double dist = Modelec::Point::distance(p1, p2);
+    EXPECT_DOUBLE_EQ(dist, 5.0);
+}
+
+TEST(PointTest, AngleDiff) {
+    Modelec::Point p1(0, 0, 0);
+    Modelec::Point p2(0, 0, M_PI / 2);
+    double diff = Modelec::Point::angleDiff(p1, p2);
+    EXPECT_NEAR(diff, -M_PI_2, 1e-6);
+}
+
+TEST(PointTest, GetTakePositionWithAngle) {
+    Modelec::Point p(0, 0, 0);
+    Modelec::Point result = p.GetTakePosition(10, 3.141592653589793 / 2);
+    EXPECT_EQ(result.x, 0.0);
+    EXPECT_EQ(result.y, 10.0);
+}
+
+TEST(PointTest, GetTakePositionDefaultAngle) {
+    Modelec::Point p(1, 1, 0);
+    Modelec::Point result = p.GetTakePosition(5);
+    EXPECT_NEAR(result.x, 6.0, 1e-6);
+    EXPECT_NEAR(result.y, 1.0, 1e-6);
+}
+
+TEST(PointTest, GetTakeBasePosition) {
+    Modelec::Point p(0, 0, 0);
+    Modelec::Point base = p.GetTakeBasePosition();
+    EXPECT_EQ(base.x, 290);
+    EXPECT_EQ(base.y, 0);
+}
+
+TEST(PointTest, GetTakeClosePosition) {
+    Modelec::Point p(0, 0, 0);
+    Modelec::Point close = p.GetTakeClosePosition();
+    EXPECT_EQ(close.x, 150);
+    EXPECT_EQ(close.y, 0);
+}
diff --git a/src/modelec_utils/test/utils.test.cpp b/src/modelec_utils/test/utils.test.cpp
new file mode 100644
index 0000000..1268771
--- /dev/null
+++ b/src/modelec_utils/test/utils.test.cpp
@@ -0,0 +1,54 @@
+#include <gtest/gtest.h>
+#include <modelec_utils/utils.hpp>
+
+TEST(UtilsTest, Split) {
+    std::string s = "a,b,c";
+    std::vector<std::string> result = Modelec::split(s, ',');
+    ASSERT_EQ(result.size(), 3);
+    EXPECT_EQ(result[0], "a");
+    EXPECT_EQ(result[1], "b");
+    EXPECT_EQ(result[2], "c");
+}
+
+TEST(UtilsTest, Join) {
+    std::vector<std::string> v{"a", "b", "c"};
+    std::string result = Modelec::join(v, ",");
+    EXPECT_EQ(result, "a,b,c");
+}
+
+TEST(UtilsTest, StartsWith) {
+    EXPECT_TRUE(Modelec::startsWith("hello world", "hello"));
+    EXPECT_FALSE(Modelec::startsWith("hello world", "world"));
+}
+
+TEST(UtilsTest, EndsWith) {
+    EXPECT_TRUE(Modelec::endsWith("hello world", "world"));
+    EXPECT_FALSE(Modelec::endsWith("hello world", "hello"));
+}
+
+TEST(UtilsTest, Contains) {
+    EXPECT_TRUE(Modelec::contains("hello world", "lo wo"));
+    EXPECT_FALSE(Modelec::contains("hello world", "bye"));
+}
+
+TEST(UtilsTest, Trim) {
+    EXPECT_EQ(Modelec::trim("   hello   "), "hello");
+    EXPECT_EQ(Modelec::trim("\t\n hello \r\n"), "hello");
+    EXPECT_EQ(Modelec::trim(""), "");
+    EXPECT_EQ(Modelec::trim("   "), "");
+}
+
+TEST(UtilsTest, MapValueInt) {
+    int result = Modelec::mapValue<int>(5, 0, 10, 0, 100);
+    EXPECT_EQ(result, 50);
+}
+
+TEST(UtilsTest, MapValueDouble) {
+    auto result = Modelec::mapValue<double>(2.5, 0.0, 5.0, 0.0, 10.0);
+    EXPECT_DOUBLE_EQ(result, 5.0);
+}
+
+TEST(UtilsTest, MapValueNegative) {
+    auto result = Modelec::mapValue<double>(-5.0, -10.0, 0.0, 0.0, 100.0);
+    EXPECT_DOUBLE_EQ(result, 50.0);
+}
\ No newline at end of file