From fb22715f4ecbd0b05b8a7570b90f5b1963ecbfbe Mon Sep 17 00:00:00 2001
From: Cosmoslip <37982167+Cosmoflips@users.noreply.github.com>
Date: Mon, 17 Apr 2023 14:15:53 +0800
Subject: [PATCH] feat: get_estop_reason (#36)

* feat: is_down

* feat:get_estop_reason
---
 sdk/include/lebai/robot.hh |  6 +++
 sdk/src/protos/system.cc   | 79 ++++++++++++++++++++++++++++++++++++++
 sdk/src/protos/system.hh   | 29 ++++++++++++++
 sdk/src/robot.cc           |  5 +++
 sdk/src/robot_impl.cc      |  9 +++++
 sdk/src/robot_impl.hh      |  1 +
 6 files changed, 129 insertions(+)

diff --git a/sdk/include/lebai/robot.hh b/sdk/include/lebai/robot.hh
index b068e5f..9f1ff67 100644
--- a/sdk/include/lebai/robot.hh
+++ b/sdk/include/lebai/robot.hh
@@ -401,6 +401,12 @@ namespace lebai
      * @note 查看 <a href="https://help.lebai.ltd/guide/basic.html#%E6%9C%BA%E5%99%A8%E4%BA%BA%E7%8A%B6%E6%80%81">具体信息</a>.
      */
     int get_robot_mode();
+    /**
+     *  @brief 查看急停原因
+     * 
+     *  @return 急停原因
+    */
+    int get_estop_reason();
     /**
      * @brief 是否已与手臂断开连接
      *
diff --git a/sdk/src/protos/system.cc b/sdk/src/protos/system.cc
index ecaa9d9..6ae1522 100755
--- a/sdk/src/protos/system.cc
+++ b/sdk/src/protos/system.cc
@@ -236,5 +236,84 @@ namespace lebai
     {
       return false;
     }
+
+
+    void GetEstopReasonResponse::set_reason(EstopReason reason)
+    {
+      reason_ = reason;
+    }
+    const EstopReason & GetEstopReasonResponse::reason() const
+    {
+      return reason_;
+    }
+    EstopReason * GetEstopReasonResponse::mutable_reason()
+    {
+      return &reason_;
+    }
+    bool GetEstopReasonResponse::Deserialize(const rapidjson::Value& obj)
+    {
+      if (obj.HasMember("reason"))
+      {
+        std::string reason_str = std::string(obj["reason"].GetString());
+        if(reason_str == "NONE")
+        {
+          reason_ = NONE;
+        }
+        else if(reason_str == "SYSTEM")
+        {
+          reason_ = SYSTEM;
+        }
+        else if(reason_str == "MANUAL")
+        {
+          reason_ = MANUAL;
+        }
+        else if(reason_str == "HARD_ESTOP")
+        {
+          reason_ = HARD_ESTOP;
+        }
+        else if(reason_str == "COLLISION")
+        {
+          reason_ = COLLISION;
+        }
+        else if(reason_str == "JOINT_LIMIT")
+        {
+          reason_ = JOINT_LIMIT;
+        }
+        else if(reason_str == "EXCEED")
+        {
+          reason_ = EXCEED;
+        }
+        else if(reason_str == "TRAJECTORY_ERROR")
+        {
+          reason_ = TRAJECTORY_ERROR;
+        }
+        else if(reason_str == "COMM_ERROR")
+        {
+          reason_ = COMM_ERROR;
+        }
+        else if(reason_str == "CAN_ERROR")
+        {
+          reason_ = CAN_ERROR;
+        }
+        else if(reason_str == "JOINT_ERROR")
+        {
+          reason_ = JOINT_ERROR;
+        }
+        return true;
+      }
+      return false;      
+    }
+    bool GetEstopReasonResponse::Serialize(rapidjson::Writer<rapidjson::StringBuffer>* writer) const
+    {
+      writer->StartObject();
+      writer->Key("reason");
+      writer->Int(reason_);
+      writer->EndObject();
+      return true;
+    }
+    bool GetEstopReasonResponse::IsNullJSONData() const
+    {
+      return false;
+    }
   }
 }
\ No newline at end of file
diff --git a/sdk/src/protos/system.hh b/sdk/src/protos/system.hh
index 53825d1..a9379ed 100644
--- a/sdk/src/protos/system.hh
+++ b/sdk/src/protos/system.hh
@@ -101,5 +101,34 @@ namespace lebai
       virtual bool Serialize(rapidjson::Writer<rapidjson::StringBuffer>* writer) const;		
       virtual bool IsNullJSONData() const;
     };
+
+    enum EstopReason 
+    {
+      NONE = 0,
+      SYSTEM = 2,
+      MANUAL = 3,
+      HARD_ESTOP = 4,
+      COLLISION = 5,
+      JOINT_LIMIT = 6,
+      EXCEED = 7,
+      TRAJECTORY_ERROR = 8,
+      COMM_ERROR = 11,
+      CAN_ERROR = 12,
+      JOINT_ERROR = 13,
+    };
+    class GetEstopReasonResponse : public JSONBase
+    {
+    public:      
+      void set_reason(EstopReason);
+      const EstopReason & reason() const;
+      EstopReason * mutable_reason();
+    protected:
+      EstopReason reason_;
+    public:
+      virtual bool Deserialize(const rapidjson::Value& obj);
+      virtual bool Serialize(rapidjson::Writer<rapidjson::StringBuffer>* writer) const;		
+      virtual bool IsNullJSONData() const;      
+    };
+
   }
 } // namespace lebai
diff --git a/sdk/src/robot.cc b/sdk/src/robot.cc
index 4ed9313..87c75a5 100644
--- a/sdk/src/robot.cc
+++ b/sdk/src/robot.cc
@@ -448,6 +448,11 @@ int Robot::get_robot_mode()
 {
   return impl_->getRobotState();
 }
+
+int Robot::get_estop_reason()
+{
+  return impl_->getEstopReason();
+}
  
 bool Robot::is_disconnected()
 {
diff --git a/sdk/src/robot_impl.cc b/sdk/src/robot_impl.cc
index bb04fc0..7cef71f 100644
--- a/sdk/src/robot_impl.cc
+++ b/sdk/src/robot_impl.cc
@@ -180,6 +180,15 @@ namespace lebai
     return get_robot_state_resp.state();
   }
 
+  system::EstopReason Robot::RobotImpl::getEstopReason()
+  {
+    std::string resp;
+    json_rpc_connector_->CallRpc("get_estop_reason", "{}", &resp);
+    system::GetEstopReasonResponse get_estop_reason_resp;
+    get_estop_reason_resp.FromJSONString(resp);
+    return get_estop_reason_resp.reason();
+  }
+
   system::PhyData Robot::RobotImpl::getPhyData()
   {
     std::string resp;
diff --git a/sdk/src/robot_impl.hh b/sdk/src/robot_impl.hh
index c0ed232..2ff4831 100644
--- a/sdk/src/robot_impl.hh
+++ b/sdk/src/robot_impl.hh
@@ -64,6 +64,7 @@ namespace lebai
       motion::GetMotionStateResponse getMotionState(const motion::MotionIndex & req);
       void stopMove();
       system::RobotState getRobotState();
+      system::EstopReason getEstopReason();
       system::PhyData getPhyData();
       kinematic::KinData getKinData();
       io::GetDioPinResponse getDI(const io::GetDioPinRequest & req);