From ab4bbfac50886805b482c9687317c8050ac11136 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 5 Aug 2026 11:11:40 +0800 Subject: [PATCH] fix(aubo): handle duplicate motion targets --- cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 13 ++++ cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 56 ++++++++++++--- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 4 -- .../devices/arm/aubo_arm/aubo_motion_result.h | 34 +++++++++ .../tests/aubo_arm_motion_result_test.cpp | 70 +++++++++++++++++++ 5 files changed, 165 insertions(+), 12 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h create mode 100644 cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index cd0e47ed..a3351b74 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -54,6 +54,19 @@ install(TARGETS aubo_arm LIBRARY DESTINATION lib) if(BUILD_TESTING) enable_testing() + add_executable(aubo_arm_motion_result_test + tests/aubo_arm_motion_result_test.cpp + ) + target_include_directories(aubo_arm_motion_result_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME aubo_arm_motion_result_test + COMMAND aubo_arm_motion_result_test + ) + set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10) + add_executable(aubo_arm_json_command_test tests/aubo_arm_json_command_test.cpp ) diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 9a3e2b19..00261397 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -1,5 +1,7 @@ #include "devices/arm/aubo_arm/aubo_arm.h" +#include "devices/arm/aubo_arm/aubo_motion_result.h" + #include #include #include @@ -628,17 +630,36 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } - robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); - robot_interface->getMotionControl()->moveJoint( + auto motion_control = robot_interface->getMotionControl(); + motion_control->setSpeedFraction(speed_scaling_); + const int ret = motion_control->moveJoint( target.position, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.5, options.blend_radius, 0); - if (waitArrival(robot_interface) != 0) { + const auto outcome = aubo_internal::resolveMotionCommand( + ret, + arcs::common_interface::AUBO_OK, + arcs::common_interface::AUBO_REQUEST_IGNORE, + [&robot_interface]() { return waitArrival(robot_interface); }); + switch (outcome) { + case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: + CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret=" + << ret << " (" + << arcs::common_interface::returnValue2Str(ret) << ")"; + return Result::success(); + case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: + return Result::success(); + case aubo_internal::MotionCommandOutcome::SubmitFailed: + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] moveJ failed: sdk ret=" + std::to_string(ret) + + " (" + arcs::common_interface::returnValue2Str(ret) + ")"); + case aubo_internal::MotionCommandOutcome::CompletionFailed: return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ did not complete"); } - return Result::success(); + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ failed: unknown outcome"); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what()); } @@ -728,20 +749,39 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } - robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + auto motion_control = robot_interface->getMotionControl(); + motion_control->setSpeedFraction(speed_scaling_); std::vector tcp_offset(6, 0.0); robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; - robot_interface->getMotionControl()->moveLine( + const int ret = motion_control->moveLine( pose, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.25, options.blend_radius, 0); - if (waitArrival(robot_interface) != 0) { + const auto outcome = aubo_internal::resolveMotionCommand( + ret, + arcs::common_interface::AUBO_OK, + arcs::common_interface::AUBO_REQUEST_IGNORE, + [&robot_interface]() { return waitArrival(robot_interface); }); + switch (outcome) { + case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: + CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret=" + << ret << " (" + << arcs::common_interface::returnValue2Str(ret) << ")"; + return Result::success(); + case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: + return Result::success(); + case aubo_internal::MotionCommandOutcome::SubmitFailed: + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] moveL failed: sdk ret=" + std::to_string(ret) + + " (" + arcs::common_interface::returnValue2Str(ret) + ")"); + case aubo_internal::MotionCommandOutcome::CompletionFailed: return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL did not complete"); } - return Result::success(); + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL failed: unknown outcome"); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what()); } diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 9c8a0725..28a19663 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -87,9 +87,7 @@ private: bool validDof_(std::size_t size, std::string& error) const; Result ensureConnected_(const std::string& context) const; -#if defined(CMVR_HAS_AUBO_SDK) struct SdkState; -#endif private: config::RobotArmConfig cfg_; @@ -107,9 +105,7 @@ private: bool emergency_stopped_{false}; mutable std::mutex mutex_; -#if defined(CMVR_HAS_AUBO_SDK) std::unique_ptr sdk_; -#endif }; } // namespace cmvr::device diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h new file mode 100644 index 00000000..d2cba859 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h @@ -0,0 +1,34 @@ +#ifndef CMVR_ES_AUBO_MOTION_RESULT_H +#define CMVR_ES_AUBO_MOTION_RESULT_H + +namespace cmvr::device::aubo_internal { + +enum class MotionCommandOutcome { + CompletedWithoutMotion, + CompletedAfterMotion, + SubmitFailed, + CompletionFailed, +}; + +template +MotionCommandOutcome resolveMotionCommand( + const int return_code, + const int success_code, + const int request_ignore_code, + WaitForCompletion&& wait_for_completion) +{ + if (return_code == request_ignore_code) { + return MotionCommandOutcome::CompletedWithoutMotion; + } + if (return_code != success_code) { + return MotionCommandOutcome::SubmitFailed; + } + if (wait_for_completion() != 0) { + return MotionCommandOutcome::CompletionFailed; + } + return MotionCommandOutcome::CompletedAfterMotion; +} + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_MOTION_RESULT_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp new file mode 100644 index 00000000..da2c16ee --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp @@ -0,0 +1,70 @@ +#include "devices/arm/aubo_arm/aubo_motion_result.h" + +#include + +namespace { + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + using cmvr::device::aubo_internal::MotionCommandOutcome; + using cmvr::device::aubo_internal::resolveMotionCommand; + + constexpr int success_code = 0; + constexpr int request_ignore_code = 13; + + int wait_calls = 0; + const auto wait_succeeded = [&wait_calls]() { + ++wait_calls; + return 0; + }; + CHECK_TRUE(resolveMotionCommand( + success_code, + success_code, + request_ignore_code, + wait_succeeded) == MotionCommandOutcome::CompletedAfterMotion); + CHECK_TRUE(wait_calls == 1); + + wait_calls = 0; + CHECK_TRUE(resolveMotionCommand( + request_ignore_code, + success_code, + request_ignore_code, + wait_succeeded) == MotionCommandOutcome::CompletedWithoutMotion); + CHECK_TRUE(wait_calls == 0); + + const int submit_failures[] = {1, 2, 3, -request_ignore_code}; + for (const int return_code : submit_failures) { + wait_calls = 0; + CHECK_TRUE(resolveMotionCommand( + return_code, + success_code, + request_ignore_code, + wait_succeeded) == MotionCommandOutcome::SubmitFailed); + CHECK_TRUE(wait_calls == 0); + } + + wait_calls = 0; + const auto wait_failed = [&wait_calls]() { + ++wait_calls; + return -1; + }; + CHECK_TRUE(resolveMotionCommand( + success_code, + success_code, + request_ignore_code, + wait_failed) == MotionCommandOutcome::CompletionFailed); + CHECK_TRUE(wait_calls == 1); + + return 0; +}