feat(grpc): expose arm robot state

cmvr-es/service/grpc/include/grpc_arm_service.h

@@ -38,6 +38,9 @@ public:
     grpc::Status getJointState(grpc::ServerContext* context,
                                const api::JointRequest* request,
                                api::JointResponse* response) override;
+    grpc::Status getRobotState(grpc::ServerContext* context,
+                               const api::GetRobotState_Request* request,
+                               api::GetRobotState_Response* response) override;
     grpc::Status getPose(grpc::ServerContext* context,
                          const api::GetPose_Request* request,
                          api::GetPose_Response* response) override;

cmvr-es/service/grpc/src/grpc_arm_client_test.cpp

@@ -82,3 +82,28 @@ TEST(GrpcArmClientTest, GetJointState)
         CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
     }
 }
+
+TEST(GrpcArmClientTest, GetRobotState)
+{
+    auto stub = makeStub();
+    cmvr::api::GetRobotState_Request request;
+    fillHeader(request.mutable_header());
+
+    cmvr::api::GetRobotState_Response response;
+    grpc::ClientContext context;
+    const grpc::Status status = stub->getRobotState(&context, request, &response);
+
+    if (status.ok()) {
+        const auto& state = response.state();
+        CMVR_LOG(INFO) << "Robot state: connected=" << state.connected()
+                       << ", powered_on=" << state.powered_on()
+                       << ", moving=" << state.moving()
+                       << ", fault=" << state.fault()
+                       << ", robot_mode=" << state.robot_mode()
+                       << ", safety_mode=" << state.safety_mode()
+                       << ", control_mode=" << state.control_mode()
+                       << ", joints=" << state.actual_joint_state().name_size();
+    } else {
+        CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
+    }
+}

cmvr-es/service/grpc/src/grpc_arm_service.cpp

@@ -92,6 +92,131 @@ api::CartesianPose toApiCartesianPose(const device::CartesianPose& src)
     return dst;
 }
 
