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

//
// Created by cmvr on 2026/6/29.
//

#ifndef CMVR_ES_HUAYAN_ARM_H
#define CMVR_ES_HUAYAN_ARM_H

#ifndef CMVR_ES_HUAYAN_ROBOT_H
#define CMVR_ES_HUAYAN_ROBOT_H

#include <atomic>
#include <memory>
#include <mutex>
#include <string>
#include <vector>

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

namespace cmvr::device {

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

    std::string typeName() const override { return "HuayanRobot"; }
    bool init() override;
    bool stop() 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 servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }

    Result torqueOn() override;
    Result torqueOff() override;
    Result calibrateZeroQ(const std::string& joint_name) override;
    Result emergencyStop() override;
    Result protectiveStop() override { return emergencyStop(); }
    Result setSpeedScaling(double scaling) override;
    double getSpeedScaling() const override { return speed_scaling_; }
    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;
    Result stopJ(double acceleration) override;
    Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame = FrameType::Base) override;
    Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
    Result stopL(double acceleration) 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;
    Result powerOn() override { return torqueOn(); }
    Result powerOff() override { return torqueOff(); }
    Result brakeRelease() override { return torqueOn(); }
    Result shutdown() override;
    Result clearFault() override;
    Result unlockProtectiveStop() override { return clearFault(); }
    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;
    bool busy() const override { return busy_.load(); }

private:
    struct HrState {
        int moving{0};
        int enabled{0};
        int error{0};
        int error_code{0};
        int error_axis{0};
        int brake{0};
        int paused{0};
        int emergency_stop{0};
        int safeguard{0};
        int electrified{0};
        int connected_to_box{0};
        int blending_done{0};
        int in_pos{0};
        bool valid{false};
    };

    Result ensureConnected_(const std::string& context) const;
    Result unsupported_(const std::string& name) const;
    Result hrResult_(int code, const std::string& context) const;
    bool validDof_(std::size_t size, std::string& error) const;
    HrState readHrState_() const;
    std::vector<double> readJointPositionRad_() const;
    std::vector<double> readJointVelocityRad_() const;
    CartesianPose readTcpPose_() const;
    CartesianVelocity readTcpVelocity_() const;
    std::vector<double> currentJointPositionDeg_() const;
    std::string nextCommandId_() const;
    Result waitMotionDone_(const std::string& context, int timeout_ms) const;

private:
    config::RobotArmConfig cfg_;
    config::VendorRobotArmBackendConfig vendor_cfg_;
    RobotModel model_;
    std::string ip_;
    int port_{10003};
    unsigned int box_id_{0};
    unsigned int robot_id_{0};
    std::string tcp_name_{"TCP"};
    std::string ucs_name_{"Base"};
    double speed_scaling_{1.0};
    std::atomic<bool> connected_{false};
    std::atomic<bool> busy_{false};
    std::atomic<bool> servo_mode_{false};
    mutable std::mutex mutex_;
    mutable std::atomic<unsigned long long> command_seq_{0};
};

} // namespace cmvr::device

#endif // CMVR_ES_HUAYAN_ROBOT_H


#endif //CMVR_ES_HUAYAN_ARM_H