Tcvetko support2

unitree_module/include/unitree_module/sensor_data_listener.h

@@ -0,0 +1,50 @@
+#include <unitree_module/common.h>
+
+/* Include the C++ DDS API. */
+#include "dds/dds.hpp"
+#include "dds/domain/qos/DomainParticipantQos.hpp"
+
+#include "CycloneData.hpp"
+
+BEGIN_NAMESPACE_UNITREE_MODULE
+
+namespace
+{
+    using MsgType_ = CycloneData::Msg;
+}
+
+class SensorDataListener : public virtual dds::sub::DataReaderListener<MsgType_>
+{
+public:
+    // using MsgType = unitree_go::msg::dds_::LowState_;
+    using MsgType = MsgType_;
+    using ArgType = std::vector<int16_t>&&;
+    inline static std::string GetTopicName()
+    {
+        return "CycloneData_Msg";
+    }
+
+    explicit SensorDataListener(const std::function<void(std::vector<int16_t>&&)>& func);
+
+    void on_data_available(dds::sub::DataReader<MsgType>& reader) override;
+
+    void on_subscription_matched(dds::sub::DataReader<MsgType>& reader,
+                                 const dds::core::status::SubscriptionMatchedStatus& status) override;
+
+    void on_sample_lost(dds::sub::DataReader<MsgType>& reader, const dds::core::status::SampleLostStatus& status) override;
+
+    void on_requested_deadline_missed(dds::sub::DataReader<MsgType>& reader,
+                                      const dds::core::status::RequestedDeadlineMissedStatus& status) override;
+
+    void on_requested_incompatible_qos(dds::sub::DataReader<MsgType>& reader,
+                                       const dds::core::status::RequestedIncompatibleQosStatus& status) override;
+
+    void on_sample_rejected(dds::sub::DataReader<MsgType>& reader, const dds::core::status::SampleRejectedStatus& status) override;
+
+    void on_liveliness_changed(dds::sub::DataReader<MsgType>& reader, const dds::core::status::LivelinessChangedStatus& status) override;
+
+private:
+    std::function<void(std::vector<int16_t>&&)> callback;
+};
+
+END_NAMESPACE_UNITREE_MODULE

unitree_module/include/unitree_module/unitree_channel.h

@@ -15,39 +15,40 @@
  */
 
 #pragma once
-#include <unitree_module/common.h>
 #include <opendaq/channel_impl.h>
 #include <opendaq/signal_config_ptr.h>
+#include <unitree_module/common.h>
 #include <optional>
 #include <random>
+#include <vector>
 
 BEGIN_NAMESPACE_UNITREE_MODULE
 
 enum class WaveformType { Sine, Rect, None, Counter, ConstantValue };
 
-DECLARE_OPENDAQ_INTERFACE(IRefChannel, IBaseObject)
+DECLARE_OPENDAQ_INTERFACE(IDogChannel, IBaseObject)
 {
-    virtual void collectSamples(std::chrono::microseconds curTime) = 0;
+    virtual void publishSamples(std::chrono::microseconds curTime, std::vector<int16_t> & data) = 0;
 };
 
-struct RefChannelInit
+struct UnitreeChannelInit
 {
     size_t index;
     double sampleRate;
     std::chrono::microseconds startTime;
     std::chrono::microseconds microSecondsFromEpochToStartTime;
 };
 
-class ExampleChannel final : public ChannelImpl<IRefChannel>
+class UnitreeChannel final : public ChannelImpl<IDogChannel>
 {
 public:
-    explicit ExampleChannel(const ContextPtr& context,
+    explicit UnitreeChannel(const ContextPtr& context,
                             const ComponentPtr& parent,
                             const StringPtr& localId,
-                            const RefChannelInit& init);
+                            const UnitreeChannelInit& init);
 
-    // IRefChannel
-    void collectSamples(std::chrono::microseconds curTime) override;
+    // IDogChannel
+    void publishSamples(std::chrono::microseconds curTime, std::vector<int16_t>& data) override;
 
     static std::string getEpoch();
     static RatioPtr getResolution();
@@ -58,7 +59,6 @@ class ExampleChannel final : public ChannelImpl<IRefChannel>
     void buildSignalDescriptors();
 
     uint64_t getSamplesSinceStart(std::chrono::microseconds time) const;
-    std::tuple<PacketPtr, PacketPtr> generateSamples(int64_t curTime, uint64_t newSamples);
     [[nodiscard]] Int getDeltaT(const double sr) const;
 
     uint64_t deltaT;
@@ -72,13 +72,12 @@ class ExampleChannel final : public ChannelImpl<IRefChannel>
     uint64_t counter;
     double sampleRate;
 
-    SignalConfigPtr valueSignal;
     SignalConfigPtr timeSignal;
 
-    // SignalConfigPtr forceFLsignal;
-    // SignalConfigPtr forceFRsignal;
-    // SignalConfigPtr forceRLsignal;
-    // SignalConfigPtr forceRRsignal;
+    SignalConfigPtr forceFLsignal;
+    SignalConfigPtr forceFRsignal;
+    SignalConfigPtr forceRLsignal;
+    SignalConfigPtr forceRRsignal;
 };
 
 END_NAMESPACE_UNITREE_MODULE