+api::CartesianVelocity toApiCartesianVelocity(const device::CartesianVelocity& src)
+{
+    api::CartesianVelocity dst;
+    dst.set_vx(src.vx);
+    dst.set_vy(src.vy);
+    dst.set_vz(src.vz);
+    dst.set_wx(src.wx);
+    dst.set_wy(src.wy);
+    dst.set_wz(src.wz);
+    return dst;
+}
+
+api::CartesianWrench toApiCartesianWrench(const device::CartesianWrench& src)
+{
+    api::CartesianWrench dst;
+    dst.set_fx(src.fx);
+    dst.set_fy(src.fy);
+    dst.set_fz(src.fz);
+    dst.set_tx(src.tx);
+    dst.set_ty(src.ty);
+    dst.set_tz(src.tz);
+    return dst;
+}
+
+api::ArmRobotMode toApiRobotMode(const device::RobotMode mode)
+{
+    switch (mode) {
+        case device::RobotMode::Disconnected:
+            return api::ARM_ROBOT_MODE_DISCONNECTED;
+        case device::RobotMode::PowerOff:
+            return api::ARM_ROBOT_MODE_POWER_OFF;
+        case device::RobotMode::Idle:
+            return api::ARM_ROBOT_MODE_IDLE;
+        case device::RobotMode::Running:
+            return api::ARM_ROBOT_MODE_RUNNING;
+        case device::RobotMode::Paused:
+            return api::ARM_ROBOT_MODE_PAUSED;
+        case device::RobotMode::Stopped:
+            return api::ARM_ROBOT_MODE_STOPPED;
+        case device::RobotMode::Fault:
+            return api::ARM_ROBOT_MODE_FAULT;
+        case device::RobotMode::Unknown:
+        default:
+            return api::ARM_ROBOT_MODE_UNKNOWN;
+    }
+}
+
+api::ArmSafetyMode toApiSafetyMode(const device::SafetyMode mode)
+{
+    switch (mode) {
+        case device::SafetyMode::Normal:
+            return api::ARM_SAFETY_MODE_NORMAL;
+        case device::SafetyMode::Reduced:
+            return api::ARM_SAFETY_MODE_REDUCED;
+        case device::SafetyMode::ProtectiveStop:
+            return api::ARM_SAFETY_MODE_PROTECTIVE_STOP;
+        case device::SafetyMode::EmergencyStop:
+            return api::ARM_SAFETY_MODE_EMERGENCY_STOP;
+        case device::SafetyMode::SafeguardStop:
+            return api::ARM_SAFETY_MODE_SAFEGUARD_STOP;
+        case device::SafetyMode::SystemEmergencyStop:
+            return api::ARM_SAFETY_MODE_SYSTEM_EMERGENCY_STOP;
+        case device::SafetyMode::Fault:
+            return api::ARM_SAFETY_MODE_FAULT;
+        case device::SafetyMode::Unknown:
+        default:
+            return api::ARM_SAFETY_MODE_UNKNOWN;
+    }
+}
+
+api::ArmControlMode toApiControlMode(const device::ControlMode mode)
+{
+    switch (mode) {
+        case device::ControlMode::Manual:
+            return api::ARM_CONTROL_MODE_MANUAL;
+        case device::ControlMode::Position:
+            return api::ARM_CONTROL_MODE_POSITION;
+        case device::ControlMode::Velocity:
+            return api::ARM_CONTROL_MODE_VELOCITY;
+        case device::ControlMode::Torque:
+            return api::ARM_CONTROL_MODE_TORQUE;
+        case device::ControlMode::Servo:
+            return api::ARM_CONTROL_MODE_SERVO;
+        case device::ControlMode::Freedrive:
+            return api::ARM_CONTROL_MODE_FREEDRIVE;
+        case device::ControlMode::None:
+        default:
+            return api::ARM_CONTROL_MODE_NONE;
+    }
+}
+
+void fillJointState(const device::RobotModel& model,
+                    const device::JointGroupState& state,
+                    api::JointState* msg)
+{
+    for (const auto& name : model.joint_names) msg->add_name(name);
+    for (const double value : state.position) msg->add_position(value);
+    for (const double value : state.velocity) msg->add_velocity(value);
+    for (const double value : state.effort) msg->add_effort(value);
+}
+
+void fillRobotState(const device::RobotModel& model,
+                    const device::ArmState& state,
+                    api::RobotState* msg)
+{
+    msg->set_timestamp(state.timestamp);
+    msg->set_robot_mode(toApiRobotMode(state.robot_mode));
+    msg->set_safety_mode(toApiSafetyMode(state.safety_mode));
+    msg->set_control_mode(toApiControlMode(state.control_mode));
+    msg->set_connected(state.connected);
+    msg->set_powered_on(state.powered_on);
+    msg->set_brake_released(state.brake_released);
+    msg->set_moving(state.moving);
+    msg->set_program_running(state.program_running);
+    msg->set_protective_stopped(state.protective_stopped);
+    msg->set_emergency_stopped(state.emergency_stopped);
+    msg->set_fault(state.fault);
+    msg->set_speed_scaling(state.speed_scaling);
+    fillJointState(model, state.actual_joint_state, msg->mutable_actual_joint_state());
+    fillJointState(model, state.target_joint_state, msg->mutable_target_joint_state());
+    *msg->mutable_actual_tcp_pose() = toApiCartesianPose(state.actual_tcp_pose);
+    *msg->mutable_actual_tcp_velocity() = toApiCartesianVelocity(state.actual_tcp_velocity);
+    *msg->mutable_actual_tcp_wrench() = toApiCartesianWrench(state.actual_tcp_wrench);
+}
+
 device::CartesianVelocity toCartesianVelocity(const api::CartesianVelocity& src)
 {
     return {src.vx(), src.vy(), src.vz(), src.wx(), src.wy(), src.wz()};
@@ -327,12 +452,7 @@ grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*,
         const auto model = arm->getRobotModel();
         const auto state = arm->getJointState();
         auto* msg = response->mutable_state();
-        for (const auto& name : model.joint_names) {
+        fillJointState(model, state, msg);
-            msg->add_name(name);
-        }
-        for (double v : state.position) msg->add_position(v);
-        for (double v : state.velocity) msg->add_velocity(v);
-        for (double v : state.effort) msg->add_effort(v);
         fillFeedback(response->mutable_header(), true);
         // CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id
         //                 << ", joints=" << msg->name_size()
@@ -344,6 +464,30 @@ grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*,
     }
 }
 
+grpc::Status gRPCArmServiceImpl::getRobotState(
+    grpc::ServerContext*,
+    const api::GetRobotState_Request* request,
+    api::GetRobotState_Response* response)
+{
+    try {
+        const std::string device_id = request->header().device_id();
+        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
+        if (!arm) {
+            return setDeviceNotFound(response, device_id);
+        }
+
+        const auto model = arm->getRobotModel();
+        const auto state = arm->getRobotState();
+        fillRobotState(model, state, response->mutable_state());
+        fillFeedback(response->mutable_header(), true);
+        logRpcSuccess("getRobotState", device_id);
+        return grpc::Status::OK;
+    } catch (const std::exception& e) {
+        fillFeedback(response->mutable_header(), false, e.what());
+        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
+    }
+}
+
 grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*,
                                          const api::GetPose_Request* request,
                                          api::GetPose_Response* response)

