fix(aubo): handle duplicate motion targets

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
     )

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 <algorithm>
 #include <cctype>
 #include <chrono>
@@ -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_);
+        auto motion_control = robot_interface->getMotionControl();
-        robot_interface->getMotionControl()->moveJoint(
+        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<double> tcp_offset(6, 0.0);
         robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
         std::vector<double> 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());
     }

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<SdkState> sdk_;
-#endif
 };
 
 } // namespace cmvr::device

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 <typename WaitForCompletion>
+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

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 <iostream>
+
+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;
+}