perseus_sensors(m2m2_lidar): Added within epsilon check

Added the member isWithinEpsilon and defined the constants used
ROAR-QUTRC · Dec 22, 2024 · 529b9d4 · 529b9d4
1 parent 606940f
commit 529b9d4
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 5 deletions.
diff --git a/software/ros_ws/src/perseus_sensors/include/m2m2_lidar/m2m2_lidar.hpp b/software/ros_ws/src/perseus_sensors/include/m2m2_lidar/m2m2_lidar.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <chrono>
+#include <cmath>
 #include <iomanip>
 #include <nlohmann/json.hpp>  // JSON parsing
 #include <optional>
@@ -48,16 +49,18 @@ class M2M2Lidar : public rclcpp::Node
     ~M2M2Lidar();
 
 private:
-    std::vector<uint8_t> _decodeBase64(const std::string& encoded);
+    static constexpr float INVALID_DISTANCE = 100000.0f;
+    static constexpr float EPSILON = 0.0001f;
+    static bool isWithinEpsilon(float a, float b, float epsilon = EPSILON);
 
+    // Data
+    std::vector<uint8_t> _decodeBase64(const std::string& encoded);
     std::vector<std::tuple<float, float, bool>> _decodeLaserPoints(const std::string& base64_encoded);
 
+    // Network
     bool connectToSensor(const std::string& host, int port);
-
     bool _sendJsonRequest(const std::string& command, const nlohmann::json& args = nullptr);
     nlohmann::json _receiveJsonResponse();
-
-    // Network communication
     void _sendCommand(const std::vector<uint8_t>& command);
     std::optional<std::vector<uint8_t>> _receiveData();
 

diff --git a/software/ros_ws/src/perseus_sensors/src/m2m2_lidar/m2m2_lidar.cpp b/software/ros_ws/src/perseus_sensors/src/m2m2_lidar/m2m2_lidar.cpp
@@ -119,6 +119,11 @@ void M2M2Lidar::_initializePublishers()
         this->get_parameter("imu_topic").as_string(), qos);
 }
 
+bool M2M2Lidar::isWithinEpsilon(float a, float b, float epsilon)
+{
+    return std::abs(a - b) <= epsilon;
+}
+
 /**
  * @brief Decodes a base64-encoded string into a byte sequence.
  *
@@ -205,7 +210,8 @@ std::vector<std::tuple<float, float, bool>> M2M2Lidar::_decodeLaserPoints(const
         std::copy_n(decompressed.data() + i, sizeof(float), reinterpret_cast<uint8_t*>(&distance));
         std::copy_n(decompressed.data() + i + 4, sizeof(float), reinterpret_cast<uint8_t*>(&angle));
 
-        bool valid = distance != 100000.0;
+        bool valid = !isWithinEpsilon(distance, INVALID_DISTANCE);
+        // bool valid = distance != 100000.0;
 
         if (valid)
         {