feat: 添加wait_task接口

lebai-robotics · Jan 9, 2025 · 89b10b8 · 89b10b8
1 parent 13cc8c9
commit 89b10b8
Show file tree

Hide file tree

Showing 11 changed files with 105 additions and 23 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -8,12 +8,12 @@ endif()
 
 project(
   lebai
-  VERSION 1.1.31
+  VERSION 1.1.32
   LANGUAGES CXX)
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 
-option(CMAKE_EXPORT_COMPILE_COMMANDS "Export compile command" TRUE)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 set(PROJECT_NAMESPACE lebai)
 

diff --git a/Doxyfile b/Doxyfile
@@ -38,7 +38,7 @@ PROJECT_NAME           = "lebai sdk"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 1.1.31
+PROJECT_NUMBER         = 1.1.32
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

diff --git a/doc/changelog.md b/doc/changelog.md
@@ -1,4 +1,7 @@
 # ChangeLog
+## 1.1.32
+添加wait_task接口
+
 ## 1.1.31
 python镜像升级
 

diff --git a/examples/example_dummy.cc b/examples/example_dummy.cc
@@ -45,27 +45,18 @@ int main(int argc, char **argv)
   lebai::l_master::Robot robot(ip, sim);
   std::this_thread::sleep_for(std::chrono::seconds(1));
 
-  robot.movej({73.6359 / 180.0 * M_PI, -68.4064 / 180.0 * M_PI, -52.2675 / 180.0 * M_PI, 60.5182 / 180.0 * M_PI, -79.6674 / 180.0 * M_PI, 92.9829 / 180.0 * M_PI}, 1.0, 0.5, 0.0, 0.0);
-  robot.wait_move();
 
-  std::vector<double> jp = {73.6359 / 180.0 * M_PI, -68.4064 / 180.0 * M_PI, -52.2675 / 180.0 * M_PI, 60.5182 / 180.0 * M_PI, -79.6674 / 180.0 * M_PI, 92.9829 / 180.0 * M_PI};
-  std::cout << "jp " << jp[0] << ", " << jp[1] << ", " << jp[2] << ", " << jp[3] << ", " << jp[4] << ", " << jp[5] << std::endl;
-  lebai::l_master::CartesianPose pose;
-  pose["x"] = 0.088501997;
-  pose["y"] = -0.29743993;
-  pose["z"] = 0.70731318;
-  pose["rx"] = 16.6127 / 180.0 * M_PI;
-  pose["ry"] = 86.6987 / 180.0 * M_PI;
-  pose["rz"] = -66.8482 / 180.0 * M_PI;
-
-  auto ik_resp = robot.kinematics_inverse(pose, jp);
-  if (ik_resp.ok)
-  {
-    std::cout << "kinematics_inverse: " << ik_resp.joint_positions[0] << ", " << ik_resp.joint_positions[1] << ", " << ik_resp.joint_positions[2] << ", " << ik_resp.joint_positions[3] << ", " << ik_resp.joint_positions[4] << ", " << ik_resp.joint_positions[5] << std::endl;
-  }
-  else
+  robot.start_sys();
+  auto id = robot.start_task("10006");
+  std::cout<<"start task id: "<<id<<std::endl;
+  while (true)
   {
-    std::cout << "ik not ok!\n";
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+    std::cout<<"wait_task: "<<robot.wait_task(id)<<std::endl;
+    // if(robot.get_task_state(10004) == "SUCCESS")
+    // {
+    //   break;
+    // }
   }
   // jp = {-28/ 180.0 * M_PI, -59.0/ 180.0 * M_PI, 96.0/ 180.0 * M_PI, -2.0/ 180.0 * M_PI, -92.0/ 180.0 * M_PI, 16.0/ 180.0 * M_PI};
   // std::cout<<"jp "<<jp[0]<<", "<<jp[1]<<", "<<jp[2]<<", "<<jp[3]<<", "<<jp[4]<<", "<<jp[5]<<std::endl;

