fix(build): restore merged AUBO support files

cmvr-es/common/types/arm/arm_types.h

@@ -2,6 +2,7 @@
 #define CMVR_ES_ARM_TYPES_H
 
 #include <cstdint>
+#include <functional>
 #include <string>
 #include <vector>
 
@@ -103,6 +104,11 @@ struct JointGroupState {
     std::vector<double> position;
     std::vector<double> velocity;
     std::vector<double> 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<double> joint_velocity_limits;
     bool asynchronous{false};
+    std::function<bool()> 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};

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)

cmvr-es/devices/arm/aubo_arm/aubo_arm.h

@@ -2,6 +2,8 @@
 #define CMVR_ES_AUBO_ARM_H
 
 #include <atomic>
+#include <cstdint>
+#include <functional>
 #include <memory>
 #include <mutex>
 #include <optional>
@@ -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; }
+    SafetyMode getSafetyMode() const override;
-    ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
+    ControlMode getControlMode() const override;
+    bool supportsActionQueueMotion() const noexcept override { return true; }
 
     Result torqueOn() override;
+    Result torqueOn(
+        const std::function<bool()>& cancellation_requested) override;
     Result torqueOff() override;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
-    Result protectiveStop() override { return emergencyStop(); }
+    Result protectiveStop() override;
-    Result recoverProtectiveStop(
-        const JointTrajectory&,
-        const MotionOptions&) override
-    {
-        return Result::failure(
-            ArmErrorCode::UnsupportedCommand,
-            "protective recovery is not implemented for AuboArm");
-    }
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
-    bool isProtectiveStopped() const override { return false; }
+    bool isProtectiveStopped() const override;
-    bool isEmergencyStopped() const override { return emergency_stopped_; }
+    bool isEmergencyStopped() const override;
-    bool isFault() const override { return false; }
+    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 clearFault() override;
-    Result unlockProtectiveStop() override { return Result::success(); }
+    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<std::uint64_t> 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<bool> connected_{false};
     std::atomic<bool> busy_{false};
     std::atomic<bool> servo_mode_{false};
-    bool emergency_stopped_{false};
+    std::atomic<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,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 <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;
+    }
+    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

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 <algorithm>
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+
+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<StopCompletion> 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<StopCompletion>();
+        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<StopCompletion> 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<StopCompletion> 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<StopCompletion> 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

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 <optional>
+#include <string>
+#include <utility>
+
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::device::aubo_internal {
+
+inline Result preservePrimaryTorqueOnFailure(
+    Result primary_failure,
+    const std::optional<Result>& 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 <typename Mutation, typename FailureResult,
+          typename CancellationOutcome>
+std::optional<Result> runTorqueOnControllerMutation(
+    const int success_code,
+    Mutation&& mutation,
+    FailureResult&& failure_result,
+    CancellationOutcome&& cancellation_outcome)
+{
+    const int return_code = std::forward<Mutation>(mutation)();
+    if (return_code != success_code) {
+        auto primary_failure =
+            std::forward<FailureResult>(failure_result)(return_code);
+        return preservePrimaryTorqueOnFailure(
+            std::move(primary_failure),
+            std::forward<CancellationOutcome>(cancellation_outcome)());
+    }
+    return std::forward<CancellationOutcome>(cancellation_outcome)();
+}
+
+} // namespace cmvr::device::aubo_internal
+
+#endif // CMVR_ES_AUBO_TORQUE_ON_RESULT_H

cmvr-es/devices/arm/robot_arm.h

@@ -2,6 +2,7 @@
 #define CMVR_ES_ROBOT_ARM_H
 
 #include <cstddef>
+#include <functional>
 #include <memory>
 #include <optional>
 #include <string>
@@ -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<bool()>& 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 JointTrajectory&,
-        const MotionOptions& options) = 0;
+        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;

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 作为未知字段跳过。
 
 // 地图坐标系下的三维位置，单位：米。