diff --git a/.gitignore b/.gitignore index 68bc17f..262db13 100644 --- a/.gitignore +++ b/.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 diff --git a/Go2Py/simple_lowcmd_sender.py b/Go2Py/simple_lowcmd_sender.py new file mode 100644 index 0000000..fb67d0c --- /dev/null +++ b/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) \ No newline at end of file diff --git a/cpp_bridge/.vscode/settings.json b/cpp_bridge/.vscode/settings.json deleted file mode 100644 index 39d2543..0000000 --- a/cpp_bridge/.vscode/settings.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "cmake.configureOnOpen": true, - "files.associations": { - "thread": "cpp", - "chrono": "cpp", - "ostream": "cpp", - "list": "cpp" - } -} \ No newline at end of file diff --git a/cpp_bridge/CMakeLists.txt b/cpp_bridge/CMakeLists.txt index 77471ae..ea70dfd 100644 --- a/cpp_bridge/CMakeLists.txt +++ b/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) diff --git a/cpp_bridge/include/utils/dds_publisher.hpp b/cpp_bridge/include/utils/dds_publisher.hpp new file mode 100644 index 0000000..b2a776d --- /dev/null +++ b/cpp_bridge/include/utils/dds_publisher.hpp @@ -0,0 +1,34 @@ +#ifndef __DDS_PUBLISHER_HPP__ +#define __DDS_PUBLISHER_HPP__ + +#include +#include +#include +#include + +using namespace dds::sub; +using namespace dds::domain; +using namespace dds::topic; + +template +class DDSPublisher { + private: + dds::domain::DomainParticipant participant; + dds::topic::Topic topic; + dds::pub::Publisher publisher; + dds::pub::DataWriter 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(publisher, topic)) {} + + // Method to publish a message + void publish(const T& message) { + writer.write(message); + } +}; +#endif \ No newline at end of file diff --git a/cpp_bridge/include/utils/dds_subscriber.hpp b/cpp_bridge/include/utils/dds_subscriber.hpp new file mode 100644 index 0000000..b2ae021 --- /dev/null +++ b/cpp_bridge/include/utils/dds_subscriber.hpp @@ -0,0 +1,66 @@ +#ifndef __DDS_SUBSCRIBER_HPP__ +#define __DDS_SUBSCRIBER_HPP__ + +#include +#include +#include +#include +using namespace dds::sub; +using namespace dds::domain; +using namespace dds::topic; + +template +class DDSSubscriber { + public: + // Define a callback type for user-provided callbacks + using UserCallback = std::function; + + private: + DomainParticipant participant; + Topic topic; + Subscriber subscriber; + DataReader reader; + T latestMessage; + UserCallback userCallback; + + // Internal Listener class + class CustomListener : public NoOpDataReaderListener { + private: + DDSSubscriber& parent; + + public: + CustomListener(DDSSubscriber& parent) : parent(parent) {} + + void on_data_available(DataReader& reader) override { + // Take all available data + dds::sub::LoanedSamples 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 \ No newline at end of file diff --git a/cpp_bridge/lib/aarch64/libunitree_sdk2.a b/cpp_bridge/lib/aarch64/libunitree_sdk2.a new file mode 100644 index 0000000..baae04d Binary files /dev/null and b/cpp_bridge/lib/aarch64/libunitree_sdk2.a differ diff --git a/cpp_bridge/lib/x86_64/libunitree_sdk2.a b/cpp_bridge/lib/x86_64/libunitree_sdk2.a new file mode 100644 index 0000000..323c9a5 Binary files /dev/null and b/cpp_bridge/lib/x86_64/libunitree_sdk2.a differ diff --git a/cpp_bridge/src/lowlevel/bridge.cpp b/cpp_bridge/src/lowlevel/bridge.cpp index f2e3423..b627ae5 100644 --- a/cpp_bridge/src/lowlevel/bridge.cpp +++ b/cpp_bridge/src/lowlevel/bridge.cpp @@ -10,11 +10,8 @@ #include #include #include -#include "dds/dds.hpp" -#include "dds/dds.h" #include - 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 -{ - public: - using callback_func = std::function&, - dds::pub::DataWriter&)>; - Go2PyListener() = delete; - Go2PyListener( const callback_func &f): - dds::sub::DataReaderListener(), _f(f) { ; } - - void on_data_available(dds::sub::DataReader& rd, - dds::pub::DataWriter& rw) { - (void)_f(rd, rw); - } - private: - callback_func _f; // Private member variable to store the callback function -}; - -static bool data_available(dds::sub::DataReader& rd, - dds::pub::DataWriter& 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 cmd_topic(participant, "go2py/lowcmd", tqos); - dds::topic::Topic 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 writer(publisher, state_topic, wqos); - - Go2PyListener listener(&data_available); - - dds::sub::DataReader - 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) diff --git a/cpp_bridge/src/simple_test.cpp b/cpp_bridge/src/simple_test.cpp new file mode 100644 index 0000000..c102dde --- /dev/null +++ b/cpp_bridge/src/simple_test.cpp @@ -0,0 +1,40 @@ +#include +#include "go2py/LowCmd.hpp" +#include +#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 subscriber("go2py/lowcmd", userDefinedCallback); + + // Create a publisher for the LowCmd message type on the topic "LowCmdTopic" + DDSPublisher 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; +}