add calibrateZeroQ

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
+}

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_;
         };

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;
+}

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;
+    }
+}

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);
 }