diff --git a/sdk/include/lebai/robot.hh b/sdk/include/lebai/robot.hh
@@ -827,6 +827,13 @@ class Robot {
    * @brief 查询任务列表
    */
   std::vector<unsigned int> load_task_list();
+  /**
+   * @brief 等待任务完成
+   *
+   * @param id: 任务的ID
+   * @return 返回任务是否成功
+   */
+  bool wait_task(unsigned int id);
   /**
    * @brief 暂停任务与运动
    *

diff --git a/sdk/src/jsonrpc_connector.cc b/sdk/src/jsonrpc_connector.cc
@@ -127,7 +127,7 @@ int JSONRpcConnector::CallRpc(const std::string &method,
   }
   using namespace std::chrono_literals;
   // TODO(@liufang) This corner case is not not good, need to be improved.
-  if (method != "wait_move") {
+  if (method != "wait_move" && method != "wait_task") {
     // TODO(@liufang) Make the timeout configurable.
     std::future_status status = future.wait_for(5s);
     switch (status) {

diff --git a/sdk/src/protos/control.cc b/sdk/src/protos/control.cc
@@ -125,6 +125,47 @@ bool TaskIndex::Serialize(
 }
 bool TaskIndex::IsNullJSONData() const { return false; }
 
+void TaskStdout::set_id(unsigned int id) { id_ = id; }
+unsigned int TaskStdout::id() { return id_; }
+unsigned int *TaskStdout::mutable_id() { return &id_; }
+
+void TaskStdout::set_done(bool done) { done_ = done; }
+bool TaskStdout::done() { return done_; }
+bool *TaskStdout::mutable_done() { return &done_; }
+
+void TaskStdout::set_stdout(std::string stdout) { stdout_ = stdout; }
+std::string TaskStdout::stdout() { return stdout_; }
+std::string *TaskStdout::mutable_stdout() { return &stdout_; }
+
+bool TaskStdout::Deserialize(const rapidjson::Value &obj) {
+  if (obj.HasMember("id")) {
+    unsigned int id_int = (unsigned int)(obj["id"].GetUint());
+    id_ = id_int;
+  }
+  if (obj.HasMember("done")) {
+    bool done_bool = (bool)(obj["done"].GetBool());
+    done_ = done_bool;
+  }
+  if (obj.HasMember("stdout")) {
+    std::string stdout_str = (std::string)(obj["stdout"].GetString());
+    stdout_ = stdout_str;
+  }
+  return true;
+}
+bool TaskStdout::Serialize(
+    rapidjson::Writer<rapidjson::StringBuffer> *writer) const {
+  writer->StartObject();
+  writer->Key("id");
+  writer->Uint(id_);
+  writer->Key("done");
+  writer->Bool(done_);
+  writer->Key("stdout");
+  writer->String(stdout_.c_str());
+  writer->EndObject();
+  return true;
+}
+bool TaskStdout::IsNullJSONData() const { return false; }
+
 void PauseRequest::set_id(unsigned int id) { id_ = id; }
 unsigned int PauseRequest::id() { return id_; }
 unsigned int *PauseRequest::mutable_id() { return &id_; }

diff --git a/sdk/src/protos/control.hh b/sdk/src/protos/control.hh
@@ -93,6 +93,31 @@ class TaskIndex : public JSONBase {
   virtual bool IsNullJSONData() const;
 };
 
+class TaskStdout : public JSONBase {
+ public:
+  void set_id(unsigned int id);
+  unsigned int id();
+  unsigned int *mutable_id();
+
+  void set_done(bool done);
+  bool done();
+  bool *mutable_done();
+
+  void set_stdout(std::string stdout);
+  std::string stdout();
+  std::string *mutable_stdout();
+
+ protected:
+  unsigned int id_;
+  bool done_;
+  std::string stdout_;
+ public:
+  virtual bool Deserialize(const rapidjson::Value &obj);
+  virtual bool Serialize(
+      rapidjson::Writer<rapidjson::StringBuffer> *writer) const;
+  virtual bool IsNullJSONData() const;  
+};
+
 class Task : public JSONBase {
  public:
   void set_id(unsigned int id);

diff --git a/sdk/src/robot.cc b/sdk/src/robot.cc
@@ -1001,6 +1001,12 @@ std::vector<unsigned int> Robot::load_task_list() {
   control::TaskIds resp = impl_->loadTaskList();
   return resp.ids();
 }
+bool Robot::wait_task(unsigned int id) {
+  control::TaskIndex task_index;
+  task_index.set_id(id);
+  control::TaskStdout resp = impl_->waitTask(task_index);
+  return resp.done();
+}
 void Robot::pause_task(unsigned int id) {
   control::PauseRequest req;
   req.set_id(id);

diff --git a/sdk/src/robot_impl.cc b/sdk/src/robot_impl.cc
@@ -346,6 +346,14 @@ control::TaskIds Robot::RobotImpl::loadTaskList() {
   return list_resp;
 }
 
+control::TaskStdout Robot::RobotImpl::waitTask(const control::TaskIndex &req) {
+  std::string resp;
+  json_rpc_connector_->CallRpc("wait_task", req.ToJSONString(), &resp);
+  control::TaskStdout wait_task_resp;
+  wait_task_resp.FromJSONString(resp);
+  return wait_task_resp;
+}
+
 void Robot::RobotImpl::pauseTask(const control::PauseRequest &req) {
   json_rpc_connector_->CallRpc("pause_task", req.ToJSONString(), nullptr);
 }

diff --git a/sdk/src/robot_impl.hh b/sdk/src/robot_impl.hh
@@ -89,6 +89,7 @@ class Robot::RobotImpl {
   void addSignal(const signal::SetSignalRequest &req);
   control::TaskIndex scene(const control::StartTaskRequest &req);
   control::TaskIds loadTaskList();
+  control::TaskStdout waitTask(const control::TaskIndex &req);
   void pauseTask(const control::PauseRequest &req);
   void resumeTask(const control::TaskIndex &req);
   void cancelTask(const control::TaskIndex &req);