protos/cmvr/api/arm_command.proto

@@ -11,6 +11,38 @@ enum ArmFrameType {
   ARM_FRAME_USER = 3;
 }
 
+enum ArmRobotMode {
+  ARM_ROBOT_MODE_UNKNOWN = 0;
+  ARM_ROBOT_MODE_DISCONNECTED = 1;
+  ARM_ROBOT_MODE_POWER_OFF = 2;
+  ARM_ROBOT_MODE_IDLE = 3;
+  ARM_ROBOT_MODE_RUNNING = 4;
+  ARM_ROBOT_MODE_PAUSED = 5;
+  ARM_ROBOT_MODE_STOPPED = 6;
+  ARM_ROBOT_MODE_FAULT = 7;
+}
+
+enum ArmSafetyMode {
+  ARM_SAFETY_MODE_UNKNOWN = 0;
+  ARM_SAFETY_MODE_NORMAL = 1;
+  ARM_SAFETY_MODE_REDUCED = 2;
+  ARM_SAFETY_MODE_PROTECTIVE_STOP = 3;
+  ARM_SAFETY_MODE_EMERGENCY_STOP = 4;
+  ARM_SAFETY_MODE_SAFEGUARD_STOP = 5;
+  ARM_SAFETY_MODE_SYSTEM_EMERGENCY_STOP = 6;
+  ARM_SAFETY_MODE_FAULT = 7;
+}
+
+enum ArmControlMode {
+  ARM_CONTROL_MODE_NONE = 0;
+  ARM_CONTROL_MODE_MANUAL = 1;
+  ARM_CONTROL_MODE_POSITION = 2;
+  ARM_CONTROL_MODE_VELOCITY = 3;
+  ARM_CONTROL_MODE_TORQUE = 4;
+  ARM_CONTROL_MODE_SERVO = 5;
+  ARM_CONTROL_MODE_FREEDRIVE = 6;
+}
+
 message JointPositionCommand {
   repeated double position = 1;
 }
@@ -46,6 +78,15 @@ message CartesianVelocity {
   double wz = 6;
 }
 
+message CartesianWrench {
+  double fx = 1;
+  double fy = 2;
+  double fz = 3;
+  double tx = 4;
+  double ty = 5;
+  double tz = 6;
+}
+
 message TransformMatrix4x4 {
   double m00 = 1; double m01 = 2; double m02 = 3; double m03 = 4;
   double m10 = 5; double m11 = 6; double m12 = 7; double m13 = 8;
@@ -133,6 +174,38 @@ message JointResponse {
   JointState state = 2;
 }
 
+message RobotState {
+  double timestamp = 1;
+  ArmRobotMode robot_mode = 2;
+  ArmSafetyMode safety_mode = 3;
+  ArmControlMode control_mode = 4;
+  bool connected = 5;
+  bool powered_on = 6;
+  bool brake_released = 7;
+  bool moving = 8;
+  bool program_running = 9;
+  bool protective_stopped = 10;
+  bool emergency_stopped = 11;
+  bool fault = 12;
+  double speed_scaling = 13;
+  JointState actual_joint_state = 14;
+  JointState target_joint_state = 15;
+  CartesianPose actual_tcp_pose = 16;
+  CartesianVelocity actual_tcp_velocity = 17;
+  CartesianWrench actual_tcp_wrench = 18;
+}
+
+message GetRobotState {
+  message Request {
+    CommandHeader.Request header = 1;
+  }
+
+  message Response {
+    CommandHeader.Feedback header = 1;
+    RobotState state = 2;
+  }
+}
+
 message GetPose {
   message Request {
     CommandHeader.Request header = 1;

protos/cmvr/api/arm_service.proto

@@ -16,6 +16,7 @@ service ArmService {
   rpc servoJ(ServoJ.Request) returns (ServoJ.Response);
   rpc stopMotion(CommandHeader.Request) returns (CommandHeader.Feedback);
   rpc getJointState(JointRequest) returns (JointResponse);
+  rpc getRobotState(GetRobotState.Request) returns (GetRobotState.Response);
   rpc getPose(GetPose.Request) returns (GetPose.Response);
   rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response);
   rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response);