From 564810f6cdd529b035c7c2bb2f9a421bbf28a030 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Thu, 27 Aug 2026 11:06:20 +0800 Subject: [PATCH] fix(build): restore merged AUBO support files --- cmvr-es/common/types/arm/arm_types.h | 20 + cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 4 + cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 52 +-- .../devices/arm/aubo_arm/aubo_motion_result.h | 45 +++ .../devices/arm/aubo_arm/aubo_motion_state.h | 369 ++++++++++++++++++ .../arm/aubo_arm/aubo_torque_on_result.h | 50 +++ cmvr-es/devices/arm/robot_arm.h | 54 ++- ...0_map3d.proto => seer_robokit_map3d.proto} | 2 +- 8 files changed, 571 insertions(+), 25 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h rename protos/rbk/protocol/{src1100_map3d.proto => seer_robokit_map3d.proto} (97%) diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 4728236c..eaa171aa 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -2,6 +2,7 @@ #define CMVR_ES_ARM_TYPES_H #include +#include #include #include @@ -103,6 +104,11 @@ struct JointGroupState { std::vector position; std::vector velocity; std::vector effort; + std::uint64_t sequence{0}; + std::int64_t sample_monotonic_ns{0}; + bool position_valid{false}; + bool velocity_valid{false}; + bool effort_valid{false}; bool validForModel(const RobotModel& model) const { @@ -165,6 +171,7 @@ struct MotionOptions { double jerk{5.0}; std::vector joint_velocity_limits; bool asynchronous{false}; + std::function cancellation_requested; }; struct ServoOptions { @@ -173,6 +180,11 @@ struct ServoOptions { double gain{300.0}; }; +struct TorqueServoOptions { + double period{0.00125}; + std::uint32_t command_watchdog_ms{20}; +}; + enum class RobotMode { Unknown = 0, Disconnected, @@ -205,6 +217,14 @@ enum class ControlMode { Freedrive }; +enum class JointEffortSource { + Unspecified = 0, + MotorEstimate, + JointSensor, + ForceTorqueSensor, + Observer +}; + struct ArmState { double timestamp{0.0}; RobotMode robot_mode{RobotMode::Unknown}; diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index d56d09cb..64d4cca8 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -2,6 +2,8 @@ add_library(aubo_arm SHARED aubo_arm.cpp ) +find_package(Threads REQUIRED) + target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1) @@ -71,6 +73,8 @@ target_link_libraries(aubo_arm cmvr_es::proto PRIVATE glog + jsoncpp + Threads::Threads ) add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm) diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index a4d4dd6f..8665aecc 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -2,6 +2,8 @@ #define CMVR_ES_AUBO_ARM_H #include +#include +#include #include #include #include @@ -21,6 +23,8 @@ public: std::string typeName() const override { return "AuboARM"; } bool init() override; bool stop() override; + bool executeJsonCommand(const std::string& request_json, + std::string& response_json) override; RobotModel getRobotModel() const override { return model_; } std::size_t getDof() const override { return model_.dof; } @@ -28,27 +32,22 @@ public: JointGroupState getJointState() const override; CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; - SafetyMode getSafetyMode() const override { return SafetyMode::Normal; } - ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } + SafetyMode getSafetyMode() const override; + ControlMode getControlMode() const override; + bool supportsActionQueueMotion() const noexcept override { return true; } Result torqueOn() override; + Result torqueOn( + const std::function& cancellation_requested) override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; - Result protectiveStop() override { return emergencyStop(); } - Result recoverProtectiveStop( - const JointTrajectory&, - const MotionOptions&) override - { - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "protective recovery is not implemented for AuboArm"); - } + Result protectiveStop() override; Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } - bool isProtectiveStopped() const override { return false; } - bool isEmergencyStopped() const override { return emergency_stopped_; } - bool isFault() const override { return false; } + bool isProtectiveStopped() const override; + bool isEmergencyStopped() const override; + bool isFault() const override; Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override; Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override; @@ -72,8 +71,8 @@ public: Result powerOff() override { return torqueOff(); } Result brakeRelease() override { return torqueOn(); } Result shutdown() override; - Result clearFault() override { return Result::success(); } - Result unlockProtectiveStop() override { return Result::success(); } + Result clearFault() override; + Result unlockProtectiveStop() override; Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -86,16 +85,27 @@ public: CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; CartesianPose fk(bool is_tcp = true) override; CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; } - bool busy() const override { return busy_.load(); } + bool busy() const override; private: + enum class MotionStopKind { + Automatic, + Joint, + Linear, + }; + Result unsupported_(const std::string& name) const; bool validDof_(std::size_t size, std::string& error) const; Result ensureConnected_(const std::string& context) const; + Result ensureMotionReady_(const std::string& context, + std::uint64_t& safety_epoch) const; + Result completeSafetyRecovery_(const std::string& context, + std::uint64_t expected_safety_epoch); + Result unlockProtectiveStop_( + std::optional expected_safety_epoch); + Result stopMotion_(MotionStopKind kind, double acceleration); -#if defined(CMVR_HAS_AUBO_SDK) struct SdkState; -#endif private: config::RobotArmConfig cfg_; @@ -110,12 +120,10 @@ private: std::atomic connected_{false}; std::atomic busy_{false}; std::atomic servo_mode_{false}; - bool emergency_stopped_{false}; + std::atomic 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..d1c92fb0 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h @@ -0,0 +1,45 @@ +#ifndef CMVR_ES_AUBO_MOTION_RESULT_H +#define CMVR_ES_AUBO_MOTION_RESULT_H + +namespace cmvr::device::aubo_internal { + +enum class MotionCommandOutcome { + CompletedWithoutMotion, + CompletedAfterMotion, + Cancelled, + SubmitFailed, + CompletionFailed, +}; + +enum class MotionWaitResult { + Completed, + Cancelled, + Failed, +}; + +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; + } + const auto wait_result = wait_for_completion(); + if (wait_result == MotionWaitResult::Cancelled) { + return MotionCommandOutcome::Cancelled; + } + if (wait_result != MotionWaitResult::Completed) { + 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/aubo_motion_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h new file mode 100644 index 00000000..84c84367 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h @@ -0,0 +1,369 @@ +#ifndef CMVR_ES_AUBO_MOTION_STATE_H +#define CMVR_ES_AUBO_MOTION_STATE_H + +#include +#include +#include +#include +#include +#include + +namespace cmvr::device::aubo_internal { + +enum class MotionKind { + None, + Joint, + Linear, +}; + +struct MotionToken { + std::uint64_t generation{0}; + MotionKind kind{MotionKind::None}; + + bool valid() const noexcept + { + return generation != 0 && kind != MotionKind::None; + } +}; + +enum class MotionStartStatus { + Started, + Invalid, + Busy, + Stopping, + Blocked, +}; + +struct MotionStartResult { + MotionStartStatus status{MotionStartStatus::Busy}; + MotionToken token; + + bool started() const noexcept + { + return status == MotionStartStatus::Started; + } +}; + +enum class MotionFinishMode { + RestorePrevious, + Clear, + Retain, +}; + +enum class StopStartStatus { + Started, + AlreadyStopping, +}; + +enum class StopWaitStatus { + Completed, + Failed, + Timeout, +}; + +class StopCompletion final { +public: + StopWaitStatus waitFor(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + if (!cv_.wait_for(lock, timeout, [this]() { return completed_; })) { + return StopWaitStatus::Timeout; + } + return succeeded_ + ? StopWaitStatus::Completed + : StopWaitStatus::Failed; + } + +private: + friend class MotionState; + + void finish(const bool succeeded) + { + { + std::lock_guard lock(mutex_); + succeeded_ = succeeded; + completed_ = true; + } + cv_.notify_all(); + } + + std::mutex mutex_; + std::condition_variable cv_; + bool completed_{false}; + bool succeeded_{false}; +}; + +struct StopRequest { + StopStartStatus status{StopStartStatus::AlreadyStopping}; + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; + std::shared_ptr completion; + + bool started() const noexcept + { + return status == StopStartStatus::Started; + } +}; + +struct SafetyCancelResult { + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; +}; + +// Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized +// through the vendor call. Speed calls release the outer mutex before their +// potentially blocking SDK call, so the generation cancellation below also +// closes the stop-vs-speed-submission race. +class MotionState final { +public: + MotionStartResult begin( + const MotionKind kind, + const bool replace_retained_same_kind = false) + { + std::lock_guard lock(mutex_); + if (kind == MotionKind::None) { + return {MotionStartStatus::Invalid, {}}; + } + if (stop_in_progress_) { + return {MotionStartStatus::Stopping, {}}; + } + if (blocked_) { + return {MotionStartStatus::Blocked, {}}; + } + if (owner_active_) { + return {MotionStartStatus::Busy, {}}; + } + if (last_kind_ != MotionKind::None && + (!replace_retained_same_kind || last_kind_ != kind)) { + return {MotionStartStatus::Busy, {}}; + } + + MotionToken token{++next_generation_, kind}; + owner_active_ = true; + active_token_ = token; + previous_kind_ = last_kind_; + return {MotionStartStatus::Started, token}; + } + + void finish( + const MotionToken& token, + const MotionFinishMode mode = MotionFinishMode::RestorePrevious) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + + owner_active_ = false; + active_token_ = {}; + if (!stop_in_progress_ && !blocked_) { + if (mode == MotionFinishMode::Retain) { + last_kind_ = token.kind; + } else if (mode == MotionFinishMode::Clear) { + last_kind_ = MotionKind::None; + } else { + last_kind_ = previous_kind_; + } + } + previous_kind_ = MotionKind::None; + owner_finished_cv_.notify_all(); + } + + void failMotion(const MotionToken& token) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + owner_active_ = false; + active_token_ = {}; + last_kind_ = token.kind; + previous_kind_ = MotionKind::None; + blocked_ = true; + owner_finished_cv_.notify_all(); + } + + StopRequest beginStop( + const MotionKind requested_kind = MotionKind::None) + { + std::lock_guard lock(mutex_); + if (stop_in_progress_) { + return { + StopStartStatus::AlreadyStopping, + MotionKind::None, + {}, + false, + active_stop_completion_}; + } + + auto completion = std::make_shared(); + stop_in_progress_ = true; + active_stop_completion_ = completion; + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + // A successful speedJoint/speedLine call may keep the controller in + // velocity mode after the SDK function returns, even when the target + // velocity is zero and the robot currently reports steady. Retain that + // motion kind until a typed stop has been acknowledged. + const bool tracked_motion = + active.valid() || last_kind_ != MotionKind::None; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + MotionKind kind = MotionKind::None; + if (active.valid()) { + kind = active.kind; + } else if (last_kind_ != MotionKind::None) { + kind = last_kind_; + } else if (requested_kind != MotionKind::None) { + kind = requested_kind; + } else { + kind = last_kind_; + } + if (kind != MotionKind::None) { + last_kind_ = kind; + } + return { + StopStartStatus::Started, + kind, + active, + tracked_motion, + completion}; + } + + SafetyCancelResult cancelActiveForSafety() + { + std::lock_guard lock(mutex_); + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + + const MotionKind kind = active.valid() + ? active.kind + : last_kind_; + if (kind != MotionKind::None) { + last_kind_ = kind; + } + // This block is intentionally independent of stop_in_progress_. The + // monitor may observe the safety event while a software Stop owns the + // stop transaction; either way no new motion may enter. + blocked_ = true; + owner_finished_cv_.notify_all(); + return {kind, active, active.valid() || kind != MotionKind::None}; + } + + bool cancelled(const MotionToken& token) const + { + std::lock_guard lock(mutex_); + return token.valid() && + token.generation <= cancelled_generation_; + } + + bool waitForOwnerExit( + const MotionToken& token, + const std::chrono::milliseconds timeout) + { + if (!token.valid()) { + return true; + } + std::unique_lock lock(mutex_); + return owner_finished_cv_.wait_for( + lock, + timeout, + [this, &token]() { + return !owner_active_ || + active_token_.generation != token.generation; + }); + } + + bool ownerActive(const MotionToken& token) const + { + if (!token.valid()) { + return false; + } + std::lock_guard lock(mutex_); + return owner_active_ && + active_token_.generation == token.generation; + } + + StopWaitStatus waitForStopCompletion( + const StopRequest& request, + const std::chrono::milliseconds timeout) const + { + if (!request.completion) { + return StopWaitStatus::Failed; + } + return request.completion->waitFor(timeout); + } + + bool completeStop() + { + std::shared_ptr completion; + { + std::lock_guard lock(mutex_); + if (owner_active_) { + return false; + } + stop_in_progress_ = false; + blocked_ = false; + active_token_ = {}; + last_kind_ = MotionKind::None; + previous_kind_ = MotionKind::None; + completion = std::move(active_stop_completion_); + owner_finished_cv_.notify_all(); + } + if (completion) { + completion->finish(true); + } + return true; + } + + void failStop() + { + std::shared_ptr completion; + { + std::lock_guard lock(mutex_); + stop_in_progress_ = false; + blocked_ = true; + completion = std::move(active_stop_completion_); + owner_finished_cv_.notify_all(); + } + if (completion) { + completion->finish(false); + } + } + + bool busy() const + { + std::lock_guard lock(mutex_); + return owner_active_ || stop_in_progress_ || blocked_ || + last_kind_ != MotionKind::None; + } + +private: + mutable std::mutex mutex_; + std::condition_variable owner_finished_cv_; + std::uint64_t next_generation_{0}; + std::uint64_t cancelled_generation_{0}; + MotionToken active_token_; + MotionKind last_kind_{MotionKind::None}; + MotionKind previous_kind_{MotionKind::None}; + std::shared_ptr active_stop_completion_; + bool owner_active_{false}; + bool stop_in_progress_{false}; + bool blocked_{false}; +}; + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_MOTION_STATE_H diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h new file mode 100644 index 00000000..4f2f1365 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h @@ -0,0 +1,50 @@ +#ifndef CMVR_ES_AUBO_TORQUE_ON_RESULT_H +#define CMVR_ES_AUBO_TORQUE_ON_RESULT_H + +#include +#include +#include + +#include "common/types/arm/arm_types.h" + +namespace cmvr::device::aubo_internal { + +inline Result preservePrimaryTorqueOnFailure( + Result primary_failure, + const std::optional& cancellation_outcome) +{ + if (!cancellation_outcome.has_value() || + cancellation_outcome->message.empty()) { + return primary_failure; + } + + if (!primary_failure.message.empty()) { + primary_failure.message += "; "; + } + primary_failure.message += + "cancellation handling: " + cancellation_outcome->message; + return primary_failure; +} + +template +std::optional runTorqueOnControllerMutation( + const int success_code, + Mutation&& mutation, + FailureResult&& failure_result, + CancellationOutcome&& cancellation_outcome) +{ + const int return_code = std::forward(mutation)(); + if (return_code != success_code) { + auto primary_failure = + std::forward(failure_result)(return_code); + return preservePrimaryTorqueOnFailure( + std::move(primary_failure), + std::forward(cancellation_outcome)()); + } + return std::forward(cancellation_outcome)(); +} + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_TORQUE_ON_RESULT_H diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index f3b927d2..5e444713 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -2,6 +2,7 @@ #define CMVR_ES_ROBOT_ARM_H #include +#include #include #include #include @@ -28,15 +29,45 @@ public: virtual SafetyMode getSafetyMode() const = 0; virtual ControlMode getControlMode() const = 0; + virtual bool supportsActionQueueMotion() const noexcept { return false; } + virtual bool supportsTeleopGroupServo() const noexcept { return false; } + virtual JointEffortSource jointEffortSource() const noexcept + { + return JointEffortSource::Unspecified; + } + virtual Result torqueOn() = 0; + virtual Result torqueOn( + const std::function& cancellation_requested) + { + if (cancellation_requested) { + try { + if (cancellation_requested()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "torqueOn cancelled before execution"); + } + } catch (...) { + return Result::failure( + ArmErrorCode::CommandRejected, + "torqueOn cancellation check failed"); + } + } + return torqueOn(); + } virtual Result torqueOff() = 0; virtual Result calibrateZeroQ(const std::string& joint_name) = 0; virtual Result emergencyStop() = 0; virtual Result protectiveStop() = 0; virtual Result recoverProtectiveStop( - const JointTrajectory& path, - const MotionOptions& options) = 0; + const JointTrajectory&, + const MotionOptions&) + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "protective recovery is unsupported by this RobotArm"); + } virtual Result setSpeedScaling(double scaling) = 0; virtual double getSpeedScaling() const = 0; virtual bool isProtectiveStopped() const = 0; @@ -76,6 +107,25 @@ public: FrameType frame = FrameType::Base) = 0; virtual Result stopServoMode() = 0; + virtual Result startTorqueMode(const TorqueServoOptions&) + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "torque servo mode is unsupported by this RobotArm"); + } + virtual Result servoTorque(const JointTorqueCommand&) + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "torque servo command is unsupported by this RobotArm"); + } + virtual Result stopTorqueMode() + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "torque servo mode is unsupported by this RobotArm"); + } + virtual Result connect(const std::string& ip, int port) = 0; virtual Result disconnect() = 0; virtual bool isConnected() const = 0; diff --git a/protos/rbk/protocol/src1100_map3d.proto b/protos/rbk/protocol/seer_robokit_map3d.proto similarity index 97% rename from protos/rbk/protocol/src1100_map3d.proto rename to protos/rbk/protocol/seer_robokit_map3d.proto index b33474f1..4936722f 100644 --- a/protos/rbk/protocol/src1100_map3d.proto +++ b/protos/rbk/protocol/seer_robokit_map3d.proto @@ -2,7 +2,7 @@ syntax = "proto3"; package rbk.protocol; -// 仙工 SRC1100 3D 地图文件 0.3dsmap 的最小解析结构。 +// 仙工 SEER Robokit 3D 地图文件 0.3dsmap 的最小解析结构。 // 这里只保留转换统一地图所需字段，未声明字段由 protobuf 作为未知字段跳过。 // 地图坐标系下的三维位置，单位：米。