unitree_module/include/unitree_module/unitree_device.h

@@ -15,37 +15,22 @@
  */
 
 #pragma once
-#include <unitree_module/common.h>
-#include <opendaq/channel_ptr.h>
-#include <opendaq/device_impl.h>
-#include <thread>
 #include <condition_variable>
+#include <queue>
+#include <thread>
 
-BEGIN_NAMESPACE_UNITREE_MODULE
-
-class myDDSDevice
-{
-public:
-    explicit myDDSDevice(const std::function<void(int16_t)>& function);
-
-    void acqLoop();
+#include <opendaq/channel_ptr.h>
+#include <opendaq/device_impl.h>
 
-    std::thread acqThread;
-    std::function<void(int16_t)> function;
-    std::condition_variable cv;
-    std::mutex mutex;
+#include <unitree_module/common.h>
 
-    int16_t smpl;
-    int sign;
-};
+BEGIN_NAMESPACE_UNITREE_MODULE
 
 class UnitreeDevice final : public Device
 {
 public:
     explicit UnitreeDevice(const ContextPtr& ctx, const ComponentPtr& parent);
-    // ~UnitreeDevice() override;
-
-    // void UnitreeDevice_fun(const std::function<void(int16_t)>& function);
+    ~UnitreeDevice() override;
 
     static DeviceInfoPtr CreateDeviceInfo();
     static DeviceTypePtr CreateType();
@@ -54,41 +39,30 @@ class UnitreeDevice final : public Device
     DeviceInfoPtr onGetInfo() override;
     uint64_t onGetTicksSinceOrigin() override;
 
-    // std::condition_variable cv;
-    // std::mutex mutex;
-
-    // int16_t smpl;
-    // int sign;
-
-    std::shared_ptr<myDDSDevice> myDev;
-
 private:
-    // void initDomain();
-    // void initChannels();
-    void initSignals();
-    // void acqLoop();
-    void processData(int16_t data) const;
+    void initDomain();
+    void initChannels();
+
+    // void processData(int16_t data) const;
     // void processData(std::vector<int16_t> data) const;
 
+    void acqLoop();
+    void forwardDataCallback(std::vector<int16_t>&& data);
+    void forwardData(std::vector<int16_t>& data);
+
     std::chrono::microseconds getMicroSecondsSinceDeviceStart() const;
 
-    std::thread acqThread;
+    bool stopAcq;
+    std::queue<std::vector<int16_t>> queue;
+
+    std::mutex mutex;
     std::condition_variable cv;
+    std::thread acqThread;
 
     std::chrono::steady_clock::time_point startTime;
     std::chrono::microseconds microSecondsFromEpochToDeviceStart;
 
     ChannelPtr channel1;
-    ChannelPtr channel2;
-
-    SignalConfigPtr sig_F_fl;
-    SignalConfigPtr sig_F_fr;
-    SignalConfigPtr sig_F_rl;
-    SignalConfigPtr sig_F_rr;
-    SignalConfigPtr sigTime;
-
-    size_t acqLoopTime;
-    bool stopAcq;
 };
 
 END_NAMESPACE_UNITREE_MODULE

unitree_module/include/unitree_module/unitree_module.h

@@ -32,6 +32,7 @@ class UnitreeModule final : public Module
 private:
     std::mutex sync;
     bool deviceAdded;
+    size_t deviceIndex;
 };
 
 END_NAMESPACE_UNITREE_MODULE

unitree_module/src/CMakeLists.txt

@@ -1,17 +1,22 @@
 set(LIB_NAME unitree_module)
 set(MODULE_HEADERS_DIR ../include/${TARGET_FOLDER_NAME})
 
