basic lowlevel bridge functionality added.

Go2Py/simple_lowcmd_sender.py

@@ -24,4 +24,4 @@ cmd = LowCmd(
 )
 while True:
     dw.write(cmd)
-    time.sleep(0.1)
+    time.sleep(0.01)

cpp_bridge/include/utils/dds_subscriber.hpp

@@ -13,6 +13,7 @@ template<typename T>
 class DDSSubscriber {
     public:
         // Define a callback type for user-provided callbacks
+        
         using UserCallback = std::function<void(const T&)>;
 
     private:

cpp_bridge/src/lowlevel/bridge.cpp

@@ -11,6 +11,9 @@
 #include <go2py/LowCmd.hpp>
 #include <go2py/LowState.hpp>
 #include <thread>
+#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<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
-    /*subscriber*/
-    ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
-    /*LowCmd write thread*/
-    ThreadPtr lowCmdWriteThreadPtr;
-    // dds::topic::Topic<unitree_go::msg::dds_::LowCmd_> 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<msgs::LowState> lowstate_publisher;
+    DDSSubscriber<msgs::LowCmd> 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<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
+    /*subscriber*/
+    ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> 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<unitree_go::msg::dds_::LowState_>(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);  
+    // lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Go2LowLevelBridge::LowCmdWrite, this);  
-    go2py_thread = std::thread(&Bridge::go2py_callback, this);
 }
 
-void Bridge::go2py_callback()
+void Go2LowLevelBridge::go2py_callback(const msgs::LowCmd& msg)
 {
-    // dds::domain::DomainParticipant participant(domain::default_id());
+    for(int i=0; i<12; i++)
-    // /* To subscribe to something, a topic is needed. */
-    // dds::topic::Topic<msgs::LowCmd> 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<msgs::LowCmd> reader(subscriber, topic, rqos, &listener, dds::core::status::StatusMask::data_available());
-    // dds::sub::DataReader<msgs::LowCmd> reader(sub, topic, rqos, &listener, dds::core::status::StatusMask::data_available());
-    // dds::sub::LoanedSamples<msgs::LowCmd> samples;
-
-    while (running)
     {
-
+        low_cmd.motor_cmd()[i].q() = msg.q()[i];
-        // /* Try taking samples from the reader. */
+        low_cmd.motor_cmd()[i].dq() = msg.dq()[i];
-        // samples = reader.take();
+        low_cmd.motor_cmd()[i].kp() = msg.kp()[i];
-
+        low_cmd.motor_cmd()[i].kd() = msg.kv()[i];
-        // /* Are samples read? */
+        low_cmd.motor_cmd()[i].tau() = msg.tau_ff()[i];
-        // if (samples.length() > 0) {
-        //     /* Use an iterator to run over the set of samples. */
-        //     dds::sub::LoanedSamples<msgs::LowCmd>::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));
     }
-    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]);
+    // ChannelFactory::Instance()->Init(0, argv[1]);
-    Bridge bridge;
+    Go2LowLevelBridge bridge("go2py/lowcmd", "go2py/lowstate");
     bridge.Init();
     while (1)
     {