From 39eb1d664533a359f4174da8d2cc637c2c7b0979 Mon Sep 17 00:00:00 2001 From: Cosmoslip <37982167+Cosmoflips@users.noreply.github.com> Date: Thu, 2 Mar 2023 14:03:34 +0800 Subject: [PATCH] feat:Dio and Hook (#32) * fix:bugfix * fix:bugfix * Update build.yml * Update build.yml * Update build.yml * Update build.yml * Update build.yml * Update build.yml * Update build.yml * Update build.yml * Update build.yml * Update build.yml * update ci * feat:Dio features for arm * fix:bugfix * fix:bugfix * fix:bugfix * fix:bugfix * fix:bugfix * add python-setup in windows_python_build * merge master from liufang-robot:master * Update windows_python_build.yml * Update windows_python_build.yml * Update windows_python_build.yml * Update windows_python_build.yml * fix:aach64 cpp build * Update linux_aarch64_cpp_build.yml * Update linux_aarch64_cpp_build.yml * Update linux_aarch64_cpp_build.yml * Update linux_aarch64_cpp_build.yml * Update linux_aarch64_cpp_build.yml * Delete linux_aarch64_cpp_build.yml * Update linux_python_build.yml * Update linux_python_build.yml * fix:Cartesian rx rz wrong swap * fix:bugfix * chore:dio fields change * Update linux_python_build.yml [skip ci] * fix:bugfix * fix:bugfix * fix:bugfix * feat:hook exec * fix:bugfix * fix:get_task_state undefine * Update robot.cc * Update robot.hh * Update CMakeLists.txt * Update Doxyfile --------- Co-authored-by: liufang --- .github/workflows/linux_python_build.yml | 3 + .github/workflows/windows_python_build.yml | 9 +- CMakeLists.txt | 2 +- Doxyfile | 2 +- sdk/include/lebai/robot.hh | 34 ++-- sdk/src/protos/control.cc | 85 ++++++++ sdk/src/protos/control.hh | 35 ++++ sdk/src/protos/io.cc | 91 +++++++++ sdk/src/protos/io.hh | 15 ++ sdk/src/robot.cc | 219 +++++++-------------- sdk/src/robot_impl.cc | 50 +++-- sdk/src/robot_impl.hh | 6 +- 12 files changed, 353 insertions(+), 198 deletions(-) mode change 100644 => 100755 CMakeLists.txt diff --git a/.github/workflows/linux_python_build.yml b/.github/workflows/linux_python_build.yml index ac0fc55..8b70fd0 100644 --- a/.github/workflows/linux_python_build.yml +++ b/.github/workflows/linux_python_build.yml @@ -3,6 +3,9 @@ run-name: ${{ github.actor }} Linux python build on: workflow_dispatch: pull_request: + push: + branches: + - master jobs: linux_python_build: runs-on: ubuntu-latest diff --git a/.github/workflows/windows_python_build.yml b/.github/workflows/windows_python_build.yml index a2dc0bd..424ff9e 100644 --- a/.github/workflows/windows_python_build.yml +++ b/.github/workflows/windows_python_build.yml @@ -21,7 +21,14 @@ jobs: python-version: '3.10' - name: Check cmake run: cmake --version + - run: pip install python-dev-tools +# - uses: msys2/setup-msys2@v2 +# with: +# update: true +# install: >- +# base-devel +# mingw-w64-x86_64-python3 - name: Configure run: cmake -S. -Bbuild -G "Visual Studio 17 2022" -DBUILD_PYTHON=ON -DBUILD_DOCUMENTATION=OFF - name: Build - run: cmake --build build \ No newline at end of file + run: cmake --build build diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100644 new mode 100755 index 7150d56..f25b8db --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,7 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") option(CMAKE_EXPORT_COMPILE_COMMANDS "Export compile command" TRUE) -project(lebai VERSION 1.0.17 LANGUAGES CXX) +project(lebai VERSION 1.0.18 LANGUAGES CXX) set(PROJECT_NAMESPACE lebai) message(STATUS "${PROJECT_NAME} version: ${PROJECT_VERSION}") # message(STATUS "major: ${PROJECT_VERSION_MAJOR}") diff --git a/Doxyfile b/Doxyfile index 594036a..0f8c23c 100644 --- 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.0.17 +PROJECT_NUMBER = 1.0.18 # 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/sdk/include/lebai/robot.hh b/sdk/include/lebai/robot.hh index d3584db..32da580 100644 --- a/sdk/include/lebai/robot.hh +++ b/sdk/include/lebai/robot.hh @@ -401,6 +401,12 @@ namespace lebai * @note 查看 具体信息. */ int get_robot_mode(); + /** + * @brief 是否已与手臂断开连接 + * + * @return 是否已断开连接 + */ + bool is_disconnected(); /** * @brief 获取机械臂关节当前反馈位置 * @@ -550,34 +556,20 @@ namespace lebai * @return 返回多个模拟输入数值 */ std::vector get_ais(std::string device, unsigned int pin, unsigned int num); - /** - * @brief 设置数字端口输出值 - * @param pin: 端口号，从 0 开始 - * @param value: 待设置的模拟输出值 - * @return 若设置成功为true,若当前此端口为输入模式则为false - */ - bool set_dio(unsigned int pin, bool value); /** * @brief 设置数字端口模式 * @param pin 端口号，从 0 开始 * @param value 设置的值，false为输入模式，true为输出模式 * @return 返回是否成功 */ - bool set_dio_mode(unsigned int pin, bool value); - /** - * @brief 获取数字端口输出值 - * @param pin: 端口号，从 0 开始 - * @param count: 查询的连续端口数 - * @return 从pin开始的连续count个端口的当前值 - */ - std::vector get_dios(unsigned int pin, unsigned int count); + void set_dio_mode(std::string device,unsigned int pin, bool value); /** * @brief 获取数字端口模式 * @param pin 端口号，从 0 开始 * @param count 查询的连续端口数 * @return 从pin开始的连续count个端口的当前模式 */ - std::vector get_dios_mode(unsigned int pin, unsigned int count); + std::vector get_dios_mode(std::string device,unsigned int pin, unsigned int count); /** @}*/ @@ -707,6 +699,16 @@ namespace lebai * @param id 任务的ID. */ void cancel_task(unsigned int id); + /** + * @brief 根据已设置的Hook执行对应场景 + * + * @param id 任务的ID. + */ + unsigned int exec_hook(unsigned int id); + /** + * @brief 获取任务状态. + */ + std::string get_task_state(); /** * @brief 获取任务状态. * diff --git a/sdk/src/protos/control.cc b/sdk/src/protos/control.cc index 3970ade..01ac3d1 100644 --- a/sdk/src/protos/control.cc +++ b/sdk/src/protos/control.cc @@ -665,5 +665,90 @@ namespace lebai { return false; } + + void Exec::set_id(unsigned int id) + { + id_ = id; + } + unsigned int Exec::id() + { + return id_; + } + unsigned int *Exec::mutable_id() + { + return &id_; + } + bool Exec::Deserialize(const rapidjson::Value &obj) + { + if(obj.HasMember("id")) + { + id_ = (unsigned int)(obj["id"].GetUint()); + } + return true; + } + bool Exec::Serialize(rapidjson::Writer *writer) const + { + writer->StartObject(); + writer->Key("id"); + writer->Uint(id_); + writer->EndObject(); + return true; + } + bool Exec::IsNullJSONData() const + { + return false; + } + + void HookResponse::set_success(unsigned int success) + { + success_ = success; + } + unsigned int HookResponse::success() + { + return success_; + } + unsigned int *HookResponse::mutable_success() + { + return &success_; + } + + void HookResponse::set_error(std::string error) + { + error_ = error; + } + std::string HookResponse::error() + { + return error_; + } + std::string *HookResponse::mutable_error() + { + return &error_; + } + bool HookResponse::Deserialize(const rapidjson::Value &obj) + { + if(obj.HasMember("success")) + { + success_ = bool(obj["success"].GetBool()); + } + if(obj.HasMember("error")) + { + error_ = (std::string)(obj["error"].GetString()); + } + return true; + } + bool HookResponse::Serialize(rapidjson::Writer *writer) const + { + writer->StartObject(); + writer->Key("success"); + writer->Bool(success_); + writer->Key("error"); + writer->String(error_.c_str()); + writer->EndObject(); + return true; + } + bool HookResponse::IsNullJSONData() const + { + return false; + } } } \ No newline at end of file diff --git a/sdk/src/protos/control.hh b/sdk/src/protos/control.hh index e91e917..feab62a 100644 --- a/sdk/src/protos/control.hh +++ b/sdk/src/protos/control.hh @@ -211,5 +211,40 @@ namespace lebai virtual bool Serialize(rapidjson::Writer *writer) const; virtual bool IsNullJSONData() const; }; + + class Exec : public JSONBase + { + public: + void set_id(unsigned int id); + unsigned int id(); + unsigned int * mutable_id(); + + protected: + unsigned int id_; + public: + virtual bool Deserialize(const rapidjson::Value &obj); + virtual bool Serialize(rapidjson::Writer *writer) const; + virtual bool IsNullJSONData() const; + }; + + class HookResponse : public JSONBase + { + public: + void set_success(unsigned int success); + unsigned int success(); + unsigned int * mutable_success(); + + void set_error(std::string error); + std::string error(); + std::string * mutable_error(); + + protected: + unsigned int success_; + std::string error_; + public: + virtual bool Deserialize(const rapidjson::Value &obj); + virtual bool Serialize(rapidjson::Writer *writer) const; + virtual bool IsNullJSONData() const; + }; } } \ No newline at end of file diff --git a/sdk/src/protos/io.cc b/sdk/src/protos/io.cc index 0c2bb5e..8419da4 100644 --- a/sdk/src/protos/io.cc +++ b/sdk/src/protos/io.cc @@ -827,6 +827,19 @@ namespace lebai return false; } + void SetDioModeRequest::set_device(IoDevice device) + { + device_ = device; + } + IoDevice SetDioModeRequest::device() + { + return device_; + } + IoDevice* SetDioModeRequest::mutable_device() + { + return &device_; + } + void SetDioModeRequest::set_pin(unsigned int pin) { pin_ = pin; @@ -855,6 +868,22 @@ namespace lebai bool SetDioModeRequest::Deserialize(const rapidjson::Value &obj) { + if(obj.HasMember("device")) + { + std::string device_str = std::string(obj["device"].GetString()); + if(device_str == "ROBOT") + { + device_ = ROBOT; + } + else if(device_str == "FLANGE") + { + device_ = FLANGE; + } + else if(device_str == "EXTRA") + { + device_ = EXTRA; + } + } if(obj.HasMember("pin")) { pin_ = obj["pin"].GetUint(); @@ -868,6 +897,8 @@ namespace lebai bool SetDioModeRequest::Serialize(rapidjson::Writer *writer) const { writer->StartObject(); + writer->Key("device"); + writer->Int(device_); writer->Key("pin"); writer->Uint(pin_); writer->Key("value"); @@ -913,6 +944,18 @@ namespace lebai return false; } + void SetDioRequest::set_device(IoDevice device) + { + device_ = device; + } + IoDevice SetDioRequest::device() + { + return device_; + } + IoDevice* SetDioRequest::mutable_device() + { + return &device_; + } void SetDioRequest::set_pin(unsigned int pin) { pin_ = pin; @@ -941,6 +984,22 @@ namespace lebai bool SetDioRequest::Deserialize(const rapidjson::Value &obj) { + if(obj.HasMember("device")) + { + std::string device_str = std::string(obj["device"].GetString()); + if(device_str == "ROBOT") + { + device_ = ROBOT; + } + else if(device_str == "FLANGE") + { + device_ = FLANGE; + } + else if(device_str == "EXTRA") + { + device_ = EXTRA; + } + } if(obj.HasMember("pin")) { pin_ = obj["pin"].GetUint(); @@ -954,6 +1013,8 @@ namespace lebai bool SetDioRequest::Serialize(rapidjson::Writer *writer) const { writer->StartObject(); + writer->Key("device"); + writer->Int(device_); writer->Key("pin"); writer->Uint(pin_); writer->Key("value"); @@ -1095,6 +1156,18 @@ namespace lebai return false; } + void GetDiosModeRequest::set_device(IoDevice device) + { + device_ = device; + } + IoDevice GetDiosModeRequest::device() + { + return device_; + } + IoDevice* GetDiosModeRequest::mutable_device() + { + return &device_; + } void GetDiosModeRequest::set_pin(unsigned int pin) { pin_ = pin; @@ -1123,6 +1196,22 @@ namespace lebai bool GetDiosModeRequest::Deserialize(const rapidjson::Value &obj) { + if(obj.HasMember("device")) + { + std::string device_str = std::string(obj["device"].GetString()); + if(device_str == "ROBOT") + { + device_ = ROBOT; + } + else if(device_str == "FLANGE") + { + device_ = FLANGE; + } + else if(device_str == "EXTRA") + { + device_ = EXTRA; + } + } if(obj.HasMember("pin")) { pin_ = obj["pin"].GetUint(); @@ -1136,6 +1225,8 @@ namespace lebai bool GetDiosModeRequest::Serialize(rapidjson::Writer *writer) const { writer->StartObject(); + writer->Key("device"); + writer->Int(device_); writer->Key("pin"); writer->Uint(pin_); writer->Key("count"); diff --git a/sdk/src/protos/io.hh b/sdk/src/protos/io.hh index 6c9d8e1..7b35124 100644 --- a/sdk/src/protos/io.hh +++ b/sdk/src/protos/io.hh @@ -301,6 +301,10 @@ namespace lebai class SetDioModeRequest : public JSONBase { public: + void set_device(IoDevice device); + IoDevice device(); + IoDevice* mutable_device(); + void set_pin(unsigned int pin); unsigned int pin() const; unsigned int *mutable_pin(); @@ -310,6 +314,7 @@ namespace lebai bool *mutable_value(); protected: + IoDevice device_; unsigned int pin_; bool value_; // These methods are used to serialize and deserialize the class. @@ -340,6 +345,10 @@ namespace lebai class SetDioRequest : public JSONBase { public: + void set_device(IoDevice device); + IoDevice device(); + IoDevice* mutable_device(); + void set_pin(unsigned int pin); unsigned int pin() const; unsigned int *mutable_pin(); @@ -349,6 +358,7 @@ namespace lebai bool *mutable_value(); protected: + IoDevice device_; unsigned int pin_; bool value_; // These methods are used to serialize and deserialize the class. @@ -416,6 +426,10 @@ namespace lebai class GetDiosModeRequest : public JSONBase { public: + void set_device(IoDevice device); + IoDevice device(); + IoDevice* mutable_device(); + void set_pin(unsigned int pin); unsigned int pin() const; unsigned int *mutable_pin(); @@ -424,6 +438,7 @@ namespace lebai unsigned int count() const; unsigned int *mutable_count(); protected: + IoDevice device_; unsigned int pin_; unsigned int count_; // These methods are used to serialize and deserialize the class. diff --git a/sdk/src/robot.cc b/sdk/src/robot.cc index 61bf6ec..4c45e17 100644 --- a/sdk/src/robot.cc +++ b/sdk/src/robot.cc @@ -448,6 +448,11 @@ int Robot::get_robot_mode() { return impl_->getRobotState(); } + +bool Robot::is_disconnected() +{ + return impl_->getRobotState() == 0; +} std::vector Robot::get_actual_joint_positions() { @@ -478,9 +483,9 @@ CartesianPose Robot::get_actual_tcp_pose() cart_pose["z"] = pose.position().z(); if(pose.rotation().euler_zyx()) { - cart_pose["rx"] = pose.rotation().euler_zyx()->x(); - cart_pose["ry"] = pose.rotation().euler_zyx()->y(); cart_pose["rz"] = pose.rotation().euler_zyx()->z(); + cart_pose["ry"] = pose.rotation().euler_zyx()->y(); + cart_pose["rx"] = pose.rotation().euler_zyx()->x(); } return cart_pose; } @@ -493,9 +498,9 @@ CartesianPose Robot::get_target_tcp_pose() cart_pose["z"] = pose.position().z(); if(pose.rotation().euler_zyx()) { - cart_pose["rx"] = pose.rotation().euler_zyx()->x(); - cart_pose["ry"] = pose.rotation().euler_zyx()->y(); cart_pose["rz"] = pose.rotation().euler_zyx()->z(); + cart_pose["ry"] = pose.rotation().euler_zyx()->y(); + cart_pose["rx"] = pose.rotation().euler_zyx()->x(); } return cart_pose; } @@ -730,155 +735,46 @@ std::vector Robot::get_ais(std::string device, unsigned int pin, unsigne io::GetAioPinsResponse resp = impl_->getAIS(req); return resp.values(); } -bool Robot::set_dio(unsigned int pin, bool value) -{ - io::SetDioRequest req; - req.set_pin(pin); - req.set_value(value); - io::SetDioResponse resp = impl_->setDio(req); - return resp.success(); -} -bool Robot::set_dio_mode(unsigned int pin, bool value) +void Robot::set_dio_mode(std::string device,unsigned int pin, bool value) { io::SetDioModeRequest req; + if(device == "ROBOT") + { + req.set_device(io::IoDevice::ROBOT); + } + else if(device == "FLANGE") + { + req.set_device(io::IoDevice::FLANGE); + } + else if(device == "EXTRA") + { + req.set_device(io::IoDevice::EXTRA); + } req.set_pin(pin); req.set_value(value); - io::SetDioModeResponse resp = impl_->setDioMode(req); - return resp.success(); -} -std::vector Robot::get_dios(unsigned int pin, unsigned int count) -{ - io::GetDiosRequest req; - req.set_pin(pin); - req.set_count(count); - io::GetDiosResponse resp = impl_->getDios(req); - return resp.values(); + impl_->setDioMode(req); } -std::vector Robot::get_dios_mode(unsigned int pin, unsigned int count) +std::vector Robot::get_dios_mode(std::string device,unsigned int pin, unsigned int count) { io::GetDiosModeRequest req; + if(device == "ROBOT") + { + req.set_device(io::IoDevice::ROBOT); + } + else if(device == "FLANGE") + { + req.set_device(io::IoDevice::FLANGE); + } + else if(device == "EXTRA") + { + req.set_device(io::IoDevice::EXTRA); + } req.set_pin(pin); req.set_count(count); io::GetDiosModeResponse resp = impl_->getDiosMode(req); return resp.values(); } -// bool Robot::get_robot_di(unsigned int pin) -// { -// io::GetDioPinRequest req; -// req.set_pin(pin); -// req.set_device(io::IoDevice::ROBOT); -// auto resp = impl_->getDI(req); -// return resp.value() ? true : false; -// } - -// void Robot::set_robot_do(unsigned int pin, bool value) -// { -// io::SetDoPinRequest req; -// req.set_pin(pin); -// req.set_value(value); -// req.set_device(io::IoDevice::ROBOT); -// impl_->setDO(req); -// } - -// void Robot::set_robot_do(unsigned int pin, unsigned int value) -// { -// io::SetDoPinRequest req; -// req.set_pin(pin); -// req.set_value(value); -// req.set_device(io::IoDevice::ROBOT); -// impl_->setDO(req); -// } - -// double Robot::get_robot_ai(unsigned int pin) -// { -// io::GetAioPinRequest req; -// req.set_pin(pin); -// req.set_device(io::IoDevice::ROBOT); -// auto resp = impl_->getAI(req); -// return resp.value(); -// } - -// void Robot::set_robot_ao(unsigned int pin, double value) -// { -// io::SetAoPinRequest req; -// req.set_pin(pin); -// req.set_value(value); -// req.set_device(io::IoDevice::ROBOT); -// impl_->setAO(req); -// } - - -// bool Robot::get_flange_di(unsigned int pin) -// { -// io::GetDioPinRequest req; -// req.set_pin(pin); -// req.set_device(io::IoDevice::FLANGE); -// auto resp = impl_->getDI(req); -// return resp.value() ? true : false; -// } - -// void Robot::set_flange_do(unsigned int pin, bool value) -// { -// io::SetDoPinRequest req; -// req.set_pin(pin); -// req.set_value(value); -// req.set_device(io::IoDevice::FLANGE); -// impl_->setDO(req); -// } -// void Robot::set_flange_do(unsigned int pin, unsigned int value) -// { -// io::SetDoPinRequest req; -// req.set_pin(pin); -// req.set_value(value); -// req.set_device(io::IoDevice::FLANGE); -// impl_->setDO(req); -// } - -// bool Robot::get_extra_di(unsigned int pin) -// { -// io::GetDioPinRequest req; -// req.set_pin(pin); -// req.set_device(io::IoDevice::EXTRA); -// auto resp = impl_->getDI(req); -// return resp.value() ? true : false; -// } - -// void Robot::set_extra_do(unsigned int pin, bool value) -// { -// io::SetDoPinRequest req; -// req.set_pin(pin); -// req.set_value(value); -// req.set_device(io::IoDevice::EXTRA); -// impl_->setDO(req); -// } -// void Robot::set_extra_do(unsigned int pin, unsigned int value) -// { -// io::SetDoPinRequest req; -// req.set_pin(pin); -// req.set_value(value); -// req.set_device(io::IoDevice::EXTRA); -// impl_->setDO(req); -// } - -// double Robot::get_extra_ai(unsigned int pin) -// { -// io::GetAioPinRequest req; -// req.set_pin(pin); -// req.set_device(io::IoDevice::EXTRA); -// auto resp = impl_->getAI(req); -// return resp.value(); -// } - -// void Robot::set_extra_ao(unsigned int pin, double value) -// { -// io::SetAoPinRequest req; -// req.set_pin(pin); -// req.set_value(value); -// req.set_device(io::IoDevice::EXTRA); -// impl_->setAO(req); -// } - void Robot::set_claw(double force, double amplitude) { claw::SetClawRequest req; @@ -1066,6 +962,17 @@ void Robot::cancel_task(unsigned int id) req.set_id(id); impl_->cancelTask(req); } +unsigned int Robot::exec_hook(unsigned int id) +{ + control::Exec req; + req.set_id(id); + control::HookResponse resp = impl_->execHook(req); + if(!resp.success()) + { + return 0; + } + return atoi(resp.error().c_str()); +} std::string Robot::get_task_state(unsigned int id) { control::TaskIndex req; @@ -1082,6 +989,20 @@ std::string Robot::get_task_state(unsigned int id) default:return "Undefined State"; } } +std::string Robot::get_task_state() +{ + control::Task resp = impl_->loadTask(); + switch(resp.state()) + { + case control::TaskState::WAIT:return "WAIT";break; + case control::TaskState::RUNNING:return "RUNNING";break; + case control::TaskState::PAUSE:return "PAUSE";break; + case control::TaskState::SUCCESS:return "SUCCESS";break; + case control::TaskState::INTERRUPT:return "INTERRUPT";break; + case control::TaskState::FAIL:return "FAIL";break; + default:return "Undefined State"; + } +} KinematicsForwardResp Robot::kinematics_forward(const std::vector & joint_positions) @@ -1096,9 +1017,9 @@ KinematicsForwardResp Robot::kinematics_forward(const std::vector & join kf_resp.pose["x"] = resp.position().x(); kf_resp.pose["y"] = resp.position().y(); kf_resp.pose["z"] = resp.position().z(); - kf_resp.pose["rz"] = resp.rotation().euler_zyx()->x(); + kf_resp.pose["rz"] = resp.rotation().euler_zyx()->z(); kf_resp.pose["ry"] = resp.rotation().euler_zyx()->y(); - kf_resp.pose["rx"] = resp.rotation().euler_zyx()->z(); + kf_resp.pose["rx"] = resp.rotation().euler_zyx()->x(); kf_resp.ok = true; return kf_resp; } @@ -1140,9 +1061,9 @@ KinematicsInverseResp Robot::kinematics_inverse(const CartesianPose & pose, cons req.mutable_pose()->mutable_cart()->mutable_position()->set_x(x); req.mutable_pose()->mutable_cart()->mutable_position()->set_y(y); req.mutable_pose()->mutable_cart()->mutable_position()->set_z(z); - req.mutable_pose()->mutable_cart()->mutable_rotation()->mutable_euler_zyx()->set_x(rz); + req.mutable_pose()->mutable_cart()->mutable_rotation()->mutable_euler_zyx()->set_z(rz); req.mutable_pose()->mutable_cart()->mutable_rotation()->mutable_euler_zyx()->set_y(ry); - req.mutable_pose()->mutable_cart()->mutable_rotation()->mutable_euler_zyx()->set_z(rx); + req.mutable_pose()->mutable_cart()->mutable_rotation()->mutable_euler_zyx()->set_x(rx); for(auto && p: joint_init_positions) { @@ -1428,9 +1349,9 @@ void Robot::set_tcp(std::array tcp) pos.set_z(tcp[2]); req.set_position(pos); posture::Position p; - p.set_x(tcp[3]); + p.set_z(tcp[3]); p.set_y(tcp[4]); - p.set_z(tcp[5]); + p.set_x(tcp[5]); rot.set_euler_zyx(p); req.set_rotation(rot); impl_->setTcp(req); @@ -1442,9 +1363,9 @@ std::array Robot::get_tcp() ret[0] = resp.position().x(); ret[1] = resp.position().y(); ret[2] = resp.position().z(); - ret[3] = resp.rotation().euler_zyx()->x(); + ret[3] = resp.rotation().euler_zyx()->z(); ret[4] = resp.rotation().euler_zyx()->y(); - ret[5] = resp.rotation().euler_zyx()->z(); + ret[5] = resp.rotation().euler_zyx()->x(); return ret; } @@ -1550,4 +1471,4 @@ std::vector Robot::read_input_registers(std::string device, std::s } } -} // namespace l_master_sdk \ No newline at end of file +} // namespace l_master_sdk diff --git a/sdk/src/robot_impl.cc b/sdk/src/robot_impl.cc index 2c5fa73..bb04fc0 100644 --- a/sdk/src/robot_impl.cc +++ b/sdk/src/robot_impl.cc @@ -271,29 +271,9 @@ namespace lebai get_aio_resp.FromJSONString(resp); return get_aio_resp; } - io::SetDioResponse Robot::RobotImpl::setDio(const io::SetDioRequest &req) + void Robot::RobotImpl::setDioMode(const io::SetDioModeRequest &req) { - std::string resp; - json_rpc_connector_->CallRpc("set_dio", req.ToJSONString(), &resp); - io::SetDioResponse resp_; - resp_.FromJSONString(resp); - return resp_; - } - io::SetDioModeResponse Robot::RobotImpl::setDioMode(const io::SetDioModeRequest &req) - { - std::string resp; - json_rpc_connector_->CallRpc("set_dio_mode", req.ToJSONString(), &resp); - io::SetDioModeResponse resp_; - resp_.FromJSONString(resp); - return resp_; - } - io::GetDiosResponse Robot::RobotImpl::getDios(const io::GetDiosRequest &req) - { - std::string resp; - json_rpc_connector_->CallRpc("get_dios", req.ToJSONString(), &resp); - io::GetDiosResponse resp_; - resp_.FromJSONString(resp); - return resp_; + json_rpc_connector_->CallRpc("set_dio_mode", req.ToJSONString(),nullptr); } io::GetDiosModeResponse Robot::RobotImpl::getDiosMode(const io::GetDiosModeRequest &req) { @@ -386,13 +366,29 @@ namespace lebai { json_rpc_connector_->CallRpc("cancel_task",req.ToJSONString(),nullptr); } + control::HookResponse Robot::RobotImpl::execHook(const control::Exec &req) + { + std::string resp; + json_rpc_connector_->CallRpc("exec_hook",req.ToJSONString(),&resp); + control::HookResponse hook_resp; + hook_resp.FromJSONString(resp); + return hook_resp; + } control::Task Robot::RobotImpl::loadTask(const control::TaskIndex & req) { - std::string resp_str; - json_rpc_connector_->CallRpc("load_task",req.ToJSONString(),nullptr); - control::Task resp; - resp.FromJSONString(resp_str); - return resp; + std::string resp; + json_rpc_connector_->CallRpc("load_task",req.ToJSONString(),&resp); + control::Task task_resp; + task_resp.FromJSONString(resp); + return task_resp; + } + control::Task Robot::RobotImpl::loadTask() + { + std::string resp; + json_rpc_connector_->CallRpc("load_task","{}",&resp); + control::Task task_resp; + task_resp.FromJSONString(resp); + return task_resp; } posture::CartesianPose Robot::RobotImpl::getForwardKin(const posture::PoseRequest & req) diff --git a/sdk/src/robot_impl.hh b/sdk/src/robot_impl.hh index 760c71d..c0ed232 100644 --- a/sdk/src/robot_impl.hh +++ b/sdk/src/robot_impl.hh @@ -75,9 +75,7 @@ namespace lebai io::GetAioPinsResponse getAIS(const io::GetAioPinsRequest & req); io::GetAioPinResponse getAO(const io::GetAioPinRequest & req); io::GetAioPinsResponse getAOS(const io::GetAioPinsRequest & req); - io::SetDioResponse setDio(const io::SetDioRequest & req); - io::SetDioModeResponse setDioMode(const io::SetDioModeRequest & req); - io::GetDiosResponse getDios(const io::GetDiosRequest & req); + void setDioMode(const io::SetDioModeRequest & req); io::GetDiosModeResponse getDiosMode(const io::GetDiosModeRequest & req); void setAO(const io::SetAoPinRequest & req); void setClaw(const claw::SetClawRequest & req); @@ -92,7 +90,9 @@ namespace lebai void pauseTask(const control::PauseRequest & req); void resumeTask(const control::TaskIndex & req); void cancelTask(const control::TaskIndex & req); + control::HookResponse execHook(const control::Exec & req); control::Task loadTask(const control::TaskIndex & req); + control::Task loadTask(); claw::Claw getClaw(); posture::CartesianPose getForwardKin(const posture::PoseRequest & req); posture::JointPose getInverseKin(const posture::GetInverseKinRequest & req);