+# Use Cyclone IDL compiler to generate source code
+idlcxx_generate(TARGET cyclonedata FILES CycloneData.idl WARNINGS no-implicit-extensibility)
+
 set(SRC_Include common.h
                 module_dll.h
                 unitree_module.h
                 unitree_device.h
                 unitree_channel.h
+                sensor_data_listener.h
 )
 
 set(SRC_Srcs module_dll.cpp
              unitree_module.cpp
              unitree_device.cpp
              unitree_channel.cpp
+             sensor_data_listener.cpp
 )
 
 prepend_include(${TARGET_FOLDER_NAME} SRC_Include)
@@ -41,6 +46,7 @@ target_link_libraries(${LIB_NAME}
     PUBLIC
     daq::opendaq
     CycloneDDS-CXX::ddscxx
+    cyclonedata
 )
 
 target_include_directories(${LIB_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include>

unitree_module/src/CycloneData.idl

@@ -0,0 +1,21 @@
+/*
+ * Copyright(c) 2006 to 2020 ZettaScale Technology and others
+ *
+ * This program and the accompanying materials are made available under the
+ * terms of the Eclipse Public License v. 2.0 which is available at
+ * http://www.eclipse.org/legal/epl-2.0, or the Eclipse Distribution License
+ * v. 1.0 which is available at
+ * http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * SPDX-License-Identifier: EPL-2.0 OR BSD-3-Clause
+ */
+module CycloneData
+{
+  struct Msg
+  {
+    long userID;
+    string message;
+    short values[4];
+  };
+  #pragma keylist Msg userID
+};

unitree_module/src/sensor_data_listener.cpp

@@ -0,0 +1,80 @@
+#include <unitree_module/sensor_data_listener.h>
+
+BEGIN_NAMESPACE_UNITREE_MODULE
+
+SensorDataListener::SensorDataListener(const std::function<void(std::vector<int16_t>&&)>& func)
+    : callback(func)
+{
+}
+
+void SensorDataListener::on_data_available(dds::sub::DataReader<MsgType>& reader)
+{
+    dds::sub::LoanedSamples<MsgType> samples = reader.take();
+    size_t numberOfSamples = samples.length();
+
+    constexpr size_t numberOfComponents = 4;
+    std::vector<int16_t> buffer(numberOfComponents * numberOfSamples, 0);
+
+    size_t sampleIndex = 0;
+    for (const auto& sample : samples)
+    {
+        if (!sample.info().valid())
+        {
+            continue;
+        }
+
+        const MsgType& msg = sample.data();
+        const std::array<int16_t, 4>& values = msg.values();
+
+        // Pack into flat array as [x0, y0, z0, w0, x1, y1, z1, w1]
+        std::copy(values.data(), values.data() + 4, &buffer[4 * sampleIndex]);
+
+        // Equivalent but a bit easer to control
+        // for (size_t i = 0; i < 4; ++i)
+        // {
+        //     const size_t index = numberOfComponents * sampleIndex + i;
+        //     buffer[i] - values[i];
+        // }
+
+        // Question: Is there any information about the time these values were measured that we must propagate?
+
+        ++sampleIndex;
+    }
+    this->callback(std::move(buffer));
+}
+
+void SensorDataListener::on_subscription_matched(dds::sub::DataReader<MsgType>& reader,
+                                                 const dds::core::status::SubscriptionMatchedStatus& status)
+{
+    std::cout << "on_subscription_matched" << std::endl;
+}
+
+void SensorDataListener::on_sample_lost(dds::sub::DataReader<MsgType>& reader, const dds::core::status::SampleLostStatus& status)
+{
+    std::cout << "on_sample_lost" << std::endl;
+}
+
+void SensorDataListener::on_requested_deadline_missed(dds::sub::DataReader<MsgType>& reader,
+                                                      const dds::core::status::RequestedDeadlineMissedStatus& status)
+{
+    std::cout << "on_requested_deadline_missed" << std::endl;
+}
+
+void SensorDataListener::on_requested_incompatible_qos(dds::sub::DataReader<MsgType>& reader,
+                                                       const dds::core::status::RequestedIncompatibleQosStatus& status)
+{
+    std::cout << "on_requested_incompatible_qos" << std::endl;
+}
+
+void SensorDataListener::on_sample_rejected(dds::sub::DataReader<MsgType>& reader, const dds::core::status::SampleRejectedStatus& status)
+{
+    std::cout << "on_sample_rejected" << std::endl;
+}
+
+void SensorDataListener::on_liveliness_changed(dds::sub::DataReader<MsgType>& reader,
+                                               const dds::core::status::LivelinessChangedStatus& status)
+{
+    std::cout << "on_liveliness_changed" << std::endl;
+}
+
+END_NAMESPACE_UNITREE_MODULE