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

#ifndef CMVR_ES_AUBO_ARM_H
#define CMVR_ES_AUBO_ARM_H

#include <atomic>
#include <condition_variable>
#include <cstdint>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <thread>
#include <vector>

#include "cmvr/config/arm_config/arm_config.pb.h"
#include "devices/arm/robot_arm.h"

namespace cmvr::device {

class AuboArm final : public RobotArm {
public:
    explicit AuboArm(const config::RobotArmConfig& cfg);
    ~AuboArm() override;

    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; }
    ArmState getRobotState() const override;
    JointGroupState getJointState() const override;
    CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
    RobotMode getRobotMode() const override;
    SafetyMode getSafetyMode() const override;
    ControlMode getControlMode() const override { return ControlMode::Position; }
    bool supportsActionQueueMotion() const noexcept override { return true; }
    Result listBaseFrame(std::vector<std::string>& frame_names) const override;
    Result listTCPFrame(std::vector<std::string>& frame_names) const override;

    Result torqueOn() 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 setSpeedScaling(double scaling) override;
    double getSpeedScaling() const override { return speed_scaling_; }
    bool isProtectiveStopped() const override { return false; }
    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;
    Result stopJ(double acceleration) override;
    Result moveL(const CartesianPose& target,
                 const MotionOptions& options,
                 FrameType frame = FrameType::Base) override;
    Result moveL(const CartesianPose& target,
                 const MotionOptions& options,
                 const std::string& base_frame,
                 const std::string& tcp_frame) override;
    Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
    Result stopL(std::optional<double> acceleration = std::nullopt) override;
    Result stopMotion() override;

    Result startServoMode(const ServoOptions& options) override;
    Result servoJ(const JointPositionCommand& target) override;
    Result servoL(const CartesianPose& target, FrameType frame = FrameType::Base) override;
    Result servoSpeedJ(const JointVelocityCommand& velocity) override;
    Result servoSpeedL(const CartesianVelocity& velocity, FrameType frame = FrameType::Base) override;
    Result stopServoMode() override;

    Result connect(const std::string& ip, int port) override;
    Result disconnect() override;
    bool isConnected() const override { return connected_.load(); }
    Result powerOn() override { return torqueOn(); }
    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 loadProgram(const std::string& program_name) override;
    Result playProgram() override;
    Result pauseProgram() override;
    Result stopProgram() override;

    std::vector<double> ik(const std::string& base_link,
                           const std::string& ee_link,
                           const CartesianPose& pose) override;
    std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override { return nullptr; }
    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(); }

private:
    enum class ActiveMotion {
        None,
        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) const;
    bool tryBeginMotion_(ActiveMotion kind, std::uint64_t& token);
    bool motionCanceled_(std::uint64_t token) const noexcept;
    void releaseMotion_(ActiveMotion kind, std::uint64_t token) noexcept;
    Result stopMotion_(std::optional<ActiveMotion> requested_kind,
                       double acceleration);

#if defined(CMVR_HAS_AUBO_SDK)
    struct SdkState;
    void autoEnableMonitorLoop_();
#endif

private:
    config::RobotArmConfig cfg_;
    config::VendorRobotArmBackendConfig vendor_cfg_;
    RobotModel model_;
    std::string ip_;
    int port_{30004};
    std::string username_;
    std::string password_;
    bool auto_enable_{false};
    double speed_scaling_{1.0};
    std::atomic<bool> connected_{false};
    std::atomic<bool> busy_{false};
    std::atomic<ActiveMotion> active_motion_{ActiveMotion::None};
    std::atomic<std::uint64_t> motion_epoch_{0};
    std::atomic<bool> emergency_stopped_{false};
    std::atomic<bool> hardware_emergency_stopped_{false};
    std::atomic<int> hardware_safety_mode_{0};
    std::atomic<bool> hardware_estop_latched_{false};
    std::atomic<bool> auto_recovery_suppressed_{false};
    mutable std::mutex mutex_;

#if defined(CMVR_HAS_AUBO_SDK)
    std::unique_ptr<SdkState> sdk_;
#endif
};

} // namespace cmvr::device

#endif // CMVR_ES_AUBO_ARM_H