diff --git a/src/nebula/CMakeLists.txt b/src/nebula/CMakeLists.txt
index d791ea8cd..8932a7e4e 100644
--- a/src/nebula/CMakeLists.txt
+++ b/src/nebula/CMakeLists.txt
@@ -11,7 +11,7 @@ if (BUILD_TESTING)
     foreach(MODEL Pandar40P Pandar64 PandarQT64 PandarQT128 Pandar128E4X PandarAT128 PandarXT16 PandarXT32 PandarXT32M ARS548 SRR520 VLP16 VLP32 VLS128 Helios Bpearl)
         string(TOLOWER ${MODEL}_smoke_test test_name)
         add_ros_test(
-            test/smoke_test.py
+            tests/smoke_test.py
             TARGET ${test_name}
             ARGS "launch_file_path:=${PROJECT_SOURCE_DIR}/launch/nebula_launch.py" "sensor_model:=${MODEL}"
             TIMEOUT "10"
diff --git a/src/nebula/test/smoke_test.py b/src/nebula/tests/smoke_test.py
similarity index 100%
rename from src/nebula/test/smoke_test.py
rename to src/nebula/tests/smoke_test.py
diff --git a/src/nebula_core/nebula_core_common/CMakeLists.txt b/src/nebula_core/nebula_core_common/CMakeLists.txt
index 8747817f1..24c0090c0 100644
--- a/src/nebula_core/nebula_core_common/CMakeLists.txt
+++ b/src/nebula_core/nebula_core_common/CMakeLists.txt
@@ -34,11 +34,53 @@ if(BUILD_TESTING)
     ament_lint_auto_find_test_dependencies()
 
     ament_add_gtest(test_rate_checker
-        test/test_rate_checker.cpp
+        tests/test_rate_checker.cpp
     )
     target_link_libraries(test_rate_checker
         nebula_core_common
     )
+
+    ament_add_gtest(test_ring_buffer
+        tests/test_ring_buffer.cpp
+    )
+    target_link_libraries(test_ring_buffer
+        nebula_core_common
+    )
+
+    ament_add_gtest(test_bitfield
+        tests/test_bitfield.cpp
+    )
+    target_link_libraries(test_bitfield
+        nebula_core_common
+    )
+
+    ament_add_gtest(test_expected
+        tests/test_expected.cpp
+    )
+    target_link_libraries(test_expected
+        nebula_core_common
+    )
+
+    ament_add_gtest(test_rate_limiter
+        tests/test_rate_limiter.cpp
+    )
+    target_link_libraries(test_rate_limiter
+        nebula_core_common
+    )
+
+    ament_add_gtest(test_string_conversions
+        tests/test_string_conversions.cpp
+    )
+    target_link_libraries(test_string_conversions
+        nebula_core_common
+    )
+
+    ament_add_gtest(test_nebula_common
+        tests/test_nebula_common.cpp
+    )
+    target_link_libraries(test_nebula_common
+        nebula_core_common
+    )
 endif()
 
 install(TARGETS nebula_core_common EXPORT export_nebula_core_common)
diff --git a/src/nebula_core/nebula_core_common/include/nebula_core_common/util/bitfield.hpp b/src/nebula_core/nebula_core_common/include/nebula_core_common/util/bitfield.hpp
index c0992cf48..d61d501ff 100644
--- a/src/nebula_core/nebula_core_common/include/nebula_core_common/util/bitfield.hpp
+++ b/src/nebula_core/nebula_core_common/include/nebula_core_common/util/bitfield.hpp
@@ -14,7 +14,9 @@
 
 #pragma once
 
-#include <cstdint>
+#include <cstddef>
+#include <limits>
+
 namespace nebula::util
 {
 
@@ -38,12 +40,14 @@ namespace nebula::util
  * @param storage The variable acting as the storage for the bitfield
  * @return constexpr OutT The extracted value
  */
-template <typename OutT, uint8_t LowBit, uint8_t HighBit, typename InT>
+template <typename OutT, size_t LowBit, size_t HighBit, typename InT>
 constexpr OutT get_bitfield(const InT & storage)
 {
-  constexpr uint8_t n_bits = HighBit - LowBit + 1;
+  constexpr size_t n_bits = HighBit - LowBit + 1;
   constexpr InT all_ones = ~static_cast<InT>(0);
-  constexpr InT mask = ~(all_ones << n_bits);
+  constexpr InT mask = (n_bits < std::numeric_limits<InT>::digits)
+                         ? static_cast<InT>(~static_cast<InT>(all_ones << n_bits))
+                         : all_ones;
 
   InT raw_value = (storage >> LowBit) & mask;
   return static_cast<OutT>(raw_value);
diff --git a/src/nebula_core/nebula_core_common/tests/test_bitfield.cpp b/src/nebula_core/nebula_core_common/tests/test_bitfield.cpp
new file mode 100644
index 000000000..26698bcd6
--- /dev/null
+++ b/src/nebula_core/nebula_core_common/tests/test_bitfield.cpp
@@ -0,0 +1,144 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_common/util/bitfield.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cstdint>
+
+using nebula::util::get_bitfield;
+
+TEST(BitfieldTest, SingleBitExtraction)
+{
+  uint8_t value = 0b10101010;
+
+  EXPECT_EQ((get_bitfield<uint8_t, 0, 0>(value)), 0);
+  EXPECT_EQ((get_bitfield<uint8_t, 1, 1>(value)), 1);
+  EXPECT_EQ((get_bitfield<uint8_t, 2, 2>(value)), 0);
+  EXPECT_EQ((get_bitfield<uint8_t, 3, 3>(value)), 1);
+  EXPECT_EQ((get_bitfield<uint8_t, 4, 4>(value)), 0);
+  EXPECT_EQ((get_bitfield<uint8_t, 5, 5>(value)), 1);
+  EXPECT_EQ((get_bitfield<uint8_t, 6, 6>(value)), 0);
+  EXPECT_EQ((get_bitfield<uint8_t, 7, 7>(value)), 1);
+}
+
+TEST(BitfieldTest, MultiBitExtraction)
+{
+  uint8_t value = 0b11010110;  // = 214
+
+  // Extract bits [0, 3] = 0b0110 = 6
+  EXPECT_EQ((get_bitfield<uint8_t, 0, 3>(value)), 6);
+
+  // Extract bits [4, 7] = 0b1101 = 13
+  EXPECT_EQ((get_bitfield<uint8_t, 4, 7>(value)), 13);
+
+  // Extract bits [2, 5] = 0b0101 = 5
+  EXPECT_EQ((get_bitfield<uint8_t, 2, 5>(value)), 5);
+}
+
+TEST(BitfieldTest, FullWidthExtraction)
+{
+  uint8_t value8 = 0xAB;
+  EXPECT_EQ((get_bitfield<uint8_t, 0, 7>(value8)), 0xAB);
+
+  uint16_t value16 = 0xABCD;
+  EXPECT_EQ((get_bitfield<uint16_t, 0, 15>(value16)), 0xABCD);
+
+  uint32_t value32 = 0x1234'5678;
+  EXPECT_EQ((get_bitfield<uint32_t, 0, 31>(value32)), 0x1234'5678);
+}
+
+TEST(BitfieldTest, Uint16Extraction)
+{
+  uint16_t value = 0xABCD;  // 1010101111001101
+
+  // Extract lower byte
+  EXPECT_EQ((get_bitfield<uint8_t, 0, 7>(value)), 0xCD);
+
+  // Extract upper byte
+  EXPECT_EQ((get_bitfield<uint8_t, 8, 15>(value)), 0xAB);
+
+  // Extract middle bits [4, 11]
+  EXPECT_EQ((get_bitfield<uint8_t, 4, 11>(value)), 0xBC);
+}
+
+TEST(BitfieldTest, Uint32Extraction)
+{
+  uint32_t value = 0x12345678;
+
+  // Extract each byte
+  EXPECT_EQ((get_bitfield<uint8_t, 0, 7>(value)), 0x78);
+  EXPECT_EQ((get_bitfield<uint8_t, 8, 15>(value)), 0x56);
+  EXPECT_EQ((get_bitfield<uint8_t, 16, 23>(value)), 0x34);
+  EXPECT_EQ((get_bitfield<uint8_t, 24, 31>(value)), 0x12);
+
+  // Extract 16-bit values
+  EXPECT_EQ((get_bitfield<uint16_t, 0, 15>(value)), 0x5678);
+  EXPECT_EQ((get_bitfield<uint16_t, 16, 31>(value)), 0x1234);
+}
+
+enum class TestEnum : uint8_t { VALUE_0 = 0, VALUE_1 = 1, VALUE_5 = 5, VALUE_15 = 15 };
+
+TEST(BitfieldTest, EnumOutput)
+{
+  uint8_t value = 0b01010001;  // bits[0,3] = 1, bits[4,7] = 5
+
+  EXPECT_EQ((get_bitfield<TestEnum, 0, 3>(value)), TestEnum::VALUE_1);
+  EXPECT_EQ((get_bitfield<TestEnum, 4, 7>(value)), TestEnum::VALUE_5);
+}
+
+TEST(BitfieldTest, ZeroValue)
+{
+  uint32_t value = 0;
+
+  EXPECT_EQ((get_bitfield<uint8_t, 0, 7>(value)), 0);
+  EXPECT_EQ((get_bitfield<uint16_t, 8, 23>(value)), 0);
+  EXPECT_EQ((get_bitfield<uint32_t, 0, 31>(value)), 0);
+}
+
+TEST(BitfieldTest, AllOnesValue)
+{
+  uint8_t value8 = 0xFF;
+  EXPECT_EQ((get_bitfield<uint8_t, 0, 3>(value8)), 0x0F);
+  EXPECT_EQ((get_bitfield<uint8_t, 4, 7>(value8)), 0x0F);
+
+  uint16_t value16 = 0xFFFF;
+  EXPECT_EQ((get_bitfield<uint8_t, 0, 7>(value16)), 0xFF);
+  EXPECT_EQ((get_bitfield<uint16_t, 0, 15>(value16)), 0xFFFF);
+}
+
+TEST(BitfieldTest, MacroAccessor)
+{
+  struct TestStruct
+  {
+    uint16_t storage;
+
+    BITFIELD_ACCESSOR(uint8_t, low_nibble, 0, 3, storage)
+    BITFIELD_ACCESSOR(uint8_t, high_nibble, 4, 7, storage)
+    BITFIELD_ACCESSOR(uint8_t, upper_byte, 8, 15, storage)
+  };
+
+  TestStruct s{0xABCD};
+
+  EXPECT_EQ(s.low_nibble(), 0x0D);
+  EXPECT_EQ(s.high_nibble(), 0x0C);
+  EXPECT_EQ(s.upper_byte(), 0xAB);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_common/tests/test_expected.cpp b/src/nebula_core/nebula_core_common/tests/test_expected.cpp
new file mode 100644
index 000000000..cf3a2feea
--- /dev/null
+++ b/src/nebula_core/nebula_core_common/tests/test_expected.cpp
@@ -0,0 +1,164 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_common/util/expected.hpp"
+
+#include <gtest/gtest.h>
+
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+using nebula::util::bad_expected_access;
+using nebula::util::expected;
+
+class TestError : public std::exception
+{
+  std::string message_;
+
+public:
+  explicit TestError(std::string msg) : message_(std::move(msg)) {}
+  [[nodiscard]] const char * what() const noexcept override { return message_.c_str(); }
+};
+
+TEST(ExpectedTest, HasValueWhenContainingValue)
+{
+  expected<int, std::string> e = 42;
+  EXPECT_TRUE(e.has_value());
+}
+
+TEST(ExpectedTest, HasValueFalseWhenContainingError)
+{
+  expected<int, std::string> e = std::string{"error"};
+  EXPECT_FALSE(e.has_value());
+}
+
+TEST(ExpectedTest, ValueReturnsContainedValue)
+{
+  expected<int, std::string> e = 42;
+  EXPECT_EQ(e.value(), 42);
+}
+
+TEST(ExpectedTest, ValueThrowsWhenContainingError)
+{
+  expected<int, std::string> e = std::string{"error message"};
+  EXPECT_THROW(static_cast<void>(e.value()), bad_expected_access);
+}
+
+TEST(ExpectedTest, ErrorReturnsContainedError)
+{
+  expected<int, std::string> e = std::string{"my error"};
+  EXPECT_EQ(e.error(), "my error");
+}
+
+TEST(ExpectedTest, ErrorThrowsWhenContainingValue)
+{
+  expected<int, std::string> e = 42;
+  EXPECT_THROW(static_cast<void>(e.error()), bad_expected_access);
+}
+
+TEST(ExpectedTest, ValueOrReturnsValueWhenPresent)
+{
+  expected<int, std::string> e = 42;
+  EXPECT_EQ(e.value_or(100), 42);
+}
+
+TEST(ExpectedTest, ValueOrReturnsDefaultWhenError)
+{
+  expected<int, std::string> e = std::string{"error"};
+  EXPECT_EQ(e.value_or(100), 100);
+}
+
+TEST(ExpectedTest, ErrorOrReturnsErrorWhenPresent)
+{
+  expected<int, std::string> e = std::string{"my error"};
+  EXPECT_EQ(e.error_or("default error"), "my error");
+}
+
+TEST(ExpectedTest, ErrorOrReturnsDefaultWhenValue)
+{
+  expected<int, std::string> e = 42;
+  EXPECT_EQ(e.error_or("default error"), "default error");
+}
+
+TEST(ExpectedTest, ValueOrThrowWithMessageThrowsRuntimeError)
+{
+  expected<int, std::string> e = std::string{"internal error"};
+  EXPECT_THROW(static_cast<void>(e.value_or_throw("custom error message")), std::runtime_error);
+
+  try {
+    e.value_or_throw("custom error message");
+    FAIL() << "Expected std::runtime_error";
+  } catch (const std::runtime_error & ex) {
+    EXPECT_STREQ(ex.what(), "custom error message");
+  }
+}
+
+TEST(ExpectedTest, ValueOrThrowWithMessageReturnsValueWhenPresent)
+{
+  expected<int, std::string> e = 42;
+  EXPECT_EQ(e.value_or_throw("should not throw"), 42);
+}
+
+TEST(ExpectedTest, ValueOrThrowThrowsStoredError)
+{
+  expected<int, TestError> e = TestError("stored error");
+  EXPECT_THROW(static_cast<void>(e.value_or_throw()), TestError);
+
+  try {
+    e.value_or_throw();
+    FAIL() << "Expected TestError";
+  } catch (const TestError & ex) {
+    EXPECT_STREQ(ex.what(), "stored error");
+  }
+}
+
+TEST(ExpectedTest, ValueOrThrowReturnsValueWhenPresent)
+{
+  expected<int, TestError> e = 42;
+  EXPECT_EQ(e.value_or_throw(), 42);
+}
+
+TEST(ExpectedTest, WorksWithStringValue)
+{
+  expected<std::string, int> e = std::string{"hello"};
+  EXPECT_TRUE(e.has_value());
+  EXPECT_EQ(e.value(), "hello");
+}
+
+TEST(ExpectedTest, WorksWithComplexTypes)
+{
+  struct ComplexValue
+  {
+    int a;
+    std::string b;
+  };
+
+  expected<ComplexValue, std::string> e = ComplexValue{42, "test"};
+  EXPECT_TRUE(e.has_value());
+  EXPECT_EQ(e.value().a, 42);
+  EXPECT_EQ(e.value().b, "test");
+}
+
+TEST(ExpectedTest, BadExpectedAccessHasMessage)
+{
+  bad_expected_access ex("test message");
+  EXPECT_STREQ(ex.what(), "test message");
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_common/tests/test_nebula_common.cpp b/src/nebula_core/nebula_core_common/tests/test_nebula_common.cpp
new file mode 100644
index 000000000..ab05d51a4
--- /dev/null
+++ b/src/nebula_core/nebula_core_common/tests/test_nebula_common.cpp
@@ -0,0 +1,282 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_common/nebula_common.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <sstream>
+#include <string>
+
+using nebula::drivers::deg2rad;
+using nebula::drivers::rad2deg;
+using nebula::drivers::return_mode_from_string;
+using nebula::drivers::ReturnMode;
+using nebula::drivers::ReturnType;
+using nebula::drivers::rpm2hz;
+using nebula::drivers::sensor_model_from_string;
+using nebula::drivers::sensor_model_to_string;
+using nebula::drivers::SensorModel;
+
+// ============================================================================
+// sensor_model_from_string tests
+// ============================================================================
+
+TEST(NebulaCommonTest, SensorModelFromStringHesai)
+{
+  EXPECT_EQ(sensor_model_from_string("Pandar64"), SensorModel::HESAI_PANDAR64);
+  EXPECT_EQ(sensor_model_from_string("Pandar40P"), SensorModel::HESAI_PANDAR40P);
+  EXPECT_EQ(sensor_model_from_string("Pandar40M"), SensorModel::HESAI_PANDAR40M);
+  EXPECT_EQ(sensor_model_from_string("PandarXT16"), SensorModel::HESAI_PANDARXT16);
+  EXPECT_EQ(sensor_model_from_string("PandarXT32"), SensorModel::HESAI_PANDARXT32);
+  EXPECT_EQ(sensor_model_from_string("PandarXT32M"), SensorModel::HESAI_PANDARXT32M);
+  EXPECT_EQ(sensor_model_from_string("PandarAT128"), SensorModel::HESAI_PANDARAT128);
+  EXPECT_EQ(sensor_model_from_string("PandarQT64"), SensorModel::HESAI_PANDARQT64);
+  EXPECT_EQ(sensor_model_from_string("PandarQT128"), SensorModel::HESAI_PANDARQT128);
+  EXPECT_EQ(sensor_model_from_string("Pandar128E4X"), SensorModel::HESAI_PANDAR128_E4X);
+}
+
+TEST(NebulaCommonTest, SensorModelFromStringVelodyne)
+{
+  EXPECT_EQ(sensor_model_from_string("VLS128"), SensorModel::VELODYNE_VLS128);
+  EXPECT_EQ(sensor_model_from_string("HDL64"), SensorModel::VELODYNE_HDL64);
+  EXPECT_EQ(sensor_model_from_string("VLP32"), SensorModel::VELODYNE_VLP32);
+  EXPECT_EQ(sensor_model_from_string("VLP32MR"), SensorModel::VELODYNE_VLP32MR);
+  EXPECT_EQ(sensor_model_from_string("HDL32"), SensorModel::VELODYNE_HDL32);
+  EXPECT_EQ(sensor_model_from_string("VLP16"), SensorModel::VELODYNE_VLP16);
+}
+
+TEST(NebulaCommonTest, SensorModelFromStringRobosense)
+{
+  EXPECT_EQ(sensor_model_from_string("Helios"), SensorModel::ROBOSENSE_HELIOS);
+  EXPECT_EQ(sensor_model_from_string("Bpearl"), SensorModel::ROBOSENSE_BPEARL_V4);
+  EXPECT_EQ(sensor_model_from_string("Bpearl_V4"), SensorModel::ROBOSENSE_BPEARL_V4);
+  EXPECT_EQ(sensor_model_from_string("Bpearl_V3"), SensorModel::ROBOSENSE_BPEARL_V3);
+}
+
+TEST(NebulaCommonTest, SensorModelFromStringContinental)
+{
+  EXPECT_EQ(sensor_model_from_string("ARS548"), SensorModel::CONTINENTAL_ARS548);
+  EXPECT_EQ(sensor_model_from_string("SRR520"), SensorModel::CONTINENTAL_SRR520);
+}
+
+TEST(NebulaCommonTest, SensorModelFromStringUnknown)
+{
+  EXPECT_EQ(sensor_model_from_string("InvalidSensor"), SensorModel::UNKNOWN);
+  EXPECT_EQ(sensor_model_from_string(""), SensorModel::UNKNOWN);
+  EXPECT_EQ(sensor_model_from_string("pandar64"), SensorModel::UNKNOWN);  // Case sensitive
+}
+
+// ============================================================================
+// sensor_model_to_string tests
+// ============================================================================
+
+TEST(NebulaCommonTest, SensorModelToStringHesai)
+{
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDAR64), "Pandar64");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDAR40P), "Pandar40P");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDAR40M), "Pandar40M");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDARXT16), "PandarXT16");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDARXT32), "PandarXT32");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDARXT32M), "PandarXT32M");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDARAT128), "PandarAT128");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDARQT64), "PandarQT64");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDARQT128), "PandarQT128");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::HESAI_PANDAR128_E4X), "Pandar128E4X");
+}
+
+TEST(NebulaCommonTest, SensorModelToStringVelodyne)
+{
+  EXPECT_EQ(sensor_model_to_string(SensorModel::VELODYNE_VLS128), "VLS128");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::VELODYNE_HDL64), "HDL64");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::VELODYNE_VLP32), "VLP32");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::VELODYNE_VLP32MR), "VLP32MR");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::VELODYNE_HDL32), "HDL32");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::VELODYNE_VLP16), "VLP16");
+}
+
+TEST(NebulaCommonTest, SensorModelToStringRobosense)
+{
+  EXPECT_EQ(sensor_model_to_string(SensorModel::ROBOSENSE_HELIOS), "Helios");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::ROBOSENSE_BPEARL_V3), "Bpearl_V3");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::ROBOSENSE_BPEARL_V4), "Bpearl_V4");
+}
+
+TEST(NebulaCommonTest, SensorModelToStringContinental)
+{
+  EXPECT_EQ(sensor_model_to_string(SensorModel::CONTINENTAL_ARS548), "ARS548");
+  EXPECT_EQ(sensor_model_to_string(SensorModel::CONTINENTAL_SRR520), "SRR520");
+}
+
+TEST(NebulaCommonTest, SensorModelToStringUnknown)
+{
+  EXPECT_EQ(sensor_model_to_string(SensorModel::UNKNOWN), "UNKNOWN");
+}
+
+// ============================================================================
+// sensor_model roundtrip tests
+// ============================================================================
+
+TEST(NebulaCommonTest, SensorModelRoundtrip)
+{
+  // Test that to_string -> from_string -> to_string is consistent
+  auto test_roundtrip = [](SensorModel model) {
+    std::string str = sensor_model_to_string(model);
+    SensorModel parsed = sensor_model_from_string(str);
+    EXPECT_EQ(parsed, model) << "Roundtrip failed for: " << str;
+  };
+
+  test_roundtrip(SensorModel::HESAI_PANDAR64);
+  test_roundtrip(SensorModel::HESAI_PANDAR40P);
+  test_roundtrip(SensorModel::VELODYNE_VLP16);
+  test_roundtrip(SensorModel::VELODYNE_VLS128);
+  test_roundtrip(SensorModel::ROBOSENSE_HELIOS);
+  test_roundtrip(SensorModel::CONTINENTAL_ARS548);
+}
+
+// ============================================================================
+// return_mode_from_string tests
+// ============================================================================
+
+TEST(NebulaCommonTest, ReturnModeFromString)
+{
+  EXPECT_EQ(return_mode_from_string("SingleFirst"), ReturnMode::SINGLE_FIRST);
+  EXPECT_EQ(return_mode_from_string("SingleStrongest"), ReturnMode::SINGLE_STRONGEST);
+  EXPECT_EQ(return_mode_from_string("SingleLast"), ReturnMode::SINGLE_LAST);
+  EXPECT_EQ(return_mode_from_string("Dual"), ReturnMode::DUAL_ONLY);
+}
+
+TEST(NebulaCommonTest, ReturnModeFromStringUnknown)
+{
+  EXPECT_EQ(return_mode_from_string("InvalidMode"), ReturnMode::UNKNOWN);
+  EXPECT_EQ(return_mode_from_string(""), ReturnMode::UNKNOWN);
+  EXPECT_EQ(return_mode_from_string("singlefirst"), ReturnMode::UNKNOWN);  // Case sensitive
+}
+
+// ============================================================================
+// deg2rad / rad2deg tests
+// ============================================================================
+
+TEST(NebulaCommonTest, Deg2Rad)
+{
+  EXPECT_FLOAT_EQ(deg2rad(0.0), 0.0f);
+  EXPECT_FLOAT_EQ(deg2rad(180.0), static_cast<float>(M_PI));
+  EXPECT_FLOAT_EQ(deg2rad(360.0), static_cast<float>(2 * M_PI));
+  EXPECT_FLOAT_EQ(deg2rad(90.0), static_cast<float>(M_PI / 2));
+  EXPECT_FLOAT_EQ(deg2rad(-180.0), static_cast<float>(-M_PI));
+}
+
+TEST(NebulaCommonTest, Rad2Deg)
+{
+  EXPECT_FLOAT_EQ(rad2deg(0.0), 0.0f);
+  EXPECT_FLOAT_EQ(rad2deg(M_PI), 180.0f);
+  EXPECT_FLOAT_EQ(rad2deg(2 * M_PI), 360.0f);
+  EXPECT_FLOAT_EQ(rad2deg(M_PI / 2), 90.0f);
+  EXPECT_FLOAT_EQ(rad2deg(-M_PI), -180.0f);
+}
+
+TEST(NebulaCommonTest, Deg2RadRad2DegRoundtrip)
+{
+  for (double deg = -360.0; deg <= 360.0; deg += 45.0) {
+    float rad = deg2rad(deg);
+    float back = rad2deg(rad);
+    EXPECT_NEAR(back, deg, 1e-5) << "Roundtrip failed for: " << deg;
+  }
+}
+
+// ============================================================================
+// rpm2hz tests
+// ============================================================================
+
+TEST(NebulaCommonTest, Rpm2Hz)
+{
+  EXPECT_DOUBLE_EQ(rpm2hz(60.0), 1.0);
+  EXPECT_DOUBLE_EQ(rpm2hz(120.0), 2.0);
+  EXPECT_DOUBLE_EQ(rpm2hz(600.0), 10.0);
+  EXPECT_DOUBLE_EQ(rpm2hz(0.0), 0.0);
+  EXPECT_DOUBLE_EQ(rpm2hz(30.0), 0.5);
+}
+
+// ============================================================================
+// Stream operator tests for ReturnType
+// ============================================================================
+
+TEST(NebulaCommonTest, ReturnTypeStreamOperator)
+{
+  auto to_string = [](ReturnType rt) {
+    std::stringstream ss;
+    ss << rt;
+    return ss.str();
+  };
+
+  EXPECT_EQ(to_string(ReturnType::UNKNOWN), "Unknown");
+  EXPECT_EQ(to_string(ReturnType::LAST), "Last");
+  EXPECT_EQ(to_string(ReturnType::FIRST), "First");
+  EXPECT_EQ(to_string(ReturnType::STRONGEST), "Strongest");
+  EXPECT_EQ(to_string(ReturnType::FIRST_WEAK), "FirstWeak");
+  EXPECT_EQ(to_string(ReturnType::LAST_WEAK), "LastWeak");
+  EXPECT_EQ(to_string(ReturnType::IDENTICAL), "Identical");
+  EXPECT_EQ(to_string(ReturnType::SECOND), "Second");
+  EXPECT_EQ(to_string(ReturnType::SECONDSTRONGEST), "SecondStrongest");
+  EXPECT_EQ(to_string(ReturnType::FIRST_STRONGEST), "FirstStrongest");
+  EXPECT_EQ(to_string(ReturnType::LAST_STRONGEST), "LastStrongest");
+}
+
+// ============================================================================
+// Stream operator tests for ReturnMode
+// ============================================================================
+
+TEST(NebulaCommonTest, ReturnModeStreamOperator)
+{
+  auto to_string = [](ReturnMode rm) {
+    std::stringstream ss;
+    ss << rm;
+    return ss.str();
+  };
+
+  EXPECT_EQ(to_string(ReturnMode::UNKNOWN), "Unknown");
+  EXPECT_EQ(to_string(ReturnMode::SINGLE_FIRST), "SingleFirst");
+  EXPECT_EQ(to_string(ReturnMode::SINGLE_STRONGEST), "SingleStrongest");
+  EXPECT_EQ(to_string(ReturnMode::SINGLE_LAST), "SingleLast");
+  EXPECT_EQ(to_string(ReturnMode::DUAL_ONLY), "Dual");
+  EXPECT_EQ(to_string(ReturnMode::DUAL_FIRST), "DualFirst");
+  EXPECT_EQ(to_string(ReturnMode::DUAL_LAST), "DualLast");
+  EXPECT_EQ(to_string(ReturnMode::TRIPLE), "Triple");
+}
+
+// ============================================================================
+// Stream operator tests for SensorModel
+// ============================================================================
+
+TEST(NebulaCommonTest, SensorModelStreamOperator)
+{
+  auto to_string = [](SensorModel sm) {
+    std::stringstream ss;
+    ss << sm;
+    return ss.str();
+  };
+
+  EXPECT_EQ(to_string(SensorModel::UNKNOWN), "Sensor Unknown");
+  EXPECT_EQ(to_string(SensorModel::HESAI_PANDAR64), "Pandar64");
+  EXPECT_EQ(to_string(SensorModel::VELODYNE_VLP16), "VLP16");
+  EXPECT_EQ(to_string(SensorModel::ROBOSENSE_HELIOS), "HELIOS");
+  EXPECT_EQ(to_string(SensorModel::CONTINENTAL_ARS548), "ARS548");
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_common/test/test_rate_checker.cpp b/src/nebula_core/nebula_core_common/tests/test_rate_checker.cpp
similarity index 100%
rename from src/nebula_core/nebula_core_common/test/test_rate_checker.cpp
rename to src/nebula_core/nebula_core_common/tests/test_rate_checker.cpp
diff --git a/src/nebula_core/nebula_core_common/tests/test_rate_limiter.cpp b/src/nebula_core/nebula_core_common/tests/test_rate_limiter.cpp
new file mode 100644
index 000000000..e4214ac6f
--- /dev/null
+++ b/src/nebula_core/nebula_core_common/tests/test_rate_limiter.cpp
@@ -0,0 +1,136 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_common/util/rate_limiter.hpp"
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+
+using nebula::util::RateLimiter;
+
+// Note: The RateLimiter blocks calls if (now_ns - last_passed_time_ns_) < rate_limit_ns_
+// Since last_passed_time_ns_ starts at 0, the first call at time 0 is blocked for non-zero rate
+// limits. This means calls only pass when now_ns >= rate_limit_ns_ (after the rate limit period has
+// elapsed).
+
+TEST(RateLimiterTest, FirstCallAtRateLimitPasses)
+{
+  RateLimiter limiter(std::chrono::milliseconds(100));
+
+  int call_count = 0;
+  // First call at exactly the rate limit time passes
+  limiter.with_rate_limit(100'000'000, [&call_count]() { call_count++; });  // 100ms in ns
+
+  EXPECT_EQ(call_count, 1);
+}
+
+TEST(RateLimiterTest, CallBeforeRateLimitBlocked)
+{
+  RateLimiter limiter(std::chrono::milliseconds(100));
+
+  int call_count = 0;
+  auto action = [&call_count]() { call_count++; };
+
+  // Calls before the rate limit time are blocked
+  limiter.with_rate_limit(0, action);
+  EXPECT_EQ(call_count, 0);
+
+  limiter.with_rate_limit(50'000'000, action);  // 50ms
+  EXPECT_EQ(call_count, 0);
+
+  limiter.with_rate_limit(99'999'999, action);  // Just under 100ms
+  EXPECT_EQ(call_count, 0);
+}
+
+TEST(RateLimiterTest, CallAtRateLimitPasses)
+{
+  RateLimiter limiter(std::chrono::milliseconds(100));
+
+  int call_count = 0;
+  auto action = [&call_count]() { call_count++; };
+
+  // Call at exactly 100ms passes
+  limiter.with_rate_limit(100'000'000, action);
+  EXPECT_EQ(call_count, 1);
+}
+
+TEST(RateLimiterTest, MultipleCallsOverTime)
+{
+  RateLimiter limiter(std::chrono::milliseconds(50));
+
+  int call_count = 0;
+  auto action = [&call_count]() { call_count++; };
+
+  // t=50ms: passes (first at rate limit)
+  limiter.with_rate_limit(50'000'000, action);
+  EXPECT_EQ(call_count, 1);
+
+  // t=75ms: blocked (only 25ms since last pass)
+  limiter.with_rate_limit(75'000'000, action);
+  EXPECT_EQ(call_count, 1);
+
+  // t=100ms: passes (50ms since last pass)
+  limiter.with_rate_limit(100'000'000, action);
+  EXPECT_EQ(call_count, 2);
+
+  // t=125ms: blocked (25ms since last pass)
+  limiter.with_rate_limit(125'000'000, action);
+  EXPECT_EQ(call_count, 2);
+
+  // t=150ms: passes (50ms since last pass)
+  limiter.with_rate_limit(150'000'000, action);
+  EXPECT_EQ(call_count, 3);
+
+  // t=200ms: passes (50ms since last pass)
+  limiter.with_rate_limit(200'000'000, action);
+  EXPECT_EQ(call_count, 4);
+}
+
+TEST(RateLimiterTest, ZeroRateLimit)
+{
+  RateLimiter limiter(std::chrono::milliseconds(0));
+
+  int call_count = 0;
+  auto action = [&call_count]() { call_count++; };
+
+  // With zero rate limit, all calls pass
+  limiter.with_rate_limit(0, action);
+  limiter.with_rate_limit(0, action);
+  limiter.with_rate_limit(0, action);
+
+  EXPECT_EQ(call_count, 3);
+}
+
+TEST(RateLimiterTest, LargeTimeGap)
+{
+  RateLimiter limiter(std::chrono::milliseconds(100));
+
+  int call_count = 0;
+  auto action = [&call_count]() { call_count++; };
+
+  // First call at rate limit
+  limiter.with_rate_limit(100'000'000, action);
+  EXPECT_EQ(call_count, 1);
+
+  // Call after a very long time
+  limiter.with_rate_limit(1'000'000'000'000ULL, action);  // 1000 seconds in ns
+  EXPECT_EQ(call_count, 2);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_common/tests/test_ring_buffer.cpp b/src/nebula_core/nebula_core_common/tests/test_ring_buffer.cpp
new file mode 100644
index 000000000..0a15daf33
--- /dev/null
+++ b/src/nebula_core/nebula_core_common/tests/test_ring_buffer.cpp
@@ -0,0 +1,148 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_common/util/ring_buffer.hpp"
+
+#include <gtest/gtest.h>
+
+#include <stdexcept>
+
+using nebula::util::RingBuffer;
+
+TEST(RingBufferTest, InitialState)
+{
+  RingBuffer<double> buffer(5);
+
+  EXPECT_EQ(buffer.size(), 0);
+  EXPECT_FALSE(buffer.is_full());
+}
+
+TEST(RingBufferTest, EmptyBufferThrowsOnGetAverage)
+{
+  RingBuffer<double> buffer(5);
+
+  EXPECT_THROW(static_cast<void>(buffer.get_average()), std::runtime_error);
+}
+
+TEST(RingBufferTest, PushIncrementsSize)
+{
+  RingBuffer<double> buffer(5);
+
+  buffer.push_back(1.0);
+  EXPECT_EQ(buffer.size(), 1);
+
+  buffer.push_back(2.0);
+  EXPECT_EQ(buffer.size(), 2);
+
+  buffer.push_back(3.0);
+  EXPECT_EQ(buffer.size(), 3);
+}
+
+TEST(RingBufferTest, IsFullWhenCapacityReached)
+{
+  RingBuffer<double> buffer(3);
+
+  buffer.push_back(1.0);
+  EXPECT_FALSE(buffer.is_full());
+
+  buffer.push_back(2.0);
+  EXPECT_FALSE(buffer.is_full());
+
+  buffer.push_back(3.0);
+  EXPECT_TRUE(buffer.is_full());
+  EXPECT_EQ(buffer.size(), 3);
+}
+
+TEST(RingBufferTest, AverageCalculation)
+{
+  RingBuffer<double> buffer(5);
+
+  buffer.push_back(1.0);
+  EXPECT_DOUBLE_EQ(buffer.get_average(), 1.0);
+
+  buffer.push_back(2.0);
+  EXPECT_DOUBLE_EQ(buffer.get_average(), 1.5);  // (1 + 2) / 2
+
+  buffer.push_back(3.0);
+  EXPECT_DOUBLE_EQ(buffer.get_average(), 2.0);  // (1 + 2 + 3) / 3
+}
+
+TEST(RingBufferTest, OverwritesOldValuesWhenFull)
+{
+  RingBuffer<double> buffer(3);
+
+  // Fill the buffer
+  buffer.push_back(1.0);
+  buffer.push_back(2.0);
+  buffer.push_back(3.0);
+  EXPECT_TRUE(buffer.is_full());
+  EXPECT_DOUBLE_EQ(buffer.get_average(), 2.0);  // (1 + 2 + 3) / 3
+
+  // Push one more - should overwrite the first value (1.0)
+  buffer.push_back(4.0);
+  EXPECT_TRUE(buffer.is_full());
+  EXPECT_EQ(buffer.size(), 3);
+  EXPECT_DOUBLE_EQ(buffer.get_average(), 3.0);  // (2 + 3 + 4) / 3
+
+  // Push another - should overwrite the second value (2.0)
+  buffer.push_back(5.0);
+  EXPECT_DOUBLE_EQ(buffer.get_average(), 4.0);  // (3 + 4 + 5) / 3
+}
+
+TEST(RingBufferTest, WorksWithIntegers)
+{
+  RingBuffer<int> buffer(4);
+
+  buffer.push_back(10);
+  buffer.push_back(20);
+  buffer.push_back(30);
+  buffer.push_back(40);
+
+  EXPECT_EQ(buffer.get_average(), 25);  // (10 + 20 + 30 + 40) / 4
+
+  buffer.push_back(50);
+  EXPECT_EQ(buffer.get_average(), 35);  // (20 + 30 + 40 + 50) / 4
+}
+
+TEST(RingBufferTest, SingleElementBuffer)
+{
+  RingBuffer<double> buffer(1);
+
+  buffer.push_back(5.0);
+  EXPECT_TRUE(buffer.is_full());
+  EXPECT_DOUBLE_EQ(buffer.get_average(), 5.0);
+
+  buffer.push_back(10.0);
+  EXPECT_DOUBLE_EQ(buffer.get_average(), 10.0);
+}
+
+TEST(RingBufferTest, LargeCapacity)
+{
+  RingBuffer<double> buffer(1000);
+
+  double sum = 0.0;
+  for (int i = 1; i <= 1000; ++i) {
+    buffer.push_back(static_cast<double>(i));
+    sum += i;
+  }
+
+  EXPECT_TRUE(buffer.is_full());
+  EXPECT_DOUBLE_EQ(buffer.get_average(), sum / 1000.0);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_common/tests/test_string_conversions.cpp b/src/nebula_core/nebula_core_common/tests/test_string_conversions.cpp
new file mode 100644
index 000000000..045a620fc
--- /dev/null
+++ b/src/nebula_core/nebula_core_common/tests/test_string_conversions.cpp
@@ -0,0 +1,140 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_common/util/string_conversions.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cstdint>
+#include <string>
+
+using nebula::util::format_timestamp;
+using nebula::util::to_string;
+
+TEST(StringConversionsTest, ToStringInteger)
+{
+  EXPECT_EQ(to_string(42), "42");
+  EXPECT_EQ(to_string(-123), "-123");
+  EXPECT_EQ(to_string(0), "0");
+}
+
+TEST(StringConversionsTest, ToStringDouble)
+{
+  // Note: floating point to_string may have precision differences
+  std::string result = to_string(3.14159);
+  EXPECT_TRUE(result.find("3.14") != std::string::npos);
+}
+
+TEST(StringConversionsTest, ToStringCharArray)
+{
+  char buffer[10] = "hello";
+  EXPECT_EQ(to_string<10>(buffer), "hello");
+
+  // Test with null terminator in middle
+  char buffer2[10] = {'a', 'b', 'c', '\0', 'd', 'e'};
+  EXPECT_EQ(to_string<10>(buffer2), "abc");
+}
+
+TEST(StringConversionsTest, ToStringCharArrayMaxLen)
+{
+  // Buffer completely filled - no null terminator
+  char buffer[5] = {'a', 'b', 'c', 'd', 'e'};
+  EXPECT_EQ(to_string<5>(buffer), "abcde");
+}
+
+TEST(StringConversionsTest, FormatTimestampBasic)
+{
+  // 1234567890.123456789
+  std::string result = format_timestamp(1234567890, 123456789);
+  EXPECT_EQ(result, "1234567890.123456789");
+}
+
+TEST(StringConversionsTest, FormatTimestampZeroNanoseconds)
+{
+  std::string result = format_timestamp(1000, 0);
+  EXPECT_EQ(result, "1000.000000000");
+}
+
+TEST(StringConversionsTest, FormatTimestampZeroSeconds)
+{
+  std::string result = format_timestamp(0, 500000000);
+  EXPECT_EQ(result, "0.500000000");
+}
+
+TEST(StringConversionsTest, FormatTimestampSmallNanoseconds)
+{
+  // Nanoseconds with leading zeros
+  std::string result = format_timestamp(100, 1);
+  EXPECT_EQ(result, "100.000000001");
+
+  result = format_timestamp(100, 100);
+  EXPECT_EQ(result, "100.000000100");
+}
+
+TEST(StringConversionsTest, FormatTimestampMaxNanoseconds)
+{
+  std::string result = format_timestamp(0, 999999999);
+  EXPECT_EQ(result, "0.999999999");
+}
+
+TEST(StringConversionsTest, ToStringJson)
+{
+  nlohmann::json j_string = "hello";
+  EXPECT_EQ(to_string(j_string), "hello");
+
+  nlohmann::json j_number = 42;
+  EXPECT_EQ(to_string(j_number), "42");
+
+  nlohmann::json j_object = {{"key", "value"}};
+  std::string result = to_string(j_object);
+  EXPECT_EQ(result, "{\"key\":\"value\"}");
+}
+
+TEST(StringConversionsTest, ToStringOrderedJson)
+{
+  nlohmann::ordered_json j_object = {{"key1", "value1"}, {"key2", "value2"}};
+  std::string result = to_string(j_object);
+  EXPECT_EQ(result, "{\"key1\":\"value1\",\"key2\":\"value2\"}");
+}
+
+// Custom type with ostream operator
+struct CustomType
+{
+  int value;
+  friend std::ostream & operator<<(std::ostream & os, const CustomType & ct)
+  {
+    os << "CustomFormat(" << ct.value << ")";
+    return os;
+  }
+};
+
+TEST(StringConversionsTest, ToStringCustomStreamableType)
+{
+  CustomType ct{42};
+  EXPECT_EQ(to_string(ct), "CustomFormat(42)");
+}
+
+TEST(StringConversionsTest, IsStreamableTraitWorks)
+{
+  EXPECT_TRUE(nebula::util::IsStreamable<int>::value);
+  EXPECT_TRUE(nebula::util::IsStreamable<double>::value);
+  EXPECT_TRUE(nebula::util::IsStreamable<std::string>::value);
+  EXPECT_TRUE(nebula::util::IsStreamable<CustomType>::value);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_decoders/CMakeLists.txt b/src/nebula_core/nebula_core_decoders/CMakeLists.txt
index 0c1ffcb77..f4e0344f5 100644
--- a/src/nebula_core/nebula_core_decoders/CMakeLists.txt
+++ b/src/nebula_core/nebula_core_decoders/CMakeLists.txt
@@ -37,16 +37,25 @@ if(BUILD_TESTING)
     ament_lint_auto_find_test_dependencies()
 
     find_package(ament_cmake_gtest REQUIRED)
+    find_package(rcpputils REQUIRED)
 
     add_definitions(-D_TEST_RESOURCES_PATH="${PROJECT_SOURCE_DIR}/test_resources/")
 
     ament_add_gtest(test_downsample_mask tests/point_filters/test_downsample_mask.cpp)
-    target_include_directories(test_downsample_mask PRIVATE
-        include
-    )
     target_link_libraries(test_downsample_mask
         nebula_core_decoders
     )
+
+    ament_add_gtest(test_angles tests/test_angles.cpp)
+    target_link_libraries(test_angles
+        nebula_core_decoders
+    )
+
+    ament_add_gtest(test_blockage_mask tests/point_filters/test_blockage_mask.cpp)
+    target_link_libraries(test_blockage_mask
+        rcpputils::rcpputils
+        nebula_core_decoders
+    )
 endif()
 
 ament_export_targets(export_nebula_core_decoders)
diff --git a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/blockage_mask.hpp b/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/blockage_mask.hpp
index ce0590716..08950bc7b 100644
--- a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/blockage_mask.hpp
+++ b/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/blockage_mask.hpp
@@ -62,7 +62,8 @@ class BlockageMask
     bin_size_rad_{deg2rad(bin_size_mdeg / 1000.)},
     n_channels_{n_channels}
   {
-    mask_.resize(n_channels_ * get_width());
+    size_t width = std::ceil(azimuth_range_mdeg.extent() / bin_size_mdeg);
+    mask_.resize(n_channels_ * width);
   }
 
   void update(double azimuth_rad, uint16_t channel, BlockageState blockage)
@@ -83,10 +84,7 @@ class BlockageMask
 
   [[nodiscard]] const std::vector<uint8_t> & get_mask() const { return mask_; }
 
-  [[nodiscard]] size_t get_width() const
-  {
-    return static_cast<size_t>(std::ceil(azimuth_range_rad_.extent() / bin_size_rad_));
-  }
+  [[nodiscard]] size_t get_width() const { return mask_.size() / n_channels_; }
 
   [[nodiscard]] size_t get_height() const { return n_channels_; }
 
diff --git a/src/nebula_core/nebula_core_decoders/package.xml b/src/nebula_core/nebula_core_decoders/package.xml
index 5823dbafa..c60f90cb9 100644
--- a/src/nebula_core/nebula_core_decoders/package.xml
+++ b/src/nebula_core/nebula_core_decoders/package.xml
@@ -17,6 +17,7 @@
   <build_export_depend>libpng-dev</build_export_depend>
 
   <depend>nebula_core_common</depend>
+  <depend>rcpputils</depend>
   <depend>sensor_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
diff --git a/src/nebula_core/nebula_core_decoders/tests/point_filters/test_blockage_mask.cpp b/src/nebula_core/nebula_core_decoders/tests/point_filters/test_blockage_mask.cpp
new file mode 100644
index 000000000..d3f891bcb
--- /dev/null
+++ b/src/nebula_core/nebula_core_decoders/tests/point_filters/test_blockage_mask.cpp
@@ -0,0 +1,277 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_decoders/point_filters/blockage_mask.hpp"
+
+#include <nebula_core_common/nebula_common.hpp>
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+using nebula::drivers::AngleRange;
+using nebula::drivers::deg2rad;
+using nebula::drivers::MilliDegrees;
+using nebula::drivers::point_filters::BlockageMask;
+using nebula::drivers::point_filters::BlockageMaskPlugin;
+using nebula::drivers::point_filters::BlockageState;
+
+class BlockageMaskTest : public ::testing::Test
+{
+protected:
+  // Create a mask covering -180 to 180 degrees with 1 degree bins and 16 channels
+  static constexpr int32_t azi_start_mdeg = -180000;
+  static constexpr int32_t azi_end_mdeg = 180000;
+  static constexpr uint32_t bin_size_mdeg = 1000;  // 1 degree
+  static constexpr uint16_t n_channels = 16;
+
+  AngleRange<int32_t, MilliDegrees> azi_range{azi_start_mdeg, azi_end_mdeg};
+};
+
+TEST_F(BlockageMaskTest, Construction)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  EXPECT_EQ(mask.get_width(), 360);
+  EXPECT_EQ(mask.get_height(), 16);
+  EXPECT_EQ(mask.get_mask().size(), mask.get_width() * mask.get_height());
+}
+
+TEST_F(BlockageMaskTest, InitialStateIsZero)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  const auto & data = mask.get_mask();
+  for (size_t i = 0; i < data.size(); ++i) {
+    EXPECT_EQ(data[i], 0) << "Non-zero at index " << i;
+  }
+}
+
+TEST_F(BlockageMaskTest, UpdateBlockageIncrements)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  // Update at azimuth 0 (which maps to bin 180), channel 5
+  double azi_rad = deg2rad(0.0);
+  mask.update(azi_rad, 5, BlockageState::BLOCKAGE);
+
+  // Should increment the corresponding bin
+  const auto & data = mask.get_mask();
+
+  size_t expected_index = 5 * mask.get_width() + 180;  // channel 5, bin 180
+  EXPECT_EQ(data[expected_index], 1);
+}
+
+TEST_F(BlockageMaskTest, UpdateNoBlockageDoesNotIncrement)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  double azi_rad = deg2rad(0.0);
+  mask.update(azi_rad, 0, BlockageState::NO_BLOCKAGE);
+  mask.update(azi_rad, 0, BlockageState::UNSURE);
+
+  // Should not increment
+  const auto & data = mask.get_mask();
+  size_t expected_index = 0 * mask.get_width() + 180;
+  EXPECT_EQ(data[expected_index], 0);
+}
+
+TEST_F(BlockageMaskTest, MultipleBlockagesAccumulate)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  double azi_rad = deg2rad(45.0);
+  for (int i = 0; i < 5; ++i) {
+    mask.update(azi_rad, 5, BlockageState::BLOCKAGE);
+  }
+
+  const auto & data = mask.get_mask();
+  // 45 degrees from -180 = bin 225
+  size_t expected_index = 5 * mask.get_width() + 225;
+  EXPECT_EQ(data[expected_index], 5);
+}
+
+TEST_F(BlockageMaskTest, SaturatesAtMaxUint8)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  double azi_rad = deg2rad(0.0);
+
+  // Try to increment more than 255 times
+  for (int i = 0; i < 300; ++i) {
+    mask.update(azi_rad, 0, BlockageState::BLOCKAGE);
+  }
+
+  const auto & data = mask.get_mask();
+  size_t expected_index = 0 * mask.get_width() + 180;
+  EXPECT_EQ(data[expected_index], 255);  // Should saturate at UINT8_MAX
+}
+
+TEST_F(BlockageMaskTest, DifferentChannelsAreIndependent)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  double azi_rad = deg2rad(0.0);
+
+  mask.update(azi_rad, 0, BlockageState::BLOCKAGE);
+  mask.update(azi_rad, 0, BlockageState::BLOCKAGE);
+  mask.update(azi_rad, 1, BlockageState::BLOCKAGE);
+
+  const auto & data = mask.get_mask();
+  size_t index_ch0 = 0 * mask.get_width() + 180;
+  size_t index_ch1 = 1 * mask.get_width() + 180;
+
+  EXPECT_EQ(data[index_ch0], 2);
+  EXPECT_EQ(data[index_ch1], 1);
+}
+
+TEST_F(BlockageMaskTest, OutOfBoundsChannelIgnored)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  double azi_rad = deg2rad(0.0);
+
+  // Channel 16 is out of bounds (0-15 valid)
+  mask.update(azi_rad, 16, BlockageState::BLOCKAGE);
+  mask.update(azi_rad, 100, BlockageState::BLOCKAGE);
+
+  // Mask should be unchanged
+  const auto & data = mask.get_mask();
+  for (size_t i = 0; i < data.size(); ++i) {
+    EXPECT_EQ(data[i], 0);
+  }
+}
+
+TEST_F(BlockageMaskTest, OutOfBoundsAzimuthIgnored)
+{
+  // Create a smaller range to test out-of-bounds
+  AngleRange<int32_t, MilliDegrees> small_range{-45000, 45000};  // -45 to 45 degrees
+  BlockageMask mask(small_range, bin_size_mdeg, n_channels);
+
+  // Azimuth outside the range
+  double azi_outside = deg2rad(90.0);
+  mask.update(azi_outside, 0, BlockageState::BLOCKAGE);
+
+  // Mask should be unchanged
+  const auto & data = mask.get_mask();
+  for (size_t i = 0; i < data.size(); ++i) {
+    EXPECT_EQ(data[i], 0);
+  }
+}
+
+TEST_F(BlockageMaskTest, NegativeAzimuthHandled)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  // -90 degrees should be valid
+  double azi_rad = deg2rad(-90.1);
+  mask.update(azi_rad, 0, BlockageState::BLOCKAGE);
+
+  const auto & data = mask.get_mask();
+  // -90.1 degrees from -180 = bin 89
+  size_t expected_index = 0 * mask.get_width() + 89;
+  EXPECT_EQ(data[expected_index], 1);
+}
+
+TEST_F(BlockageMaskTest, BoundaryAzimuths)
+{
+  BlockageMask mask(azi_range, bin_size_mdeg, n_channels);
+
+  // Test at the boundaries
+  double azi_start = deg2rad(-179.95);
+  double azi_end = deg2rad(179.95);  // Just before 180
+
+  mask.update(azi_start, 0, BlockageState::BLOCKAGE);
+  mask.update(azi_end, 1, BlockageState::BLOCKAGE);
+
+  const auto & data = mask.get_mask();
+
+  // First bin
+  size_t index_start = 0 * mask.get_width() + 0;
+  EXPECT_EQ(data[index_start], 1);
+
+  // Last bin (359)
+  size_t index_end = 1 * mask.get_width() + 359;
+  EXPECT_EQ(data[index_end], 1);
+}
+
+// ============================================================================
+// BlockageMaskPlugin tests
+// ============================================================================
+
+TEST(BlockageMaskPluginTest, Construction)
+{
+  BlockageMaskPlugin plugin(1000);  // 1 degree bin width
+  EXPECT_EQ(plugin.get_bin_width_mdeg(), 1000);
+}
+
+TEST(BlockageMaskPluginTest, CallbackAndReset)
+{
+  BlockageMaskPlugin plugin(1000);
+
+  AngleRange<int32_t, MilliDegrees> azi_range{-180000, 180000};
+  BlockageMask mask(azi_range, 1000, 16);
+
+  // Add some blockages
+  mask.update(deg2rad(0.0), 0, BlockageState::BLOCKAGE);
+  mask.update(deg2rad(0.0), 0, BlockageState::BLOCKAGE);
+
+  bool callback_called = false;
+  size_t received_sum = 0;
+  double received_timestamp = 0.0;
+
+  plugin.set_callback([&](const BlockageMask & m, double ts) {
+    callback_called = true;
+    received_timestamp = ts;
+    // Sum all blockage counts
+    for (auto val : m.get_mask()) {
+      received_sum += val;
+    }
+  });
+
+  plugin.callback_and_reset(mask, 123.456);
+
+  EXPECT_TRUE(callback_called);
+  EXPECT_EQ(received_sum, 2);
+  EXPECT_DOUBLE_EQ(received_timestamp, 123.456);
+
+  // Mask should be reset after callback
+  for (auto val : mask.get_mask()) {
+    EXPECT_EQ(val, 0);
+  }
+}
+
+TEST(BlockageMaskPluginTest, NoCallbackSet)
+{
+  BlockageMaskPlugin plugin(1000);
+
+  AngleRange<int32_t, MilliDegrees> azi_range{-180000, 180000};
+  BlockageMask mask(azi_range, 1000, 16);
+
+  mask.update(deg2rad(0.0), 0, BlockageState::BLOCKAGE);
+
+  // Should not crash even without callback
+  EXPECT_NO_THROW(plugin.callback_and_reset(mask, 0.0));
+
+  // Mask should still be reset
+  for (auto val : mask.get_mask()) {
+    EXPECT_EQ(val, 0);
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_decoders/tests/test_angles.cpp b/src/nebula_core/nebula_core_decoders/tests/test_angles.cpp
new file mode 100644
index 000000000..842b83b58
--- /dev/null
+++ b/src/nebula_core/nebula_core_decoders/tests/test_angles.cpp
@@ -0,0 +1,241 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_decoders/angles.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+using nebula::drivers::angle_is_between;
+using nebula::drivers::AnglePair;
+using nebula::drivers::AngleRange;
+using nebula::drivers::CentiDegrees;
+using nebula::drivers::DeciDegrees;
+using nebula::drivers::Degrees;
+using nebula::drivers::FieldOfView;
+using nebula::drivers::MilliDegrees;
+using nebula::drivers::normalize_angle;
+using nebula::drivers::Radians;
+
+// ============================================================================
+// AngleRange::extent tests
+// ============================================================================
+
+TEST(AngleRangeTest, ExtentSimpleRange)
+{
+  AngleRange<int, Degrees> range{0, 180};
+  EXPECT_EQ(range.extent(), 180);
+}
+
+TEST(AngleRangeTest, ExtentFullCircle)
+{
+  AngleRange<int, Degrees> range{0, 360};
+  EXPECT_EQ(range.extent(), 360);
+}
+
+TEST(AngleRangeTest, ExtentCrossingZero)
+{
+  // Range that crosses the 0/360 boundary
+  AngleRange<int, Degrees> range{350, 10};
+  EXPECT_EQ(range.extent(), 20);  // 360 - 350 + 10 = 20
+}
+
+TEST(AngleRangeTest, ExtentCrossingZeroLarger)
+{
+  AngleRange<int, Degrees> range{270, 90};
+  EXPECT_EQ(range.extent(), 180);  // 360 - 270 + 90 = 180
+}
+
+TEST(AngleRangeTest, ExtentMilliDegrees)
+{
+  AngleRange<int32_t, MilliDegrees> range{0, 180'000};
+  EXPECT_EQ(range.extent(), 180'000);
+
+  AngleRange<int32_t, MilliDegrees> range_wrap{350'000, 10'000};
+  EXPECT_EQ(range_wrap.extent(), 20'000);  // Crosses zero
+}
+
+TEST(AngleRangeTest, ExtentRadians)
+{
+  AngleRange<double, Radians> range{0.0, M_PI};
+  EXPECT_DOUBLE_EQ(range.extent(), M_PI);
+
+  AngleRange<double, Radians> range_wrap{1.5 * M_PI, 0.5 * M_PI};
+  EXPECT_DOUBLE_EQ(range_wrap.extent(), M_PI);  // Crosses zero
+}
+
+// ============================================================================
+// angle_is_between tests - basic cases
+// ============================================================================
+
+TEST(AngleIsBetweenTest, AngleInsideRange)
+{
+  EXPECT_TRUE(angle_is_between(0, 180, 90));
+  EXPECT_TRUE(angle_is_between(10, 350, 180));
+  EXPECT_TRUE(angle_is_between(0, 360, 180));
+}
+
+TEST(AngleIsBetweenTest, AngleOutsideRange)
+{
+  EXPECT_FALSE(angle_is_between(0, 90, 180));
+  EXPECT_FALSE(angle_is_between(100, 200, 50));
+  EXPECT_FALSE(angle_is_between(100, 200, 250));
+}
+
+TEST(AngleIsBetweenTest, AngleAtBoundaryInclusive)
+{
+  // Default: both boundaries inclusive
+  EXPECT_TRUE(angle_is_between(0, 180, 0));    // At start
+  EXPECT_TRUE(angle_is_between(0, 180, 180));  // At end
+}
+
+TEST(AngleIsBetweenTest, AngleAtBoundaryStartExclusive)
+{
+  EXPECT_FALSE(angle_is_between(0, 180, 0, false, true));   // Start exclusive
+  EXPECT_TRUE(angle_is_between(0, 180, 180, false, true));  // End inclusive
+  EXPECT_TRUE(angle_is_between(0, 180, 90, false, true));   // Inside
+}
+
+TEST(AngleIsBetweenTest, AngleAtBoundaryEndExclusive)
+{
+  EXPECT_TRUE(angle_is_between(0, 180, 0, true, false));     // Start inclusive
+  EXPECT_FALSE(angle_is_between(0, 180, 180, true, false));  // End exclusive
+  EXPECT_TRUE(angle_is_between(0, 180, 90, true, false));    // Inside
+}
+
+TEST(AngleIsBetweenTest, AngleAtBoundaryBothExclusive)
+{
+  EXPECT_FALSE(angle_is_between(0, 180, 0, false, false));    // Start
+  EXPECT_FALSE(angle_is_between(0, 180, 180, false, false));  // End
+  EXPECT_TRUE(angle_is_between(0, 180, 90, false, false));    // Inside
+}
+
+// ============================================================================
+// angle_is_between tests - wraparound cases
+// ============================================================================
+
+TEST(AngleIsBetweenTest, WrapAroundAngleInside)
+{
+  // Range wraps around: 350 -> 10 (through 0)
+  EXPECT_TRUE(angle_is_between(350, 10, 355));  // Between 350 and 360
+  EXPECT_TRUE(angle_is_between(350, 10, 0));    // At zero
+  EXPECT_TRUE(angle_is_between(350, 10, 5));    // Between 0 and 10
+}
+
+TEST(AngleIsBetweenTest, WrapAroundAngleOutside)
+{
+  // Range wraps around: 350 -> 10 (through 0)
+  EXPECT_FALSE(angle_is_between(350, 10, 180));  // Middle of the excluded region
+  EXPECT_FALSE(angle_is_between(350, 10, 20));   // Just outside end
+  EXPECT_FALSE(angle_is_between(350, 10, 340));  // Just outside start
+}
+
+TEST(AngleIsBetweenTest, WrapAroundAtBoundary)
+{
+  EXPECT_TRUE(angle_is_between(350, 10, 350));  // At start
+  EXPECT_TRUE(angle_is_between(350, 10, 10));   // At end
+}
+
+TEST(AngleIsBetweenTest, WrapAroundHalfCircle)
+{
+  EXPECT_TRUE(angle_is_between(270, 90, 0));
+  EXPECT_TRUE(angle_is_between(270, 90, 45));
+  EXPECT_TRUE(angle_is_between(270, 90, 315));
+  EXPECT_FALSE(angle_is_between(270, 90, 180));
+  EXPECT_FALSE(angle_is_between(270, 90, 135));
+}
+
+// ============================================================================
+// normalize_angle tests
+// ============================================================================
+
+TEST(NormalizeAngleTest, AlreadyNormalized)
+{
+  EXPECT_DOUBLE_EQ(normalize_angle(0.0, 360.0), 0.0);
+  EXPECT_DOUBLE_EQ(normalize_angle(180.0, 360.0), 180.0);
+  EXPECT_DOUBLE_EQ(normalize_angle(359.0, 360.0), 359.0);
+}
+
+TEST(NormalizeAngleTest, NegativeAngle)
+{
+  EXPECT_DOUBLE_EQ(normalize_angle(-90.0, 360.0), 270.0);
+  EXPECT_DOUBLE_EQ(normalize_angle(-180.0, 360.0), 180.0);
+  EXPECT_DOUBLE_EQ(normalize_angle(-360.0, 360.0), 0.0);
+  EXPECT_DOUBLE_EQ(normalize_angle(-450.0, 360.0), 270.0);  // -450 + 2*360 = 270
+}
+
+TEST(NormalizeAngleTest, LargePositiveAngle)
+{
+  EXPECT_DOUBLE_EQ(normalize_angle(360.0, 360.0), 0.0);
+  EXPECT_DOUBLE_EQ(normalize_angle(450.0, 360.0), 90.0);
+  EXPECT_DOUBLE_EQ(normalize_angle(720.0, 360.0), 0.0);
+  EXPECT_DOUBLE_EQ(normalize_angle(810.0, 360.0), 90.0);  // 810 - 2*360 = 90
+}
+
+TEST(NormalizeAngleTest, Radians)
+{
+  double two_pi = 2.0 * M_PI;
+  EXPECT_DOUBLE_EQ(normalize_angle(0.0, two_pi), 0.0);
+  EXPECT_NEAR(normalize_angle(-M_PI, two_pi), M_PI, 1e-10);
+  EXPECT_NEAR(normalize_angle(3.0 * M_PI, two_pi), M_PI, 1e-10);
+  EXPECT_NEAR(normalize_angle(4.0 * M_PI, two_pi), 0.0, 1e-10);
+}
+
+TEST(NormalizeAngleTest, ScaledDegrees)
+{
+  // CentiDegrees: max = 36000
+  EXPECT_EQ(normalize_angle(0, 36'000), 0);
+  EXPECT_EQ(normalize_angle(36'000, 36'000), 0);
+  EXPECT_EQ(normalize_angle(45'000, 36'000), 9'000);   // 45000 - 36000 = 9000
+  EXPECT_EQ(normalize_angle(-9'000, 36'000), 27'000);  // -9000 + 36000 = 27000
+}
+
+// ============================================================================
+// Type definitions tests
+// ============================================================================
+
+TEST(AngleTypesTest, CircleModulus)
+{
+  EXPECT_DOUBLE_EQ(Radians::circle_modulus, 2 * M_PI);
+  EXPECT_DOUBLE_EQ(Degrees::circle_modulus, 360.0);
+  EXPECT_DOUBLE_EQ(DeciDegrees::circle_modulus, 3600.0);
+  EXPECT_DOUBLE_EQ(CentiDegrees::circle_modulus, 36'000.0);
+  EXPECT_DOUBLE_EQ(MilliDegrees::circle_modulus, 360'000.0);
+}
+
+TEST(AngleTypesTest, AnglePairConstruction)
+{
+  AnglePair<double, Radians> pair{M_PI / 4, M_PI / 6};
+  EXPECT_DOUBLE_EQ(pair.azimuth, M_PI / 4);
+  EXPECT_DOUBLE_EQ(pair.elevation, M_PI / 6);
+}
+
+TEST(AngleTypesTest, FieldOfViewConstruction)
+{
+  FieldOfView<int, Degrees> fov;
+  fov.azimuth = {0, 180};
+  fov.elevation = {-15, 15};
+
+  EXPECT_EQ(fov.azimuth.start, 0);
+  EXPECT_EQ(fov.azimuth.end, 180);
+  EXPECT_EQ(fov.elevation.start, -15);
+  EXPECT_EQ(fov.elevation.end, 15);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_hw_interfaces/CMakeLists.txt b/src/nebula_core/nebula_core_hw_interfaces/CMakeLists.txt
index 7a680dc06..7f994bf30 100644
--- a/src/nebula_core/nebula_core_hw_interfaces/CMakeLists.txt
+++ b/src/nebula_core/nebula_core_hw_interfaces/CMakeLists.txt
@@ -35,12 +35,12 @@ if(BUILD_TESTING)
     find_package(ament_cmake_gtest REQUIRED)
 
     ament_add_gtest(test_udp
-        test/common/test_udp.cpp
+        tests/common/test_udp.cpp
     )
 
     target_include_directories(test_udp PRIVATE
         include
-        test
+        tests
     )
     target_link_libraries(test_udp
         nebula_core_common::nebula_core_common
diff --git a/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp.cpp b/src/nebula_core/nebula_core_hw_interfaces/tests/common/test_udp.cpp
similarity index 100%
rename from src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp.cpp
rename to src/nebula_core/nebula_core_hw_interfaces/tests/common/test_udp.cpp
diff --git a/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp/utils.hpp b/src/nebula_core/nebula_core_hw_interfaces/tests/common/test_udp/utils.hpp
similarity index 100%
rename from src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp/utils.hpp
rename to src/nebula_core/nebula_core_hw_interfaces/tests/common/test_udp/utils.hpp
diff --git a/src/nebula_core/nebula_core_ros/CMakeLists.txt b/src/nebula_core/nebula_core_ros/CMakeLists.txt
index 98533a4da..393820266 100644
--- a/src/nebula_core/nebula_core_ros/CMakeLists.txt
+++ b/src/nebula_core/nebula_core_ros/CMakeLists.txt
@@ -44,13 +44,21 @@ if(BUILD_TESTING)
 
     # Unit tests
     ament_add_gtest(test_single_consumer_processor
-        test/test_single_consumer_processor.cpp
-    )
-    target_include_directories(test_single_consumer_processor PRIVATE
-        include
+        tests/test_single_consumer_processor.cpp
     )
     target_link_libraries(test_single_consumer_processor
         rcpputils::rcpputils
+        nebula_core_ros
+    )
+
+    ament_add_gtest(test_hysteresis_state_machine
+        tests/test_hysteresis_state_machine.cpp
+    )
+    target_link_libraries(test_hysteresis_state_machine
+        nebula_core_ros
+    )
+    ament_target_dependencies(test_hysteresis_state_machine
+        diagnostic_msgs
     )
 endif()
 
diff --git a/src/nebula_core/nebula_core_ros/tests/test_hysteresis_state_machine.cpp b/src/nebula_core/nebula_core_ros/tests/test_hysteresis_state_machine.cpp
new file mode 100644
index 000000000..52159229b
--- /dev/null
+++ b/src/nebula_core/nebula_core_ros/tests/test_hysteresis_state_machine.cpp
@@ -0,0 +1,341 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_core_ros/diagnostics/hysteresis_state_machine.hpp"
+
+#include <gtest/gtest.h>
+
+using custom_diagnostic_tasks::Error;
+using custom_diagnostic_tasks::generate_state;
+using custom_diagnostic_tasks::get_level;
+using custom_diagnostic_tasks::get_level_string;
+using custom_diagnostic_tasks::get_num_observations;
+using custom_diagnostic_tasks::HysteresisStateMachine;
+using custom_diagnostic_tasks::Ok;
+using custom_diagnostic_tasks::Stale;
+using custom_diagnostic_tasks::Warn;
+using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus;
+
+// ============================================================================
+// Helper functions tests
+// ============================================================================
+
+TEST(HysteresisHelperTest, GenerateState)
+{
+  auto stale = generate_state(DiagStatus::STALE);
+  EXPECT_EQ(get_level(stale), DiagStatus::STALE);
+
+  auto ok = generate_state(DiagStatus::OK);
+  EXPECT_EQ(get_level(ok), DiagStatus::OK);
+
+  auto warn = generate_state(DiagStatus::WARN);
+  EXPECT_EQ(get_level(warn), DiagStatus::WARN);
+
+  auto error = generate_state(DiagStatus::ERROR);
+  EXPECT_EQ(get_level(error), DiagStatus::ERROR);
+}
+
+TEST(HysteresisHelperTest, GenerateStateThrowsOnInvalid)
+{
+  EXPECT_THROW(generate_state(99), std::runtime_error);
+}
+
+TEST(HysteresisHelperTest, GetLevelString)
+{
+  EXPECT_EQ(get_level_string(DiagStatus::OK), "OK");
+  EXPECT_EQ(get_level_string(DiagStatus::WARN), "WARN");
+  EXPECT_EQ(get_level_string(DiagStatus::ERROR), "ERROR");
+  EXPECT_EQ(get_level_string(DiagStatus::STALE), "STALE");
+  EXPECT_EQ(get_level_string(99), "UNDEFINED");
+}
+
+TEST(HysteresisHelperTest, GetNumObservations)
+{
+  auto state = generate_state(DiagStatus::OK);
+  EXPECT_EQ(get_num_observations(state), 1);
+}
+
+// ============================================================================
+// State structs tests
+// ============================================================================
+
+TEST(StateBaseTest, StaleInitialization)
+{
+  Stale stale;
+  EXPECT_EQ(stale.level, DiagStatus::STALE);
+  EXPECT_EQ(stale.num_observations, 1);
+}
+
+TEST(StateBaseTest, OkInitialization)
+{
+  Ok ok;
+  EXPECT_EQ(ok.level, DiagStatus::OK);
+  EXPECT_EQ(ok.num_observations, 1);
+}
+
+TEST(StateBaseTest, WarnInitialization)
+{
+  Warn warn;
+  EXPECT_EQ(warn.level, DiagStatus::WARN);
+  EXPECT_EQ(warn.num_observations, 1);
+}
+
+TEST(StateBaseTest, ErrorInitialization)
+{
+  Error error;
+  EXPECT_EQ(error.level, DiagStatus::ERROR);
+  EXPECT_EQ(error.num_observations, 1);
+}
+
+// ============================================================================
+// HysteresisStateMachine basic tests
+// ============================================================================
+
+TEST(HysteresisStateMachineTest, DefaultConstruction)
+{
+  HysteresisStateMachine hsm;
+
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::STALE);
+  EXPECT_EQ(hsm.get_num_frame_transition(), 1);
+  EXPECT_FALSE(hsm.get_immediate_error_report_param());
+  EXPECT_TRUE(hsm.get_immediate_relax_state_param());
+}
+
+TEST(HysteresisStateMachineTest, ParameterizedConstruction)
+{
+  HysteresisStateMachine hsm(5, true, false);
+
+  EXPECT_EQ(hsm.get_num_frame_transition(), 5);
+  EXPECT_TRUE(hsm.get_immediate_error_report_param());
+  EXPECT_FALSE(hsm.get_immediate_relax_state_param());
+}
+
+TEST(HysteresisStateMachineTest, ZeroFrameTransitionClamped)
+{
+  HysteresisStateMachine hsm(0, false, true);
+
+  // Should be clamped to 1
+  EXPECT_EQ(hsm.get_num_frame_transition(), 1);
+}
+
+// ============================================================================
+// State transition tests
+// ============================================================================
+
+TEST(HysteresisStateMachineTest, ImmediateTransitionWithThresholdOne)
+{
+  HysteresisStateMachine hsm(1);  // Threshold 1 means immediate transition
+
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+
+  hsm.update_state(DiagStatus::WARN);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::WARN);
+
+  hsm.update_state(DiagStatus::ERROR);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::ERROR);
+}
+
+TEST(HysteresisStateMachineTest, HysteresisDelaysTransition)
+{
+  HysteresisStateMachine hsm(3, false, false);  // Need 3 observations, no immediate relax
+
+  // Start at STALE
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::STALE);
+
+  // First OK observation - should not transition yet
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::STALE);
+  EXPECT_EQ(hsm.get_candidate_level(), DiagStatus::OK);
+  EXPECT_EQ(hsm.get_candidate_num_observation(), 1);
+
+  // Second OK observation
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::STALE);
+  EXPECT_EQ(hsm.get_candidate_num_observation(), 2);
+
+  // Third OK observation - should transition now
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+}
+
+TEST(HysteresisStateMachineTest, CandidateResetOnDifferentObservation)
+{
+  HysteresisStateMachine hsm(3, false, false);
+
+  hsm.update_state(DiagStatus::OK);
+  hsm.update_state(DiagStatus::OK);  // 2 OK observations
+
+  // Different observation resets candidate
+  hsm.update_state(DiagStatus::WARN);
+  EXPECT_EQ(hsm.get_candidate_level(), DiagStatus::WARN);
+  EXPECT_EQ(hsm.get_candidate_num_observation(), 1);
+}
+
+// ============================================================================
+// Immediate error report tests
+// ============================================================================
+
+TEST(HysteresisStateMachineTest, ImmediateErrorReportEnabled)
+{
+  HysteresisStateMachine hsm(5, true, false);  // Immediate error, threshold 5
+
+  // Start with some OK observations
+  hsm.update_state(DiagStatus::OK);
+  hsm.update_state(DiagStatus::OK);
+
+  // ERROR should be reported immediately
+  hsm.update_state(DiagStatus::ERROR);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::ERROR);
+}
+
+TEST(HysteresisStateMachineTest, ImmediateErrorReportDisabled)
+{
+  HysteresisStateMachine hsm(3, false, false);  // No immediate error
+
+  hsm.update_state(DiagStatus::OK);
+  hsm.update_state(DiagStatus::OK);
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+
+  // ERROR should not be immediate
+  hsm.update_state(DiagStatus::ERROR);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+
+  hsm.update_state(DiagStatus::ERROR);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+
+  // Third ERROR observation triggers transition
+  hsm.update_state(DiagStatus::ERROR);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::ERROR);
+}
+
+// ============================================================================
+// Immediate relax state tests
+// ============================================================================
+
+TEST(HysteresisStateMachineTest, ImmediateRelaxEnabled)
+{
+  HysteresisStateMachine hsm(5, false, true);  // Immediate relax
+
+  // Get to ERROR state
+  hsm.set_current_state_level(DiagStatus::ERROR);
+
+  // OK should immediately relax the state (OK < ERROR)
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+}
+
+TEST(HysteresisStateMachineTest, ImmediateRelaxDisabled)
+{
+  HysteresisStateMachine hsm(3, false, false);  // No immediate relax
+
+  // Get to ERROR state
+  hsm.set_current_state_level(DiagStatus::ERROR);
+
+  // OK should not immediately relax
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::ERROR);
+
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::ERROR);
+
+  // Third OK observation triggers transition
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+}
+
+TEST(HysteresisStateMachineTest, RelaxOnlyToLowerSeverity)
+{
+  HysteresisStateMachine hsm(5, false, true);  // Immediate relax
+
+  hsm.set_current_state_level(DiagStatus::WARN);
+
+  // OK should relax (OK < WARN)
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+}
+
+TEST(HysteresisStateMachineTest, NoRelaxToHigherSeverity)
+{
+  HysteresisStateMachine hsm(5, false, true);  // Immediate relax
+
+  hsm.set_current_state_level(DiagStatus::WARN);
+
+  // ERROR is higher severity, should not "relax"
+  hsm.update_state(DiagStatus::ERROR);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::WARN);  // Still WARN
+}
+
+// ============================================================================
+// Set current state level tests
+// ============================================================================
+
+TEST(HysteresisStateMachineTest, SetCurrentStateLevel)
+{
+  HysteresisStateMachine hsm;
+
+  hsm.set_current_state_level(DiagStatus::WARN);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::WARN);
+
+  hsm.set_current_state_level(DiagStatus::ERROR);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::ERROR);
+
+  hsm.set_current_state_level(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+}
+
+// ============================================================================
+// Complex scenario tests
+// ============================================================================
+
+TEST(HysteresisStateMachineTest, RealWorldScenario)
+{
+  // Simulate a sensor that occasionally has glitches
+  HysteresisStateMachine hsm(3, true, true);  // 3 frames, immediate error, immediate relax
+
+  // Start with OK data
+  hsm.update_state(DiagStatus::OK);
+  hsm.update_state(DiagStatus::OK);
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+
+  // Single warning glitch - should not change state
+  hsm.update_state(DiagStatus::WARN);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);  // Hysteresis not yet reached
+
+  // Back to OK
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+
+  // Sustained warning
+  hsm.update_state(DiagStatus::WARN);
+  hsm.update_state(DiagStatus::WARN);
+  hsm.update_state(DiagStatus::WARN);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::WARN);
+
+  // Error should be immediate
+  hsm.update_state(DiagStatus::ERROR);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::ERROR);
+
+  // Recovery to OK should be immediate (relaxation)
+  hsm.update_state(DiagStatus::OK);
+  EXPECT_EQ(hsm.get_current_state_level(), DiagStatus::OK);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_core/nebula_core_ros/test/test_single_consumer_processor.cpp b/src/nebula_core/nebula_core_ros/tests/test_single_consumer_processor.cpp
similarity index 100%
rename from src/nebula_core/nebula_core_ros/test/test_single_consumer_processor.cpp
rename to src/nebula_core/nebula_core_ros/tests/test_single_consumer_processor.cpp
diff --git a/src/nebula_hesai/nebula_hesai/CMakeLists.txt b/src/nebula_hesai/nebula_hesai/CMakeLists.txt
index 96cd84ab0..618bf43ef 100644
--- a/src/nebula_hesai/nebula_hesai/CMakeLists.txt
+++ b/src/nebula_hesai/nebula_hesai/CMakeLists.txt
@@ -100,7 +100,7 @@ if(BUILD_TESTING)
 
     # Functional safety test
     ament_add_gtest(test_hesai_functional_safety
-        test/test_hesai_functional_safety.cpp
+        tests/test_hesai_functional_safety.cpp
     )
     target_include_directories(test_hesai_functional_safety PRIVATE
         include
diff --git a/src/nebula_hesai/nebula_hesai/test/test_hesai_functional_safety.cpp b/src/nebula_hesai/nebula_hesai/tests/test_hesai_functional_safety.cpp
similarity index 100%
rename from src/nebula_hesai/nebula_hesai/test/test_hesai_functional_safety.cpp
rename to src/nebula_hesai/nebula_hesai/tests/test_hesai_functional_safety.cpp
diff --git a/src/nebula_hesai/nebula_hesai_decoders/CMakeLists.txt b/src/nebula_hesai/nebula_hesai_decoders/CMakeLists.txt
index e5b43ac12..d36f3b834 100644
--- a/src/nebula_hesai/nebula_hesai_decoders/CMakeLists.txt
+++ b/src/nebula_hesai/nebula_hesai_decoders/CMakeLists.txt
@@ -41,6 +41,26 @@ install(
   DESTINATION share/${PROJECT_NAME}
 )
 
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    find_package(ament_cmake_gtest REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+
+    ament_add_gtest(test_hesai_packet
+        tests/test_hesai_packet.cpp
+    )
+    target_link_libraries(test_hesai_packet
+        nebula_hesai_decoders
+    )
+
+    ament_add_gtest(test_packet_loss_detector
+        tests/test_packet_loss_detector.cpp
+    )
+    target_link_libraries(test_packet_loss_detector
+        nebula_hesai_decoders
+    )
+endif()
+
 ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_targets(export_nebula_hesai_decoders)
 
diff --git a/src/nebula_hesai/nebula_hesai_decoders/tests/test_hesai_packet.cpp b/src/nebula_hesai/nebula_hesai_decoders/tests/test_hesai_packet.cpp
new file mode 100644
index 000000000..9e71863d5
--- /dev/null
+++ b/src/nebula_hesai/nebula_hesai_decoders/tests/test_hesai_packet.cpp
@@ -0,0 +1,241 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_hesai_decoders/decoders/hesai_packet.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cstdint>
+
+using nebula::drivers::hesai_packet::Block;
+using nebula::drivers::hesai_packet::DateTime;
+using nebula::drivers::hesai_packet::FineAzimuthBlock;
+using nebula::drivers::hesai_packet::get_n_returns;
+using nebula::drivers::hesai_packet::PacketBase;
+namespace return_mode = nebula::drivers::hesai_packet::return_mode;
+using nebula::drivers::hesai_packet::SecondsSinceEpoch;
+using nebula::drivers::hesai_packet::SOBBlock;
+using nebula::drivers::hesai_packet::Unit3B;
+
+// ============================================================================
+// get_n_returns tests
+// ============================================================================
+
+TEST(HesaiPacketTest, GetNReturnsSingleModes)
+{
+  EXPECT_EQ(get_n_returns(return_mode::SINGLE_FIRST), 1);
+  EXPECT_EQ(get_n_returns(return_mode::SINGLE_SECOND), 1);
+  EXPECT_EQ(get_n_returns(return_mode::SINGLE_STRONGEST), 1);
+  EXPECT_EQ(get_n_returns(return_mode::SINGLE_LAST), 1);
+}
+
+TEST(HesaiPacketTest, GetNReturnsDualModes)
+{
+  EXPECT_EQ(get_n_returns(return_mode::DUAL_LAST_STRONGEST), 2);
+  EXPECT_EQ(get_n_returns(return_mode::DUAL_FIRST_SECOND), 2);
+  EXPECT_EQ(get_n_returns(return_mode::DUAL_FIRST_LAST), 2);
+  EXPECT_EQ(get_n_returns(return_mode::DUAL_FIRST_STRONGEST), 2);
+  EXPECT_EQ(get_n_returns(return_mode::DUAL_STRONGEST_SECONDSTRONGEST), 2);
+}
+
+TEST(HesaiPacketTest, GetNReturnsTripleMode)
+{
+  EXPECT_EQ(get_n_returns(return_mode::TRIPLE_FIRST_LAST_STRONGEST), 3);
+}
+
+TEST(HesaiPacketTest, GetNReturnsUnknownThrows)
+{
+  EXPECT_THROW(get_n_returns(0x00), std::runtime_error);
+  EXPECT_THROW(get_n_returns(0xFF), std::runtime_error);
+}
+
+// ============================================================================
+// DateTime tests (YearOffset = 1900)
+// ============================================================================
+
+TEST(DateTimeTest, GetSecondsBasic)
+{
+  DateTime<1900> dt;
+  dt.year = 124;  // 2024 (124 + 1900)
+  dt.month = 1;   // January
+  dt.day = 1;
+  dt.hour = 0;
+  dt.minute = 0;
+  dt.second = 0;
+
+  // 2024-01-01 00:00:00 UTC. Unix epoch starts at 1970
+  // 1704067200 = 2024-01-01 00:00:00 UTC
+  uint64_t expected = 1704067200;
+  EXPECT_EQ(dt.get_seconds(), expected);
+}
+
+TEST(DateTimeTest, GetSecondsWithTime)
+{
+  DateTime<1900> dt;
+  dt.year = 124;  // 2024
+  dt.month = 6;   // June
+  dt.day = 15;
+  dt.hour = 12;
+  dt.minute = 30;
+  dt.second = 45;
+
+  // Verify it's a reasonable timestamp (mid-2024)
+  uint64_t result = dt.get_seconds();
+  EXPECT_GT(result, (2024 - 1970) * 365 * 24 * 60 * 60);  // After 2024-01-01
+  EXPECT_LT(result, (2025 - 1970) * 365 * 24 * 60 * 60);  // Before 2025-01-01
+}
+
+TEST(DateTimeTest, GetSecondsEpoch)
+{
+  DateTime<1900> dt;
+  dt.year = 70;  // 1970 (70 + 1900)
+  dt.month = 1;
+  dt.day = 1;
+  dt.hour = 0;
+  dt.minute = 0;
+  dt.second = 0;
+
+  // Unix epoch
+  EXPECT_EQ(dt.get_seconds(), 0);
+}
+
+// ============================================================================
+// DateTime tests (YearOffset = 2000)
+// ============================================================================
+
+TEST(DateTimeTest, GetSecondsYear2000Offset)
+{
+  DateTime<2000> dt;
+  dt.year = 24;  // 2024 (24 + 2000)
+  dt.month = 1;
+  dt.day = 1;
+  dt.hour = 0;
+  dt.minute = 0;
+  dt.second = 0;
+
+  // 2024-01-01 00:00:00 UTC
+  uint64_t expected = 1704067200;
+  EXPECT_EQ(dt.get_seconds(), expected);
+}
+
+// ============================================================================
+// SecondsSinceEpoch tests
+// ============================================================================
+
+TEST(SecondsSinceEpochTest, GetSecondsZero)
+{
+  SecondsSinceEpoch sse;
+  sse.zero = 0;
+  sse.seconds[0] = 0;
+  sse.seconds[1] = 0;
+  sse.seconds[2] = 0;
+  sse.seconds[3] = 0;
+  sse.seconds[4] = 0;
+
+  EXPECT_EQ(sse.get_seconds(), 0);
+}
+
+TEST(SecondsSinceEpochTest, GetSecondsSmallValue)
+{
+  SecondsSinceEpoch sse;
+  sse.zero = 0;
+  // 256 (0x100) in big-endian 5-byte format
+  sse.seconds[0] = 0x00;  // MSB
+  sse.seconds[1] = 0x00;
+  sse.seconds[2] = 0x00;
+  sse.seconds[3] = 0x01;
+  sse.seconds[4] = 0x00;  // LSB
+
+  EXPECT_EQ(sse.get_seconds(), 256);
+}
+
+TEST(SecondsSinceEpochTest, GetSecondsTypicalValue)
+{
+  SecondsSinceEpoch sse;
+  sse.zero = 0;
+  // 1704067200 (2024-01-01 00:00:00) = 0x65920080
+  // In big-endian 5-byte: 00 65 92 00 80
+  sse.seconds[0] = 0x00;  // MSB
+  sse.seconds[1] = 0x65;
+  sse.seconds[2] = 0x92;
+  sse.seconds[3] = 0x00;
+  sse.seconds[4] = 0x80;  // LSB
+
+  EXPECT_EQ(sse.get_seconds(), 1704067200);
+}
+
+TEST(SecondsSinceEpochTest, GetSecondsMaxValue)
+{
+  SecondsSinceEpoch sse;
+  sse.zero = 0;
+  // Maximum 5-byte value: 0xFFFFFFFFFF = 1099511627775 in big-endian
+  sse.seconds[0] = 0xFF;
+  sse.seconds[1] = 0xFF;
+  sse.seconds[2] = 0xFF;
+  sse.seconds[3] = 0xFF;
+  sse.seconds[4] = 0xFF;
+
+  EXPECT_EQ(sse.get_seconds(), 1099511627775ULL);
+}
+
+// ============================================================================
+// Block tests
+// ============================================================================
+
+TEST(BlockTest, GetAzimuth)
+{
+  Block<Unit3B, 32> block;
+  block.azimuth = 18000;  // 180.00 degrees
+
+  EXPECT_EQ(block.get_azimuth(), 18000);
+}
+
+TEST(FineAzimuthBlockTest, GetAzimuth)
+{
+  FineAzimuthBlock<Unit3B, 32> block;
+  block.azimuth = 180;      // Coarse azimuth
+  block.fine_azimuth = 50;  // Fine azimuth
+
+  // Combined: (180 << 8) + 50 = 46130
+  EXPECT_EQ(block.get_azimuth(), 46130);
+}
+
+TEST(SOBBlockTest, GetAzimuth)
+{
+  SOBBlock<Unit3B, 32> block;
+  block.sob = 0xFFEE;
+  block.azimuth = 27000;  // 270.00 degrees
+
+  EXPECT_EQ(block.get_azimuth(), 27000);
+}
+
+// ============================================================================
+// PacketBase tests
+// ============================================================================
+
+TEST(PacketBaseTest, StaticConstants)
+{
+  using TestPacket = PacketBase<12, 32, 2, 100>;
+
+  EXPECT_EQ(TestPacket::n_blocks, 12);
+  EXPECT_EQ(TestPacket::n_channels, 32);
+  EXPECT_EQ(TestPacket::max_returns, 2);
+  EXPECT_EQ(TestPacket::degree_subdivisions, 100);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_hesai/nebula_hesai_decoders/tests/test_packet_loss_detector.cpp b/src/nebula_hesai/nebula_hesai_decoders/tests/test_packet_loss_detector.cpp
new file mode 100644
index 000000000..14e70fcad
--- /dev/null
+++ b/src/nebula_hesai/nebula_hesai_decoders/tests/test_packet_loss_detector.cpp
@@ -0,0 +1,238 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_hesai_decoders/decoders/packet_loss_detector.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cstdint>
+#include <vector>
+
+using nebula::drivers::PacketLossDetector;
+
+// Mock packet structure for testing
+struct MockPacket
+{
+  struct
+  {
+    uint32_t udp_sequence;
+  } tail;
+};
+
+class PacketLossDetectorTest : public ::testing::Test
+{
+protected:
+  PacketLossDetector<MockPacket> detector;
+
+  MockPacket make_packet(uint32_t seq)
+  {
+    MockPacket p;
+    p.tail.udp_sequence = seq;
+    return p;
+  }
+};
+
+TEST_F(PacketLossDetectorTest, FirstPacketNoCallback)
+{
+  uint64_t lost_count = 0;
+  detector.set_lost_callback([&](uint64_t n) { lost_count = n; });
+
+  auto p = make_packet(100);
+  detector.update(p);
+
+  // First packet should not trigger callback
+  EXPECT_EQ(lost_count, 0);
+}
+
+TEST_F(PacketLossDetectorTest, SequentialPacketsNoLoss)
+{
+  uint64_t callback_calls = 0;
+  detector.set_lost_callback([&](uint64_t /*n*/) { callback_calls++; });
+
+  auto p1 = make_packet(100);
+  auto p2 = make_packet(101);
+  auto p3 = make_packet(102);
+  auto p4 = make_packet(103);
+
+  detector.update(p1);
+  detector.update(p2);
+  detector.update(p3);
+  detector.update(p4);
+
+  // No loss - callback should not be called
+  EXPECT_EQ(callback_calls, 0);
+}
+
+TEST_F(PacketLossDetectorTest, SinglePacketLoss)
+{
+  uint64_t lost_count = 0;
+  detector.set_lost_callback([&](uint64_t n) { lost_count = n; });
+
+  auto p1 = make_packet(100);
+  auto p2 = make_packet(102);  // Skipped 101
+
+  detector.update(p1);
+  detector.update(p2);
+
+  EXPECT_EQ(lost_count, 1);
+}
+
+TEST_F(PacketLossDetectorTest, MultiplePacketLoss)
+{
+  uint64_t lost_count = 0;
+  detector.set_lost_callback([&](uint64_t n) { lost_count = n; });
+
+  auto p1 = make_packet(100);
+  auto p2 = make_packet(110);  // Skipped 101-109 (9 packets)
+
+  detector.update(p1);
+  detector.update(p2);
+
+  EXPECT_EQ(lost_count, 9);
+}
+
+TEST_F(PacketLossDetectorTest, AccumulatedLoss)
+{
+  std::vector<uint64_t> losses;
+  detector.set_lost_callback([&](uint64_t n) { losses.push_back(n); });
+
+  auto p1 = make_packet(100);
+  auto p2 = make_packet(102);  // Skip 1
+  auto p3 = make_packet(103);  // Sequential
+  auto p4 = make_packet(108);  // Skip 4
+
+  detector.update(p1);
+  detector.update(p2);
+  detector.update(p3);
+  detector.update(p4);
+
+  EXPECT_EQ(losses.size(), 2);
+  EXPECT_EQ(losses[0], 1);
+  EXPECT_EQ(losses[1], 4);
+}
+
+TEST_F(PacketLossDetectorTest, NoCallbackSet)
+{
+  // Should not crash when no callback is set
+  auto p1 = make_packet(100);
+  auto p2 = make_packet(200);  // Large gap
+
+  EXPECT_NO_THROW(detector.update(p1));
+  EXPECT_NO_THROW(detector.update(p2));
+}
+
+TEST_F(PacketLossDetectorTest, LargeGap)
+{
+  uint64_t lost_count = 0;
+  detector.set_lost_callback([&](uint64_t n) { lost_count = n; });
+
+  auto p1 = make_packet(0);
+  auto p2 = make_packet(10000);  // Large gap
+
+  detector.update(p1);
+  detector.update(p2);
+
+  EXPECT_EQ(lost_count, 9999);
+}
+
+TEST_F(PacketLossDetectorTest, StartingFromNonZero)
+{
+  uint64_t lost_count = 0;
+  detector.set_lost_callback([&](uint64_t n) { lost_count = n; });
+
+  // First packet can be any sequence number
+  auto p1 = make_packet(50000);
+  auto p2 = make_packet(50001);
+  auto p3 = make_packet(50003);  // Skip 1
+
+  detector.update(p1);
+  detector.update(p2);
+  detector.update(p3);
+
+  EXPECT_EQ(lost_count, 1);
+}
+
+TEST_F(PacketLossDetectorTest, SequenceNumberWraparoundNoLoss)
+{
+  // Assuming uint32_t wraparound at 0xFFFFFFFF -> 0
+  std::vector<uint64_t> losses;
+  detector.set_lost_callback([&](uint64_t n) { losses.push_back(n); });
+
+  auto p1 = make_packet(0xFFFFFFFEu);
+  auto p2 = make_packet(0xFFFFFFFFu);
+  auto p3 = make_packet(0u);
+  auto p4 = make_packet(1u);
+
+  detector.update(p1);
+  detector.update(p2);
+  detector.update(p3);
+  detector.update(p4);
+
+  // No losses expected
+  EXPECT_TRUE(losses.empty());
+}
+
+TEST_F(PacketLossDetectorTest, SequenceNumberWraparoundWithLoss)
+{
+  std::vector<uint64_t> losses;
+  detector.set_lost_callback([&](uint64_t n) { losses.push_back(n); });
+
+  auto p1 = make_packet(0xFFFFFFFEu);
+  auto p2 = make_packet(1u);  // skip 0xFFFFFFFF and 0
+
+  detector.update(p1);
+  detector.update(p2);
+
+  // One packet (0) is lost during the wrap
+  // The implementation reports total lost packets in a single callback
+  ASSERT_EQ(losses.size(), 1u);
+  EXPECT_EQ(losses[0], 2u);
+}
+
+// ============================================================================
+// Edge cases
+// ============================================================================
+
+TEST_F(PacketLossDetectorTest, CallbackCanBeChanged)
+{
+  uint64_t count1 = 0;
+  uint64_t count2 = 0;
+
+  detector.set_lost_callback([&](uint64_t n) { count1 = n; });
+
+  auto p1 = make_packet(0);
+  auto p2 = make_packet(5);  // Loss of 4
+
+  detector.update(p1);
+  detector.update(p2);
+
+  EXPECT_EQ(count1, 4);
+  EXPECT_EQ(count2, 0);
+
+  // Change callback
+  detector.set_lost_callback([&](uint64_t n) { count2 = n; });
+
+  auto p3 = make_packet(10);  // Loss of 4
+
+  detector.update(p3);
+
+  EXPECT_EQ(count1, 4);  // Unchanged
+  EXPECT_EQ(count2, 4);  // New callback used
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_robosense/nebula_robosense_common/tests/test_robosense_common.cpp b/src/nebula_robosense/nebula_robosense_common/tests/test_robosense_common.cpp
new file mode 100644
index 000000000..0dab3af8c
--- /dev/null
+++ b/src/nebula_robosense/nebula_robosense_common/tests/test_robosense_common.cpp
@@ -0,0 +1,198 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_robosense_common/robosense_common.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <sstream>
+#include <string>
+
+using nebula::drivers::ChannelCorrection;
+using nebula::drivers::return_mode_from_string_robosense;
+using nebula::drivers::ReturnMode;
+using nebula::drivers::RobosenseCalibrationConfiguration;
+
+// ============================================================================
+// return_mode_from_string_robosense tests
+// ============================================================================
+
+TEST(RobosenseCommonTest, ReturnModeFromStringDual)
+{
+  EXPECT_EQ(return_mode_from_string_robosense("Dual"), ReturnMode::DUAL);
+}
+
+TEST(RobosenseCommonTest, ReturnModeFromStringStrongest)
+{
+  EXPECT_EQ(return_mode_from_string_robosense("Strongest"), ReturnMode::SINGLE_STRONGEST);
+}
+
+TEST(RobosenseCommonTest, ReturnModeFromStringLast)
+{
+  EXPECT_EQ(return_mode_from_string_robosense("Last"), ReturnMode::SINGLE_LAST);
+}
+
+TEST(RobosenseCommonTest, ReturnModeFromStringFirst)
+{
+  EXPECT_EQ(return_mode_from_string_robosense("First"), ReturnMode::SINGLE_FIRST);
+}
+
+TEST(RobosenseCommonTest, ReturnModeFromStringUnknown)
+{
+  EXPECT_EQ(return_mode_from_string_robosense("Invalid"), ReturnMode::UNKNOWN);
+  EXPECT_EQ(return_mode_from_string_robosense(""), ReturnMode::UNKNOWN);
+  EXPECT_EQ(return_mode_from_string_robosense("dual"), ReturnMode::UNKNOWN);  // Case sensitive
+}
+
+// ============================================================================
+// ChannelCorrection tests
+// ============================================================================
+
+TEST(ChannelCorrectionTest, DefaultConstruction)
+{
+  ChannelCorrection correction{};
+
+  EXPECT_TRUE(std::isnan(correction.azimuth));
+  EXPECT_TRUE(std::isnan(correction.elevation));
+  EXPECT_EQ(correction.channel, 0);
+}
+
+TEST(ChannelCorrectionTest, HasValueWhenSet)
+{
+  ChannelCorrection correction{1.5f, 2.5f, 3};
+
+  EXPECT_TRUE(correction.has_value());
+  EXPECT_FLOAT_EQ(correction.azimuth, 1.5f);
+  EXPECT_FLOAT_EQ(correction.elevation, 2.5f);
+  EXPECT_EQ(correction.channel, 3);
+}
+
+TEST(ChannelCorrectionTest, HasValueFalseWhenNaN)
+{
+  ChannelCorrection correction{};
+  EXPECT_FALSE(correction.has_value());
+
+  ChannelCorrection partial_nan{1.5f, NAN, 0};
+  EXPECT_FALSE(partial_nan.has_value());
+
+  ChannelCorrection partial_nan2{NAN, 2.5f, 0};
+  EXPECT_FALSE(partial_nan2.has_value());
+}
+
+// ============================================================================
+// RobosenseCalibrationConfiguration tests
+// ============================================================================
+
+TEST(RobosenseCalibrationConfigTest, DefaultConstruction)
+{
+  RobosenseCalibrationConfiguration config;
+
+  EXPECT_TRUE(config.calibration.empty());
+}
+
+TEST(RobosenseCalibrationConfigTest, SetChannelSize)
+{
+  RobosenseCalibrationConfiguration config;
+
+  config.set_channel_size(32);
+  EXPECT_EQ(config.calibration.size(), 32);
+
+  config.set_channel_size(64);
+  EXPECT_EQ(config.calibration.size(), 64);
+}
+
+TEST(RobosenseCalibrationConfigTest, LoadFromValidStream)
+{
+  RobosenseCalibrationConfiguration config;
+  config.set_channel_size(3);
+
+  std::stringstream ss;
+  ss << "Laser id,Elevation,Azimuth\n";
+  ss << "1,10.5,0.5\n";
+  ss << "2,5.5,0.3\n";
+  ss << "3,-10.0,0.1\n";
+
+  auto status = config.load_from_stream(ss);
+
+  EXPECT_EQ(status, nebula::Status::OK);
+  EXPECT_TRUE(config.calibration[0].has_value());
+  EXPECT_FLOAT_EQ(config.calibration[0].elevation, 10.5f);
+  EXPECT_FLOAT_EQ(config.calibration[0].azimuth, 0.5f);
+}
+
+TEST(RobosenseCalibrationConfigTest, LoadFromString)
+{
+  RobosenseCalibrationConfiguration config;
+  config.set_channel_size(3);
+
+  std::string content =
+    "Laser id,Elevation,Azimuth\n"
+    "1,10.5,0.5\n"
+    "2,5.5,0.3\n"
+    "3,-10.0,0.1\n";
+
+  auto status = config.load_from_string(content);
+
+  EXPECT_EQ(status, nebula::Status::OK);
+  EXPECT_EQ(config.calibration.size(), 3);
+}
+
+TEST(RobosenseCalibrationConfigTest, GetCorrection)
+{
+  RobosenseCalibrationConfiguration config;
+  config.set_channel_size(3);
+
+  std::string content =
+    "Laser id,Elevation,Azimuth\n"
+    "1,10.5,0.5\n"
+    "2,5.5,0.3\n"
+    "3,-10.0,0.1\n";
+
+  config.load_from_string(content);
+
+  auto c0 = config.get_correction(0);
+  EXPECT_FLOAT_EQ(c0.elevation, 10.5f);
+  EXPECT_FLOAT_EQ(c0.azimuth, 0.5f);
+
+  auto c2 = config.get_correction(2);
+  EXPECT_FLOAT_EQ(c2.elevation, -10.0f);
+  EXPECT_FLOAT_EQ(c2.azimuth, 0.1f);
+}
+
+TEST(RobosenseCalibrationConfigTest, CreateCorrectedChannels)
+{
+  RobosenseCalibrationConfiguration config;
+  config.set_channel_size(3);
+
+  std::string content =
+    "Laser id,Elevation,Azimuth\n"
+    "1,10.5,0.5\n"    // Highest elevation -> channel 2
+    "2,5.5,0.3\n"     // Middle elevation -> channel 1
+    "3,-10.0,0.1\n";  // Lowest elevation -> channel 0
+
+  config.load_from_string(content);
+  config.create_corrected_channels();
+
+  // Channels should be ordered by elevation (lowest = 0)
+  EXPECT_EQ(config.calibration[0].channel, 2);  // 10.5 is highest
+  EXPECT_EQ(config.calibration[1].channel, 1);  // 5.5 is middle
+  EXPECT_EQ(config.calibration[2].channel, 0);  // -10.0 is lowest
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/nebula_velodyne/nebula_velodyne_common/CMakeLists.txt b/src/nebula_velodyne/nebula_velodyne_common/CMakeLists.txt
index d7fac43e6..f37e80e84 100644
--- a/src/nebula_velodyne/nebula_velodyne_common/CMakeLists.txt
+++ b/src/nebula_velodyne/nebula_velodyne_common/CMakeLists.txt
@@ -34,6 +34,21 @@ target_link_libraries(nebula_velodyne_common PUBLIC
 install(TARGETS nebula_velodyne_common EXPORT export_nebula_velodyne_common)
 install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME})
 
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    find_package(ament_cmake_gtest REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+
+    add_definitions(-D_CALIBRATION_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../nebula_velodyne_decoders/calibration/")
+
+    ament_add_gtest(test_velodyne_calibration
+        tests/test_velodyne_calibration.cpp
+    )
+    target_link_libraries(test_velodyne_calibration
+        nebula_velodyne_common
+    )
+endif()
+
 ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_targets(export_nebula_velodyne_common)
 ament_export_dependencies(nebula_core_common yaml-cpp)
diff --git a/src/nebula_velodyne/nebula_velodyne_common/tests/test_velodyne_calibration.cpp b/src/nebula_velodyne/nebula_velodyne_common/tests/test_velodyne_calibration.cpp
new file mode 100644
index 000000000..4232f69f3
--- /dev/null
+++ b/src/nebula_velodyne/nebula_velodyne_common/tests/test_velodyne_calibration.cpp
@@ -0,0 +1,211 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nebula_velodyne_common/velodyne_calibration_decoder.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <filesystem>
+#include <fstream>
+#include <string>
+#include <vector>
+
+using nebula::drivers::VelodyneCalibration;
+using nebula::drivers::VelodyneLaserCorrection;
+
+#ifndef _CALIBRATION_DIR_PATH
+#error "_CALIBRATION_DIR_PATH is not defined"
+#endif
+
+class VelodyneCalibrationTest : public ::testing::Test
+{
+protected:
+  std::filesystem::path calibration_dir{_CALIBRATION_DIR_PATH};
+};
+
+// ============================================================================
+// VelodyneLaserCorrection tests
+// ============================================================================
+
+TEST(VelodyneLaserCorrectionTest, DefaultValues)
+{
+  VelodyneLaserCorrection correction{};
+
+  // Struct should be zero-initialized by default
+  EXPECT_EQ(correction.laser_ring, 0);
+  EXPECT_FALSE(correction.two_pt_correction_available);
+}
+
+// ============================================================================
+// VelodyneCalibration basic tests
+// ============================================================================
+
+TEST(VelodyneCalibrationBasicTest, DefaultConstruction)
+{
+  VelodyneCalibration calib;
+
+  EXPECT_FALSE(calib.initialized);
+  EXPECT_FLOAT_EQ(calib.distance_resolution_m, 0.002f);
+  EXPECT_EQ(calib.num_lasers, 0);
+  EXPECT_TRUE(calib.laser_corrections.empty());
+  EXPECT_TRUE(calib.laser_corrections_map.empty());
+}
+
+TEST(VelodyneCalibrationBasicTest, NonExistentFile)
+{
+  VelodyneCalibration calib("/nonexistent/path/to/calibration.yaml");
+
+  EXPECT_FALSE(calib.initialized);
+}
+
+// ============================================================================
+// Read existing calibration files
+// ============================================================================
+
+TEST_F(VelodyneCalibrationTest, ReadVLP16Calibration)
+{
+  auto calib_file = calibration_dir / "VLP16.yaml";
+  ASSERT_TRUE(std::filesystem::exists(calib_file)) << "VLP16.yaml not found";
+
+  VelodyneCalibration calib(calib_file.string());
+
+  EXPECT_TRUE(calib.initialized);
+  EXPECT_EQ(calib.num_lasers, 16);
+  EXPECT_EQ(calib.laser_corrections.size(), 16);
+}
+
+TEST_F(VelodyneCalibrationTest, ReadVLP32Calibration)
+{
+  auto calib_file = calibration_dir / "VLP32.yaml";
+  ASSERT_TRUE(std::filesystem::exists(calib_file)) << "VLP32.yaml not found";
+
+  VelodyneCalibration calib(calib_file.string());
+
+  EXPECT_TRUE(calib.initialized);
+  EXPECT_EQ(calib.num_lasers, 32);
+  EXPECT_EQ(calib.laser_corrections.size(), 32);
+}
+
+TEST_F(VelodyneCalibrationTest, ReadVLS128Calibration)
+{
+  auto calib_file = calibration_dir / "VLS128.yaml";
+  ASSERT_TRUE(std::filesystem::exists(calib_file)) << "VLS128.yaml not found";
+
+  VelodyneCalibration calib(calib_file.string());
+
+  EXPECT_TRUE(calib.initialized);
+  EXPECT_EQ(calib.num_lasers, 128);
+  EXPECT_EQ(calib.laser_corrections.size(), 128);
+}
+
+// ============================================================================
+// Calibration data validation
+// ============================================================================
+
+TEST_F(VelodyneCalibrationTest, LaserCorrectionsHaveCachedValues)
+{
+  auto calib_file = calibration_dir / "VLP16.yaml";
+  ASSERT_TRUE(std::filesystem::exists(calib_file));
+
+  VelodyneCalibration calib(calib_file.string());
+  ASSERT_TRUE(calib.initialized);
+
+  for (const auto & correction : calib.laser_corrections) {
+    // Verify that cached sin/cos values match the angle values
+    EXPECT_NEAR(correction.sin_rot_correction, std::sin(correction.rot_correction), 1e-6);
+    EXPECT_NEAR(correction.cos_rot_correction, std::cos(correction.rot_correction), 1e-6);
+    EXPECT_NEAR(correction.sin_vert_correction, std::sin(correction.vert_correction), 1e-6);
+    EXPECT_NEAR(correction.cos_vert_correction, std::cos(correction.vert_correction), 1e-6);
+  }
+}
+
+TEST_F(VelodyneCalibrationTest, LaserRingsAreAssigned)
+{
+  auto calib_file = calibration_dir / "VLP16.yaml";
+  ASSERT_TRUE(std::filesystem::exists(calib_file));
+
+  VelodyneCalibration calib(calib_file.string());
+  ASSERT_TRUE(calib.initialized);
+
+  // Verify that laser rings are assigned (0 to num_lasers-1)
+  std::vector<bool> ring_used(calib.num_lasers, false);
+
+  for (const auto & correction : calib.laser_corrections) {
+    EXPECT_GE(correction.laser_ring, 0);
+    EXPECT_LT(correction.laser_ring, calib.num_lasers);
+    ring_used[correction.laser_ring] = true;
+  }
+
+  // All rings should be used exactly once
+  for (int i = 0; i < calib.num_lasers; ++i) {
+    EXPECT_TRUE(ring_used[i]) << "Ring " << i << " not used";
+  }
+}
+
+// ============================================================================
+// Write and read roundtrip
+// ============================================================================
+
+TEST_F(VelodyneCalibrationTest, WriteReadRoundtrip)
+{
+  auto calib_file = calibration_dir / "VLP16.yaml";
+  ASSERT_TRUE(std::filesystem::exists(calib_file));
+
+  // Read original
+  VelodyneCalibration original(calib_file.string());
+  ASSERT_TRUE(original.initialized);
+
+  // Write to temp file
+  auto temp_file = std::filesystem::temp_directory_path() / "test_velodyne_calib.yaml";
+  original.write(temp_file.string());
+
+  ASSERT_TRUE(std::filesystem::exists(temp_file));
+
+  // Read back
+  VelodyneCalibration loaded(temp_file.string());
+  EXPECT_TRUE(loaded.initialized);
+
+  // Compare
+  EXPECT_EQ(loaded.num_lasers, original.num_lasers);
+  EXPECT_FLOAT_EQ(loaded.distance_resolution_m, original.distance_resolution_m);
+  EXPECT_EQ(loaded.laser_corrections.size(), original.laser_corrections.size());
+
+  for (size_t i = 0; i < original.laser_corrections.size(); ++i) {
+    const auto & orig = original.laser_corrections[i];
+    const auto & load = loaded.laser_corrections[i];
+
+    EXPECT_FLOAT_EQ(load.rot_correction, orig.rot_correction);
+    EXPECT_FLOAT_EQ(load.vert_correction, orig.vert_correction);
+    EXPECT_FLOAT_EQ(load.dist_correction, orig.dist_correction);
+    EXPECT_EQ(load.two_pt_correction_available, orig.two_pt_correction_available);
+    EXPECT_FLOAT_EQ(load.dist_correction_x, orig.dist_correction_x);
+    EXPECT_FLOAT_EQ(load.dist_correction_y, orig.dist_correction_y);
+    EXPECT_FLOAT_EQ(load.vert_offset_correction, orig.vert_offset_correction);
+    EXPECT_FLOAT_EQ(load.horiz_offset_correction, orig.horiz_offset_correction);
+    EXPECT_EQ(load.max_intensity, orig.max_intensity);
+    EXPECT_EQ(load.min_intensity, orig.min_intensity);
+    EXPECT_FLOAT_EQ(load.focal_distance, orig.focal_distance);
+    EXPECT_FLOAT_EQ(load.focal_slope, orig.focal_slope);
+  }
+
+  // Cleanup
+  std::filesystem::remove(temp_file);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}