test

Author: Svvyppy

src/stingray_core_communication/stingray_core_communication/src/link_node_base.hpp

@@ -1,7 +1,6 @@
 #pragma once
 
 #include <array>
-#include <cstddef>
 #include <cstdint>
 #include <functional>
 #include <memory>
@@ -116,11 +115,6 @@ class LinkNodeBase : public rclcpp::Node
     }
 
 protected:
-    virtual void onSerialBytes([[maybe_unused]] const uint8_t *data,
-                               [[maybe_unused]] size_t length)
-    {
-    }
-
     void processIncoming()
     {
         stream_manager_.Process();
@@ -192,9 +186,8 @@ class LinkNodeBase : public rclcpp::Node
             return;
         }
 
-        const auto *data = msg->data.data();
+        const void *data = msg->data.data();
         const auto length = static_cast<unsigned>(msg->data.size());
-        onSerialBytes(data, static_cast<size_t>(length));
         hydrolib_RingQueue_Push(&tx_queue_, data, static_cast<uint16_t>(length));
         processIncoming();
     }

src/stingray_core_communication/stingray_core_communication/src/pressure_link_node.cpp

@@ -1,4 +1,6 @@
 #include <array>
+#include <bit>
+#include <cstdint>
 #include <cstring>
 #include <memory>
 #include <string>
@@ -25,36 +27,20 @@ class PressureLinkNode : public baseLink::LinkNodeBase
     }
 
 private:
-    void onSerialBytes(const uint8_t *data, size_t length) override
+    void publishRaw(const float value)
     {
-        serial_line_buffer_.append(reinterpret_cast<const char *>(data), length);
-
-        size_t delimiter_pos = 0;
-        while ((delimiter_pos = serial_line_buffer_.find_first_of("\r\n")) !=
-               std::string::npos)
-        {
-            std::string line = serial_line_buffer_.substr(0, delimiter_pos);
-            serial_line_buffer_.erase(0, delimiter_pos + 1);
-
-            while (!serial_line_buffer_.empty() &&
-                   (serial_line_buffer_.front() == '\r' ||
-                    serial_line_buffer_.front() == '\n'))
-            {
-                serial_line_buffer_.erase(0, 1);
-            }
-
-            if (!line.empty())
-            {
-                publishRaw(line);
-            }
-        }
+        auto msg = std_msgs::msg::String();
+        msg.data = std::to_string(value);
+        data_raw_pub_->publish(std::move(msg));
     }
 
-    void publishRaw(const std::string &raw_data)
+    static float decodeFloatLE(const uint8_t *data)
     {
-        auto msg = std_msgs::msg::String();
-        msg.data = raw_data;
-        data_raw_pub_->publish(std::move(msg));
+        const uint32_t raw = static_cast<uint32_t>(data[0]) |
+                             (static_cast<uint32_t>(data[1]) << 8U) |
+                             (static_cast<uint32_t>(data[2]) << 16U) |
+                             (static_cast<uint32_t>(data[3]) << 24U);
+        return std::bit_cast<float>(raw);
     }
 
     hydrolib::ReturnCode memoryRead(void *buffer, unsigned address,
@@ -72,32 +58,33 @@ class PressureLinkNode : public baseLink::LinkNodeBase
     hydrolib::ReturnCode memoryWrite(const void *buffer, unsigned address,
                                      unsigned length)
     {
+        RCLCPP_DEBUG(this->get_logger(),
+                     "Pressure slave write: addr=%u len=%u",
+                     address, length);
+
         if (address + length > memory_.size())
         {
             return hydrolib::ReturnCode::FAIL;
         }
 
         std::memcpy(memory_.data() + address, buffer, length);
 
-        if (address == 0U && length > 0U)
-        {
-            const auto *bytes = static_cast<const char *>(buffer);
-            std::string raw_data(bytes, bytes + length);
-
-            const auto null_pos = raw_data.find('\0');
-            if (null_pos != std::string::npos)
-            {
-                raw_data.resize(null_pos);
-            }
+        constexpr unsigned kValueAddress = 0U;
+        constexpr unsigned kValueLength = sizeof(float);
+        const bool value_is_written =
+            (address <= kValueAddress) &&
+            ((address + length) >= (kValueAddress + kValueLength));
 
-            publishRaw(raw_data);
+        if (value_is_written)
+        {
+            const float value = decodeFloatLE(memory_.data() + kValueAddress);
+            publishRaw(value);
         }
 
         return hydrolib::ReturnCode::OK;
     }
 
     std::array<uint8_t, 256> memory_{};
-    std::string serial_line_buffer_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr data_raw_pub_;
 };
 

src/stingray_core_sensors/pressure_sensor/config/pressure_sensor.param.yaml

@@ -1,4 +1,4 @@
 pressure_sensor:
   ros__parameters:
     dump_param: 1.0 # FIXME remove in the future
-    data_topic: /pressure/data_raw
+    data_topic: /data_raw

src/stingray_core_sensors/pressure_sensor/include/pressure_sensor/pressure_sensor.hpp

@@ -12,7 +12,7 @@ namespace stingray_core::pressure_sensor {
 struct PressureSensorConfig{
     PressureSensorConfig(const rclcpp::Node::SharedPtr& node)
     : dump_param(node->declare_parameter<double>("dump_param", DEFAULT_DUMP_PARAM)),
-      data_topic(node->declare_parameter<std::string>("data_topic", "/pressure/data_raw"))
+      data_topic(node->declare_parameter<std::string>("data_topic", "/data_raw"))
     {}
     const double  dump_param;
     const std::string data_topic;