feat: 初始化 Go2 导航相关代码与配置

unitree_sdk2/example/wireless_controller/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_executable(advanced_gamepad main.cpp)
+target_link_libraries(advanced_gamepad unitree_sdk2)

unitree_sdk2/example/wireless_controller/advanced_gamepad.hpp

@@ -0,0 +1,116 @@
+#pragma once
+
+#include <stdint.h>
+#include <cmath>
+#include "unitree/idl/go2/WirelessController_.hpp"
+// #include <go2_idl/WirelessController_.hpp>
+
+namespace unitree::common
+{
+    // union for keys
+    typedef union
+    {
+        struct
+        {
+            uint8_t R1 : 1;
+            uint8_t L1 : 1;
+            uint8_t start : 1;
+            uint8_t select : 1;
+            uint8_t R2 : 1;
+            uint8_t L2 : 1;
+            uint8_t F1 : 1;
+            uint8_t F2 : 1;
+            uint8_t A : 1;
+            uint8_t B : 1;
+            uint8_t X : 1;
+            uint8_t Y : 1;
+            uint8_t up : 1;
+            uint8_t right : 1;
+            uint8_t down : 1;
+            uint8_t left : 1;
+        } components;
+        uint16_t value;
+    } xKeySwitchUnion;
+
+    // single button class
+    class Button
+    {
+    public:
+        Button() {}
+
+        void update(bool state)
+        {
+            on_press = state ? state != pressed : false;
+            on_release = state ? false : state != pressed;
+            pressed = state;
+        }
+
+        bool pressed = false;
+        bool on_press = false;
+        bool on_release = false;
+    };
+
+    // full gamepad
+    class Gamepad
+    {
+    public:
+        Gamepad() {}
+
+        void Update(unitree_go::msg::dds_::WirelessController_ &key_msg)
+        {
+            // update stick values with smooth and deadzone
+            lx = lx * (1 - smooth) + (std::fabs(key_msg.lx()) < dead_zone ? 0.0 : key_msg.lx()) * smooth;
+            rx = rx * (1 - smooth) + (std::fabs(key_msg.rx()) < dead_zone ? 0.0 : key_msg.rx()) * smooth;
+            ry = ry * (1 - smooth) + (std::fabs(key_msg.ry()) < dead_zone ? 0.0 : key_msg.ry()) * smooth;
+            ly = ly * (1 - smooth) + (std::fabs(key_msg.ly()) < dead_zone ? 0.0 : key_msg.ly()) * smooth;
+
+            // update button states
+            key.value = key_msg.keys();
+
+            R1.update(key.components.R1);
+            L1.update(key.components.L1);
+            start.update(key.components.start);
+            select.update(key.components.select);
+            R2.update(key.components.R2);
+            L2.update(key.components.L2);
+            F1.update(key.components.F1);
+            F2.update(key.components.F2);
+            A.update(key.components.A);
+            B.update(key.components.B);
+            X.update(key.components.X);
+            Y.update(key.components.Y);
+            up.update(key.components.up);
+            right.update(key.components.right);
+            down.update(key.components.down);
+            left.update(key.components.left);
+        }
+
+        float smooth = 0.03;
+        float dead_zone = 0.01;
+
+        float lx = 0.;
+        float rx = 0.;
+        float ry = 0.;
+        float ly = 0.;
+
+        Button R1;
+        Button L1;
+        Button start;
+        Button select;
+        Button R2;
+        Button L2;
+        Button F1;
+        Button F2;
+        Button A;
+        Button B;
+        Button X;
+        Button Y;
+        Button up;
+        Button right;
+        Button down;
+        Button left;
+
+    private:
+        xKeySwitchUnion key;
+    };
+} // namespace unitree::common

unitree_sdk2/example/wireless_controller/main.cpp

@@ -0,0 +1,108 @@
+#include <mutex>
+
+// #include <go2_idl/WirelessController_.hpp>
+#include "unitree/idl/go2/WirelessController_.hpp"
+#include "unitree/common/thread/thread.hpp"
+#include "unitree/robot/channel/channel_subscriber.hpp"
+
+#include "advanced_gamepad.hpp"
+
+#define TOPIC_JOYSTICK "rt/wirelesscontroller"
+
+using namespace unitree::common;
+using namespace unitree::robot;
+
+class GamepadExample
+{
+public:
+    GamepadExample() {}
+
+    // setup dds model
+    void InitDdsModel(const std::string &networkInterface = "")
+    {
+        ChannelFactory::Instance()->Init(0, networkInterface);
+        joystick_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>(TOPIC_JOYSTICK));
+
+        joystick_subscriber->InitChannel(std::bind(&GamepadExample::MessageHandler, this, std::placeholders::_1), 1);
+    }
+
+    // set gamepad dead_zone parameter
+    void SetGamepadDeadZone(float deadzone)
+    {
+        gamepad.dead_zone = deadzone;
+    }
+
+    // set gamepad smooth parameter
+    void setGamepadSmooth(float smooth)
+    {
+        gamepad.smooth = smooth;
+    }
+
+    // callback function to save joystick message
+    void MessageHandler(const void *message)
+    {
+        std::lock_guard<std::mutex> lock(joystick_mutex);
+        joystick_msg = *(unitree_go::msg::dds_::WirelessController_ *)message;
+    }
+
+    // work thread
+    void Step()
+    {
+        {
+            std::lock_guard<std::mutex> lock(joystick_mutex);
+            gamepad.Update(joystick_msg);
+        }
+
+        // some operations
+        if (gamepad.A.on_press)
+        {
+            press_count += 1;
+        }
+
+        // print gamepad state
+        std::cout << "lx: " << gamepad.lx << std::endl
+                  << "A: pressed: " << gamepad.A.pressed
+                  << "; on_press: " << gamepad.A.on_press
+                  << "; on_release: " << gamepad.A.on_release
+                  << std::endl << "press count: " << press_count
+                  << std::endl << "===========================" << std::endl;
+    }
+
+    // start the work thread
+    void Start()
+    {
+        control_thread_ptr = CreateRecurrentThreadEx("nn_ctrl", UT_CPU_ID_NONE, 40000, &GamepadExample::Step, this);
+    }
+
+protected:
+    ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber;
+    unitree_go::msg::dds_::WirelessController_ joystick_msg;
+
+    Gamepad gamepad;
+
+    ThreadPtr control_thread_ptr;
+
+    std::mutex joystick_mutex;
+
+    int press_count = 0;
+};
+
+int main()
+{
+    // create example object
+    GamepadExample example;
+
+    // set gamepad params
+    example.setGamepadSmooth(0.2);
+    example.SetGamepadDeadZone(0.5);
+
+    // start program
+    example.InitDdsModel();
+    example.Start();
+
+    while (1)
+    {
+        usleep(20000);
+    }
+    return 0;
+}