simple c++ dds publisher/subscriber added.

.gitignore

@@ -14,7 +14,7 @@ dist/
 downloads/
 eggs/
 .eggs/
-lib/
+# lib/
 lib64/
 parts/
 sdist/
@@ -158,3 +158,4 @@ cython_debug/
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
 #.idea/
+.vscode

Go2Py/simple_lowcmd_sender.py

@@ -0,0 +1,27 @@
+# from unitree_go.msg.dds_ import LowState_, LowCmd_, MotorCmd_, BmsCmd_
+from msgs import LowCmd
+import cyclonedds.idl.types as types
+from cyclonedds.domain import DomainParticipant
+from cyclonedds.pub import DataWriter
+from cyclonedds.topic import Topic
+from cyclonedds.util import duration
+import time
+
+
+# Create a DomainParticipant, your entrypoint to DDS
+# Created in the default domain
+dp = DomainParticipant(0)
+# Create a Topic with topic name "Hello" and as datatype "HelloWorld" structs.
+tp = Topic(dp, "go2py/lowcmd", LowCmd)
+dw = DataWriter(dp, tp)
+cmd = LowCmd(
+    q = 12*[0.],
+    dq = 12*[0.],
+    tau_ff = 12*[0.],
+    kp = 12*[0.],
+    kv = 12*[0.],
+    e_stop = 0
+)
+while True:
+    dw.write(cmd)
+    time.sleep(0.1)

cpp_bridge/.vscode/settings.json

@@ -1,9 +0,0 @@
-{
-    "cmake.configureOnOpen": true,
-    "files.associations": {
-        "thread": "cpp",
-        "chrono": "cpp",
-        "ostream": "cpp",
-        "list": "cpp"
-    }
-}

cpp_bridge/CMakeLists.txt

@@ -4,7 +4,8 @@ cmake_minimum_required(VERSION 3.5)
 SET(CMAKE_CXX_STANDARD 17)
 add_library(msgs STATIC include/go2py/LowCmd.cpp include/go2py/LowState.cpp)
 
-include_directories(/usr/local/include/ddscxx /usr/local/include/iceoryx/v2.0.2)
+include_directories(/usr/local/include/ddscxx /usr/local/include/iceoryx/v2.0.2 include)
 link_libraries(unitree_sdk2 ddsc ddscxx rt pthread msgs)
 
 add_executable(lowlevel_bridge src/lowlevel/bridge.cpp)
+add_executable(simple_test src/simple_test.cpp)

cpp_bridge/include/utils/dds_publisher.hpp

@@ -0,0 +1,34 @@
+#ifndef __DDS_PUBLISHER_HPP__
+#define __DDS_PUBLISHER_HPP__
+
+#include <iostream>
+#include <functional>
+#include <dds/dds.hpp>
+#include <thread>
+
+using namespace dds::sub;
+using namespace dds::domain;
+using namespace dds::topic;
+
+template<typename T>
+class DDSPublisher {
+    private:
+        dds::domain::DomainParticipant participant;
+        dds::topic::Topic<T> topic;
+        dds::pub::Publisher publisher;
+        dds::pub::DataWriter<T> writer;
+
+    public:
+        // Constructor
+        DDSPublisher(const std::string& topicName, const int id = 0)
+            : participant(dds::domain::DomainParticipant(id)),
+            topic(participant, topicName),
+            publisher(dds::pub::Publisher(participant)),
+            writer(dds::pub::DataWriter<T>(publisher, topic)) {}
+
+        // Method to publish a message
+        void publish(const T& message) {
+            writer.write(message);
+        }
+};
+#endif

cpp_bridge/include/utils/dds_subscriber.hpp

@@ -0,0 +1,66 @@
+#ifndef __DDS_SUBSCRIBER_HPP__
+#define __DDS_SUBSCRIBER_HPP__
+
+#include <iostream>
+#include <functional>
+#include <dds/dds.hpp>
+#include <thread>
+using namespace dds::sub;
+using namespace dds::domain;
+using namespace dds::topic;
+
+template<typename T>
+class DDSSubscriber {
+    public:
+        // Define a callback type for user-provided callbacks
+        using UserCallback = std::function<void(const T&)>;
+
+    private:
+        DomainParticipant participant;
+        Topic<T> topic;
+        Subscriber subscriber;
+        DataReader<T> reader;
+        T latestMessage;
+        UserCallback userCallback;
+
+        // Internal Listener class
+        class CustomListener : public NoOpDataReaderListener<msgs::LowCmd> {
+        private:
+            DDSSubscriber& parent;
+
+        public:
+            CustomListener(DDSSubscriber& parent) : parent(parent) {}
+
+            void on_data_available(DataReader<T>& reader) override {
+                // Take all available data
+                dds::sub::LoanedSamples<T> samples = reader.take();
+                if (samples.length() >0){
+                    for (auto sample_iter = samples.begin();
+                            sample_iter < samples.end();
+                            ++sample_iter){
+                                    parent.latestMessage = sample_iter->data(); // Update the latest message
+                                    // If a user callback is provided, execute it
+                                    if (parent.userCallback) {
+                                        parent.userCallback(parent.latestMessage);
+                                    }
+                                
+                    }
+                }
+            }
+        };
+
+    public:
+        // Constructor
+        DDSSubscriber(const std::string& topicName, UserCallback callback = nullptr, const int id = 0)
+            : participant(id),
+            topic(participant, topicName),
+            subscriber(participant),
+            userCallback(callback),
+            reader(subscriber, topic, dds::sub::qos::DataReaderQos(), new CustomListener(*this), dds::core::status::StatusMask::data_available()) {}
+
+        // Method to get the latest message
+        msgs::LowCmd getLatestMessage() const {
+            return latestMessage;
+        }
+};
+#endif

