From 1cd57c36eaeef97da2d91141bd431178e94a1dac Mon Sep 17 00:00:00 2001
From: linbo <1034003879@qq.com>
Date: Wed, 25 Feb 2026 09:55:53 +0800
Subject: [PATCH] add calibrateZeroQ

---
 .../config/camera_config/camera_config.pb.txt | 49 ++++++++++++++++++-
 .../include/grpc_humanoid_robot_service.h     |  3 ++
 .../grpc/src/grpc_humanoid_robot_service.cpp  | 25 ++++++++++
 protos/cmvr/api/humanoid_robot_command.proto  | 11 +++++
 protos/cmvr/api/humanoid_robot_service.proto  |  1 +
 5 files changed, 87 insertions(+), 2 deletions(-)

diff --git a/cmvr-es/common/config/camera_config/camera_config.pb.txt b/cmvr-es/common/config/camera_config/camera_config.pb.txt
index b99b75d2..f0cc11fa 100644
--- a/cmvr-es/common/config/camera_config/camera_config.pb.txt
+++ b/cmvr-es/common/config/camera_config/camera_config.pb.txt
@@ -1,5 +1,5 @@
 realsense_cameras {
-  id: "cam2"
+  id: "cam1"
   serialNumber: "243122074587"
   width: 640
   height: 480
@@ -10,11 +10,41 @@ realsense_cameras {
   align_mode: ALIGN_MODE_COLOR
   buffer_size: 30
   sync: false
+  enable: false
+}
+
+realsense_cameras {
+  id: "left_hand_cam"
+  serialNumber: "243122072252"
+  width: 640
+  height: 480
+  fps: 30
+  codec: "H265"
+  camera_mode: CAMERA_MODE_VIDEO
+  stream_mode: STREAM_MODE_RGBD
+  align_mode: ALIGN_MODE_COLOR
+  buffer_size: 30
+  sync: false
   enable: true
 }
 
+realsense_cameras {
+  id: "cam3"
+  serialNumber: "243122075614"
+  width: 640
+  height: 480
+  fps: 30
+  codec: "H265"
+  camera_mode: CAMERA_MODE_VIDEO
+  stream_mode: STREAM_MODE_RGBD
+  align_mode: ALIGN_MODE_COLOR
+  buffer_size: 30
+  sync: false
+  enable: false
+}
+
 uvc_cameras {
-  id: "cam1"
+  id: "left_eye_cam"
   usb: "/dev/uvc_left_camera"
   width: 640
   height: 480
@@ -25,3 +55,18 @@ uvc_cameras {
   buffer_size: 30
   enable: false
 }
+
+realsense_cameras {
+  id: "cam5"
+  serialNumber: "243122075389"
+  width: 640
+  height: 480
+  fps: 30
+  codec: "H265"
+  camera_mode: CAMERA_MODE_VIDEO
+  stream_mode: STREAM_MODE_RGBD
+  align_mode: ALIGN_MODE_COLOR
+  buffer_size: 30
+  sync: false
+  enable: false
+}
\ No newline at end of file
diff --git a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
index b53e0eef..a8a404ea 100644
--- a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
+++ b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
@@ -27,7 +27,10 @@ namespace cmvr {
             grpc::Status speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response) override;
 
             grpc::Status getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) override;
+
             grpc::Status getPose(grpc::ServerContext *context, const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) override;
+
+            grpc::Status calibrateZeroQ(grpc::ServerContext* context, const cmvr::api::CalibrateZeroQ_Request* request, cmvr::api::CalibrateZeroQ_Response* response) override;
         private:
             device::DeviceManager& dmgr_;
         };
diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
index 0f032cd7..a87180b5 100644
--- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
+++ b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
@@ -219,3 +219,28 @@ grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *co
     *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
     return ret;
 }
+
+
+grpc::Status gRPCHumanoidRobotServiceImpl::calibrateZeroQ(grpc::ServerContext* context
+    , const cmvr::api::CalibrateZeroQ_Request* request
+    , cmvr::api::CalibrateZeroQ_Response* response) {
+
+    grpc::Status ret = grpc::Status::OK;
+
+    try {
+        auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
+
+        std::string joint_name = request->joint_name();
+
+        robot->calibrateZeroQ(joint_name);
+
+        response->mutable_header()->set_success(true);
+        response->mutable_header()->set_error_message("");
+    }catch (const std::exception& e) {
+        response->mutable_header()->set_success(false);
+        response->mutable_header()->set_error_message(e.what());
+        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
+    }
+    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
+    return ret;
+}
diff --git a/protos/cmvr/api/humanoid_robot_command.proto b/protos/cmvr/api/humanoid_robot_command.proto
index d3554373..f39ad778 100644
--- a/protos/cmvr/api/humanoid_robot_command.proto
+++ b/protos/cmvr/api/humanoid_robot_command.proto
@@ -133,3 +133,14 @@ message JointRequest {
   CommandHeader.Request header = 1;
 }
 
+
+message CalibrateZeroQ {
+    message Request{
+        CommandHeader.Request header = 1;
+        string joint_name = 2;
+    }
+
+    message Response{
+        CommandHeader.Feedback header= 1;
+    }
+}
diff --git a/protos/cmvr/api/humanoid_robot_service.proto b/protos/cmvr/api/humanoid_robot_service.proto
index 8438ffbc..33502e50 100644
--- a/protos/cmvr/api/humanoid_robot_service.proto
+++ b/protos/cmvr/api/humanoid_robot_service.proto
@@ -14,4 +14,5 @@ service HumanoidRobotService{
   rpc speedL(SpeedL.Request) returns (SpeedL.Response);
   rpc getJointState(JointRequest) returns (JointResponse);
   rpc getPose(GetPose.Request) returns (GetPose.Response);
+  rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response);
 }