refactor: code udpate

example/go2/go2_video_client_custom.cpp

@@ -3,156 +3,192 @@
 #include <unistd.h>
 #include <chrono>
 
-#include <opencv2/opencv.hpp>  // OpenCV for image processing
-#include <curl/curl.h> // HTTP request
+#include <opencv2/opencv.hpp>
+#include <curl/curl.h>
 
 #include <unitree/robot/go2/video/video_client.hpp>
 #include <unitree/robot/channel/channel_subscriber.hpp>
 #include <unitree/idl/go2/SportModeState_.hpp>
+#include <unitree/idl/go2/LowState_.hpp>
 
 #define TOPIC_HIGHSTATE "rt/sportmodestate"
+#define TOPIC_LOWSTATE "rt/lowstate"
 
 using namespace unitree::common;
 
 unitree_go::msg::dds_::SportModeState_ g_state;
+unitree_go::msg::dds_::LowState_ g_low_state;
 
-void state_callback(const void* message)
-{
+// for position, rpy
+void state_callback(const void* message) {
     g_state = *(unitree_go::msg::dds_::SportModeState_*)message;
 }
 
+// for joints
+void low_state_callback(const void* message) {
+    g_low_state = *(unitree_go::msg::dds_::LowState_*)message;
+}
+
+std::string generate_motor_json(const std::array<unitree_go::msg::dds_::MotorState_, 12>& motors) {
+    std::string json = "[";
+    for (int i = 0; i < 12; ++i) {
+        json += "{\"q\":" + std::to_string(motors[i].q()) +
+                ",\"dq\":" + std::to_string(motors[i].dq()) +
+                ",\"tau\":" + std::to_string(motors[i].tau_est()) + "}";
+        if (i != 11) json += ",";
+    }
+    json += "]";
+    return json;
+}
+
 void send_to_vllm_server(const std::string& image_path,
-    const std::array<float, 3>& pos,
-    const std::array<float, 3>& rpy)
+                         const std::array<float, 3>& pos,
+                         const std::array<float, 3>& rpy,
+                         const std::array<unitree_go::msg::dds_::MotorState_, 12>& joints)
 {
-    CURL *curl;
-    CURLcode res;
-    curl_mime *form = nullptr;
-    curl_mimepart *field = nullptr;
-
-    curl_global_init(CURL_GLOBAL_ALL);
-    curl = curl_easy_init();
-    if (curl) {
-        form = curl_mime_init(curl);
-
-        // 이미지 전송
-        field = curl_mime_addpart(form);
-        curl_mime_name(field, "image");
-        curl_mime_filedata(field, image_path.c_str());
-
-        // 위치 (position)
-        std::string pos_json = "{\"x\":" + std::to_string(pos[0]) +
-          ",\"y\":" + std::to_string(pos[1]) +
-          ",\"z\":" + std::to_string(pos[2]) + "}";
-        field = curl_mime_addpart(form);
-        curl_mime_name(field, "position");
-        curl_mime_data(field, pos_json.c_str(), CURL_ZERO_TERMINATED);
-
-        // RPY (roll/pitch/yaw)
-        std::string rpy_json = "{\"roll\":" + std::to_string(rpy[0]) +
-          ",\"pitch\":" + std::to_string(rpy[1]) +
-          ",\"yaw\":" + std::to_string(rpy[2]) + "}";
-        field = curl_mime_addpart(form);
-        curl_mime_name(field, "rpy");
-        curl_mime_data(field, rpy_json.c_str(), CURL_ZERO_TERMINATED);
-
-        // send to server (http://127.0.0.1:5000/analyze)
-        curl_easy_setopt(curl, CURLOPT_URL, "http://127.0.0.1:5000/analyze");
-        curl_easy_setopt(curl, CURLOPT_MIMEPOST, form);
-
-        res = curl_easy_perform(curl);
-        if (res != CURLE_OK) {
-            std::cerr << "[curl error] " << curl_easy_strerror(res) << std::endl;
-        }
+    CURL *curl = curl_easy_init();
+    if (!curl) {
+        std::cerr << "[curl error] Failed to initialize CURL." << std::endl;
+        return;
+    }
+
+    curl_mime* form = curl_mime_init(curl);
+
+    auto add_field = [&](const char* name, const char* data) {
+        curl_mimepart* field = curl_mime_addpart(form);
+        curl_mime_name(field, name);
+        curl_mime_data(field, data, CURL_ZERO_TERMINATED);
+    };
+
+    auto add_json_field = [&](const char* name, const std::string& json) {
+        add_field(name, json.c_str());
+    };
 
-        curl_mime_free(form);
-        curl_easy_cleanup(curl);
+    // Image
+    curl_mimepart* field = curl_mime_addpart(form);
+    curl_mime_name(field, "image");
+    curl_mime_filedata(field, image_path.c_str());
+
+    // Position & RPY
+    add_json_field("position", "{\"x\":" + std::to_string(pos[0]) + ",\"y\":" + std::to_string(pos[1]) + ",\"z\":" + std::to_string(pos[2]) + "}");
+    add_json_field("rpy", "{\"roll\":" + std::to_string(rpy[0]) + ",\"pitch\":" + std::to_string(rpy[1]) + ",\"yaw\":" + std::to_string(rpy[2]) + "}");
+
+    // Joints
+    add_json_field("joints", generate_motor_json(joints));
+
+    // CURL options
+    curl_easy_setopt(curl, CURLOPT_URL, "http://127.0.0.1:5000/analyze");
+    curl_easy_setopt(curl, CURLOPT_MIMEPOST, form);
+
+    CURLcode res = curl_easy_perform(curl);
+    if (res != CURLE_OK) {
+        std::cerr << "[curl error] " << curl_easy_strerror(res) << std::endl;
     }
-    curl_global_cleanup();
+
+    curl_mime_free(form);
+    curl_easy_cleanup(curl);
 }
-
+
+cv::Mat process_image(const std::vector<uint8_t>& image_data, const std::string& save_path) {
+    std::string tmp_path = "tmp_raw.jpg";
+    std::ofstream file(tmp_path, std::ios::binary);
+    file.write(reinterpret_cast<const char*>(image_data.data()), image_data.size());
+    file.close();
+
+    cv::Mat img = cv::imread(tmp_path);
+    if (img.empty()) {
+        throw std::runtime_error("Failed to load image with OpenCV.");
+    }
+
+    int size = std::min(img.rows, img.cols);
+    cv::Rect roi((img.cols - size) / 2, (img.rows - size) / 2, size, size);
+
+    cv::Mat cropped = img(roi);
+    cv::Mat resized;
+    cv::resize(cropped, resized, cv::Size(320, 320));
+    cv::imwrite(save_path, resized);
+
+    return resized;
+}
+
+void print_pose_info(const std::array<float, 3>& pos, const std::array<float, 3>& rpy) {
+    std::cout << "Position: x=" << pos[0] << ", y=" << pos[1] << ", z=" << pos[2] << std::endl;
+    std::cout << "RPY: roll=" << rpy[0] << ", pitch=" << rpy[1] << ", yaw=" << rpy[2] << std::endl;
+}
+
+void print_motor_info(const std::array<unitree_go::msg::dds_::MotorState_, 12>& motors) {
+    const char* leg_names[4] = {"FR", "FL", "RR", "RL"};
+    const char* joint_names[3] = {"Hip", "Thigh", "Calf"};
+
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            int idx = leg * 3 + j;
+            std::cout << leg_names[leg] << "_" << joint_names[j] 
+                      << " | q: " << motors[idx].q()
+                      << " dq: " << motors[idx].dq()
+                      << " tau: " << motors[idx].tau_est() << std::endl;
+        }
+    }
+}
+
 
 int main()
 {
-    // Init SDK
     unitree::robot::ChannelFactory::Instance()->Init(0);
 
-    // Init video
     unitree::robot::go2::VideoClient video_client;
     video_client.SetTimeout(1.0f);
     video_client.Init();
 
-    // Init state subscriber
-    auto sub = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>>(TOPIC_HIGHSTATE);
-    sub->InitChannel(state_callback, 1);
+    // subscribers
+    auto high_sub = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>>(TOPIC_HIGHSTATE);
+    high_sub->InitChannel(state_callback, 1);
+
+    auto low_sub = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>>(TOPIC_LOWSTATE);
+    low_sub->InitChannel(low_state_callback, 1);
 
     std::vector<uint8_t> image_sample;
-    int ret;
 
     while (true)
     {
-        auto start_time = std::chrono::steady_clock::now();  // frame start time
-        ret = video_client.GetImageSample(image_sample);
-
-        if (ret == 0 && !image_sample.empty()) {
-            std::string raw_name = "raw_image.jpg";
-            std::string processed_name = "last_image.jpg";
-
-            // Save raw image
-            std::ofstream image_file(raw_name, std::ios::binary);
-            if (image_file.is_open()) {
-                image_file.write(reinterpret_cast<const char*>(image_sample.data()), image_sample.size());
-                image_file.close();
-            } else {
-                std::cerr << "Error: Failed to save raw image." << std::endl;
-                continue;
-            }
+        auto start = std::chrono::steady_clock::now();
+        if (video_client.GetImageSample(image_sample) != 0 || image_sample.empty()) {
+            usleep(50000);
+            continue;
+        }
 
-            // Load with OpenCV
-            cv::Mat img = cv::imread(raw_name);
-            if (img.empty()) {
-                std::cerr << "Error: Failed to read image with OpenCV." << std::endl;
-                continue;
-            }
+        try {
+            std::string output_image = "processed_image.jpg";
+            process_image(image_sample, output_image);
 
-            // Center crop
-            int h = img.rows;
-            int w = img.cols;
-            int crop_size = std::min(h, w);
-            int top = (h - crop_size) / 2;
-            int left = (w - crop_size) / 2;
-            cv::Mat cropped = img(cv::Rect(left, top, crop_size, crop_size));
-
-            // Resize to 320x320
-            cv::Mat resized;
-            cv::resize(cropped, resized, cv::Size(320, 320));
-
-            // Save final image
-            cv::imwrite(processed_name, resized);
-
-            // Pose info
-            const auto& pos = g_state.position();
-            const auto& rpy = g_state.imu_state().rpy();
-
-            if (pos.size() >= 3 && rpy.size() >= 3) {
-                std::cout << "Image saved as " << processed_name << std::endl;
-                std::cout << "Position: x=" << pos[0] << ", y=" << pos[1] << ", z=" << pos[2] << std::endl;
-                std::cout << "RPY: roll=" << rpy[0] << ", pitch=" << rpy[1] << ", yaw=" << rpy[2] << std::endl;
-            } else {
+            const auto& pos_vec = g_state.position();
+            const auto& rpy_vec = g_state.imu_state().rpy();
+            const auto& motors = g_low_state.motor_state();
+
+            if (pos_vec.size() < 3 || rpy_vec.size() < 3) {
                 std::cout << "Waiting for valid pose data..." << std::endl;
+                usleep(50000);
+                continue;
             }
 
-            auto end_time = std::chrono::steady_clock::now();
-            auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
-            std::cout << "Elapsed time: " << elapsed_time << " ms" << std::endl;
+            std::array<float, 3> pos = {pos_vec[0], pos_vec[1], pos_vec[2]};
+            std::array<float, 3> rpy = {rpy_vec[0], rpy_vec[1], rpy_vec[2]};
+            std::array<unitree_go::msg::dds_::MotorState_, 12> joints;
+            for (int i = 0; i < 12; ++i) joints[i] = motors[i];
+
+            print_pose_info(pos, rpy);
+            print_motor_info(joints);
+
+            send_to_vllm_server(output_image, pos, rpy, joints);
+
+            auto end = std::chrono::steady_clock::now();
+            std::cout << "Elapsed: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << " ms" << std::endl;
 
-            // send to server
-            std::array<float, 3> pos_arr = {pos[0], pos[1], pos[2]};
-            std::array<float, 3> rpy_arr = {rpy[0], rpy[1], rpy[2]};
-            send_to_vllm_server(processed_name, pos_arr, rpy_arr);
+        } catch (const std::exception& e) {
+            std::cerr << "[Error] " << e.what() << std::endl;
         }
 
-        usleep(50000);  // 0.05 seconds
+        usleep(50000);
     }
 
     return 0;

server_test.py

@@ -5,24 +5,30 @@
 app = Flask(__name__)
 
 UPLOAD_PATH = "received.jpg"
-POSE_PATH = "pose.json"  # 위치 + 자세 값 저장용
+POSE_PATH = "pose.json"  # 위치 + 자세 + 조인트 값 저장
 
 @app.route("/analyze", methods=["POST"])
 def analyze():
     image = request.files["image"]
     pos = json.loads(request.form["position"])
     rpy = json.loads(request.form["rpy"])
+    joints = json.loads(request.form["joints"])
 
     print("Received image:", image.filename)
     print("Position:", pos)
     print("RPY:", rpy)
+    print("Joints:", joints)
 
     # 이미지 저장
     image.save(UPLOAD_PATH)
 
-    # 값 저장
+    # 값 저장 (position + rpy + joints)
     with open(POSE_PATH, "w") as f:
-        json.dump({"position": pos, "rpy": rpy}, f)
+        json.dump({
+            "position": pos,
+            "rpy": rpy,
+            "joints": joints
+        }, f)
 
     return {"result": "ok"}
 
@@ -34,6 +40,13 @@ def view():
     with open(POSE_PATH, "r") as f:
         data = json.load(f)
 
+    joint_names = [
+        "(0) FR_Hip", "(1) FR_Thigh", "(2) FR_Calf",
+        "(3) FL_Hip", "(4) FL_Thigh", "(5) FL_Calf",
+        "(6) RR_Hip", "(7) RR_Thigh", "(8) RR_Calf",
+        "(9) RL_Hip", "(10) RL_Thigh", "(11) RL_Calf"
+    ]
+
     html = """
     <html>
     <head>
@@ -42,6 +55,9 @@ def view():
             body { font-family: Arial; text-align: center; padding: 30px; }
             img { max-width: 400px; border: 1px solid #ccc; margin-bottom: 20px; }
             .info { font-size: 18px; }
+            table { margin: 0 auto; border-collapse: collapse; font-size: 16px; }
+            th, td { border: 1px solid #ccc; padding: 8px 12px; }
+            th { background-color: #f0f0f0; }
         </style>
     </head>
     <body>
@@ -53,11 +69,23 @@ def view():
             <h2>RPY</h2>
             <p>roll = {{ rpy['roll'] }}, pitch = {{ rpy['pitch'] }}, yaw = {{ rpy['yaw'] }}</p>
         </div>
+        <h2>Joints</h2>
+        <table>
+        <tr><th>Joint</th><th>q</th><th>dq</th><th>tau</th></tr>
+        {% for i in range(joints|length) %}
+            <tr>
+                <td>{{ joint_names[i] }}</td>
+                <td>{{ "%.4f"|format(joints[i]["q"]) }}</td>
+                <td>{{ "%.4f"|format(joints[i]["dq"]) }}</td>
+                <td>{{ "%.4f"|format(joints[i]["tau"]) }}</td>
+            </tr>
+        {% endfor %}
+        </table>
     </body>
     </html>
     """
 
-    return render_template_string(html, pos=data["position"], rpy=data["rpy"])
+    return render_template_string(html, pos=data["position"], rpy=data["rpy"], joints=data["joints"], joint_names=joint_names)
 
 @app.route("/latest_image", methods=["GET"])
 def latest_image():