cpp_bridge/src/lowlevel/bridge.cpp

@@ -10,11 +10,8 @@
 #include <unitree/common/thread/thread.hpp>
 #include <go2py/LowCmd.hpp>
 #include <go2py/LowState.hpp>
-#include "dds/dds.hpp"
-#include "dds/dds.h"
 #include <thread>
 
-
 using namespace unitree::common;
 using namespace unitree::robot;
 using namespace org::eclipse::cyclonedds;
@@ -184,59 +181,8 @@ void Bridge::LowCmdWrite()
     lowcmd_publisher->Write(low_cmd);
 }
 
-class Go2PyListener: public dds::sub::NoOpDataReaderListener<msgs::LowCmd>
-{
-  public:
-    using callback_func = std::function<bool(dds::sub::DataReader<msgs::LowCmd>&,
-                                             dds::pub::DataWriter<msgs::LowState>&)>;
-    Go2PyListener() = delete;
-    Go2PyListener( const callback_func &f):
-        dds::sub::DataReaderListener<msgs::LowCmd>(), _f(f) { ; }
-
-    void on_data_available(dds::sub::DataReader<msgs::LowCmd>& rd,
-                           dds::pub::DataWriter<msgs::LowState>& rw) {
-        (void)_f(rd, rw);
-    }
-  private:
-    callback_func _f; // Private member variable to store the callback function
-};
-
-static bool data_available(dds::sub::DataReader<msgs::LowCmd>& rd,
-                           dds::pub::DataWriter<msgs::LowState>& rw)
-{
-    return true;
-}
 int main(int argc, const char** argv)
 {
-    dds::domain::DomainParticipant participant(domain::default_id());
-
-    dds::topic::qos::TopicQos tqos;
-    tqos << dds::core::policy::Reliability::Reliable(dds::core::Duration::from_secs(10));
-    dds::topic::Topic<msgs::LowCmd> cmd_topic(participant, "go2py/lowcmd", tqos);
-    dds::topic::Topic<msgs::LowState> state_topic(participant, "go2py/lowstate", tqos);
-
-    dds::pub::qos::PublisherQos pqos;
-    pqos << dds::core::policy::Partition("pong");
-    dds::pub::Publisher publisher(participant, pqos);
-
-    dds::sub::qos::SubscriberQos sqos;
-    sqos << dds::core::policy::Partition("ping");
-    dds::sub::Subscriber subscriber(participant, sqos);
-
-    dds::pub::qos::DataWriterQos wqos;
-    wqos << dds::core::policy::WriterDataLifecycle::ManuallyDisposeUnregisteredInstances();
-    dds::pub::DataWriter<msgs::LowState> writer(publisher, state_topic, wqos);
-
-    Go2PyListener listener(&data_available);
-
-    dds::sub::DataReader<msgs::LowCmd>
-      reader(
-        subscriber,
-        cmd_topic,
-        dds::sub::qos::DataReaderQos(),
-        &listener,
-        dds::core::status::StatusMask::data_available());
-
     if (argc < 2)
     {
         std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
@@ -244,7 +190,6 @@ int main(int argc, const char** argv)
     }
 
     ChannelFactory::Instance()->Init(0, argv[1]);
-
     Bridge bridge;
     bridge.Init();
     while (1)

cpp_bridge/src/simple_test.cpp

@@ -0,0 +1,40 @@
+#include <iostream>
+#include "go2py/LowCmd.hpp"
+#include <thread>
+#include "utils/dds_subscriber.hpp"
+#include "utils/dds_publisher.hpp"
+
+
+// User-defined callback function
+void userDefinedCallback(const msgs::LowCmd& msg) {
+    // Do something with the received message
+    std::cout << "User callback executed with message: " << std::endl;
+}
+
+int main() {
+    // Create a subscriber with a topic name and a user callback
+    DDSSubscriber<msgs::LowCmd> subscriber("go2py/lowcmd", userDefinedCallback);
+    
+    // Create a publisher for the LowCmd message type on the topic "LowCmdTopic"
+    DDSPublisher<msgs::LowCmd> publisher("LowCmdTopic");
+
+    // Create a LowCmd message
+    msgs::LowCmd message;
+
+    // Initialize the message data
+    for(int i = 0; i < 12; i++) {
+        message.q()[i] = 0.0f;
+        message.dq()[i] = 0.0f;
+        message.tau_ff()[i] = 0.0f;
+        message.kp()[i] = 1.0f;
+        message.kv()[i] = 0.5f;
+    }
+    message.e_stop() = 0;
+    
+    while(1)
+    {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+        publisher.publish(message);
+    }
+    return 0;
+}