SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(aubo): handle duplicate motion targets

Browse Source
This commit is contained in:
xtkuang 2026-08-05 11:11:40 +08:00
parent 459b76db1d
commit ab4bbfac50
5 changed files with 165 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cmvr-es/devices/arm/aubo_arm/CMakeLists.txt
View File

@ -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
)

56
cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp
View File

@ -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_);
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<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());
}

4
cmvr-es/devices/arm/aubo_arm/aubo_arm.h
View File

@ -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

34
cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h Normal file
View File

@ -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

70
cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp Normal file
View File

@ -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;
}