From 95cff05d6a082d561a8e7bbcc7d9f811b30430f0 Mon Sep 17 00:00:00 2001 From: Rooholla-Khorrambakht Date: Thu, 4 Jan 2024 14:12:41 -0500 Subject: [PATCH] basic lowlevel bridge functionality added. --- Go2Py/simple_lowcmd_sender.py | 2 +- cpp_bridge/include/utils/dds_subscriber.hpp | 1 + cpp_bridge/src/lowlevel/bridge.cpp | 163 ++++++++++---------- 3 files changed, 80 insertions(+), 86 deletions(-) diff --git a/Go2Py/simple_lowcmd_sender.py b/Go2Py/simple_lowcmd_sender.py index fb67d0c..2a621df 100644 --- a/Go2Py/simple_lowcmd_sender.py +++ b/Go2Py/simple_lowcmd_sender.py @@ -24,4 +24,4 @@ cmd = LowCmd( ) while True: dw.write(cmd) - time.sleep(0.1) \ No newline at end of file + time.sleep(0.01) \ No newline at end of file diff --git a/cpp_bridge/include/utils/dds_subscriber.hpp b/cpp_bridge/include/utils/dds_subscriber.hpp index b2ae021..86ef873 100644 --- a/cpp_bridge/include/utils/dds_subscriber.hpp +++ b/cpp_bridge/include/utils/dds_subscriber.hpp @@ -13,6 +13,7 @@ template class DDSSubscriber { public: // Define a callback type for user-provided callbacks + using UserCallback = std::function; private: diff --git a/cpp_bridge/src/lowlevel/bridge.cpp b/cpp_bridge/src/lowlevel/bridge.cpp index b627ae5..7bf087b 100644 --- a/cpp_bridge/src/lowlevel/bridge.cpp +++ b/cpp_bridge/src/lowlevel/bridge.cpp @@ -11,6 +11,9 @@ #include #include #include +#include "utils/dds_subscriber.hpp" +#include "utils/dds_publisher.hpp" + using namespace unitree::common; using namespace unitree::robot; @@ -20,44 +23,6 @@ using namespace org::eclipse::cyclonedds; constexpr double PosStopF = (2.146E+9f); constexpr double VelStopF = (16000.0f); -class Bridge -{ -public: - explicit Bridge() - { - } - - ~Bridge() - { - } - // participant = dds::domain::DomainParticipant(domain_id); - void Init(); - -private: - void InitLowCmd(); - void LowStateMessageHandler(const void* messages); - void LowCmdWrite(); - std::thread go2py_thread; - bool running = true; - void go2py_callback(); - // Go2PyListener go2py_listener; - // dds::sub::qos::DataReaderQos rqos; - - -private: - float dt = 0.002; // 0.001~0.01 - unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init - unitree_go::msg::dds_::LowState_ low_state{}; // default init - - /*publisher*/ - ChannelPublisherPtr lowcmd_publisher; - /*subscriber*/ - ChannelSubscriberPtr lowstate_subscriber; - /*LowCmd write thread*/ - ThreadPtr lowCmdWriteThreadPtr; - // dds::topic::Topic topic(participant, "go2py/lowcmd"); -}; - uint32_t crc32_core(uint32_t* ptr, uint32_t len) { unsigned int xbit = 0; @@ -90,7 +55,47 @@ uint32_t crc32_core(uint32_t* ptr, uint32_t len) return CRC32; } -void Bridge::Init() +class Go2LowLevelBridge +{ +public: + explicit Go2LowLevelBridge(const std::string &input_cmd_topic_name, + const std::string &output_state_topic_name, + const int domain_id = 0): + lowstate_publisher(output_state_topic_name), + lowcmd_subscriber(input_cmd_topic_name, std::bind(&Go2LowLevelBridge::go2py_callback, this, std::placeholders::_1)) + { + ChannelFactory::Instance()->Init(domain_id); + } + + ~Go2LowLevelBridge() + { + } + void Init(); + +private: + void InitLowCmd(); + void LowStateMessageHandler(const void* messages); + void LowCmdWrite(); + std::thread go2py_thread; + bool running = true; + void go2py_callback(const msgs::LowCmd& msg); + DDSPublisher lowstate_publisher; + DDSSubscriber lowcmd_subscriber; + + +private: + float dt = 0.002; // 0.001~0.01 + unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init + unitree_go::msg::dds_::LowState_ low_state{}; // default init + /*publisher*/ + ChannelPublisherPtr lowcmd_publisher; + /*subscriber*/ + ChannelSubscriberPtr lowstate_subscriber; + /*LowCmd write thread*/ + ThreadPtr lowCmdWriteThreadPtr; +}; + +void Go2LowLevelBridge::Init() { InitLowCmd(); /*create publisher*/ @@ -98,56 +103,26 @@ void Bridge::Init() lowcmd_publisher->InitChannel(); /*create subscriber*/ lowstate_subscriber.reset(new ChannelSubscriber(TOPIC_LOWSTATE)); - lowstate_subscriber->InitChannel(std::bind(&Bridge::LowStateMessageHandler, this, std::placeholders::_1), 1); + lowstate_subscriber->InitChannel(std::bind(&Go2LowLevelBridge::LowStateMessageHandler, this, std::placeholders::_1), 1); /*loop publishing thread*/ - lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Bridge::LowCmdWrite, this); - go2py_thread = std::thread(&Bridge::go2py_callback, this); + // lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Go2LowLevelBridge::LowCmdWrite, this); } -void Bridge::go2py_callback() +void Go2LowLevelBridge::go2py_callback(const msgs::LowCmd& msg) { - // dds::domain::DomainParticipant participant(domain::default_id()); - // /* To subscribe to something, a topic is needed. */ - // dds::topic::Topic topic(participant, "go2py/lowcmd"); - // /* A reader also needs a subscriber. */ - // // dds::sub::Subscriber subscriber(participant); - // dds::sub::NoOpSubscriberListener qlistener; /*you need to create your own class that derives from this listener, and implement your own callbacks*/ - // /*the listener implementation should implement the on_subscription_matched virtual function as we will rely on it later*/ - // dds::sub::qos::SubscriberQos subqos; /*add custom QoS policies that you want for this subscriber*/ - // dds::sub::Subscriber sub(participant, subqos, &qlistener, dds::core::status::StatusMask::subscription_matched()); - // /* Now, the reader can be created to subscribe to a HelloWorld message. */ - // dds::sub::NoOpAnyDataReaderListener listener; /*you need to create your own class that derives from this listener, and implement your own callback functions*/ - // /*the listener implementation should implement the on_data_available virtual function as we will rely on it later*/ - // dds::sub::qos::DataReaderQos rqos; - // // dds::sub::DataReader reader(subscriber, topic, rqos, &listener, dds::core::status::StatusMask::data_available()); - // dds::sub::DataReader reader(sub, topic, rqos, &listener, dds::core::status::StatusMask::data_available()); - // dds::sub::LoanedSamples samples; - - while (running) + for(int i=0; i<12; i++) { - - // /* Try taking samples from the reader. */ - // samples = reader.take(); - - // /* Are samples read? */ - // if (samples.length() > 0) { - // /* Use an iterator to run over the set of samples. */ - // dds::sub::LoanedSamples::const_iterator sample_iter; - // for (sample_iter = samples.begin(); - // sample_iter < samples.end(); - // ++sample_iter) { - // /* Get the message and sample information. */ - // const msgs::LowCmd& msg = sample_iter->data(); - // const dds::sub::SampleInfo& info = sample_iter->info(); - // std::cout << "go2py_callback got a sample..." << std::endl; - // } - // } - std::this_thread::sleep_for(std::chrono::seconds(1)); + low_cmd.motor_cmd()[i].q() = msg.q()[i]; + low_cmd.motor_cmd()[i].dq() = msg.dq()[i]; + low_cmd.motor_cmd()[i].kp() = msg.kp()[i]; + low_cmd.motor_cmd()[i].kd() = msg.kv()[i]; + low_cmd.motor_cmd()[i].tau() = msg.tau_ff()[i]; } - std::cout << "go2py_callback stopped." << std::endl; + low_cmd.crc() = crc32_core((uint32_t *)&low_cmd, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1); + lowcmd_publisher->Write(low_cmd); } -void Bridge::InitLowCmd() +void Go2LowLevelBridge::InitLowCmd() { low_cmd.head()[0] = 0xFE; low_cmd.head()[1] = 0xEF; @@ -165,12 +140,30 @@ void Bridge::InitLowCmd() } } -void Bridge::LowStateMessageHandler(const void* message) +void Go2LowLevelBridge::LowStateMessageHandler(const void* message) { low_state = *(unitree_go::msg::dds_::LowState_*)message; + msgs::LowState lowstate; + + for(int i=0; i<12; i++) + { + lowstate.q()[i] = low_state.motor_state()[i].q(); + lowstate.dq()[i] = low_state.motor_state()[i].dq(); + lowstate.ddq()[i] = low_state.motor_state()[i].ddq(); + lowstate.tau_est()[i] = low_state.motor_state()[i].tau_est(); + lowstate.tmp()[i] = (float)low_state.motor_state()[i].temperature(); + } + + lowstate.voltage() = (float)low_state.power_v(); + lowstate.current() = (float)low_state.power_a(); + + for(int i=0; i<4; i++) + lowstate.contact()[i] = (float)low_state.foot_force()[i]; + + lowstate_publisher.publish(lowstate); } -void Bridge::LowCmdWrite() +void Go2LowLevelBridge::LowCmdWrite() { low_cmd.motor_cmd()[2].q() = 0; low_cmd.motor_cmd()[2].dq() = 0; @@ -189,8 +182,8 @@ int main(int argc, const char** argv) exit(-1); } - ChannelFactory::Instance()->Init(0, argv[1]); - Bridge bridge; + // ChannelFactory::Instance()->Init(0, argv[1]); + Go2LowLevelBridge bridge("go2py/lowcmd", "go2py/lowstate"); bridge.Init(); while (1) {