150 lines
5.4 KiB
C++
150 lines
5.4 KiB
C++
//
|
|
// Created by linbo on 2025/6/20.
|
|
//
|
|
|
|
#ifndef RH56DFTP_DEXHAND_H
|
|
#define RH56DFTP_DEXHAND_H
|
|
|
|
#include <array>
|
|
#include <atomic>
|
|
#include <bitset>
|
|
#include <chrono>
|
|
#include <condition_variable>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <shared_mutex>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
#include <modbus/modbus.h>
|
|
|
|
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
|
|
#include "../../abstract_dexhand.h"
|
|
#include "rh56dftp_tactile_buffer.h"
|
|
|
|
namespace cmvr::device {
|
|
|
|
class ModbusController {
|
|
public:
|
|
ModbusController() = default;
|
|
virtual ~ModbusController();
|
|
|
|
virtual bool open(const std::string& ip, int port);
|
|
virtual void close();
|
|
virtual bool isOpen() const;
|
|
|
|
virtual bool writeRegisters(int address, const uint16_t* values, int count);
|
|
virtual bool readRegisterBlock(
|
|
int start_address,
|
|
int count,
|
|
std::vector<uint16_t>& values);
|
|
|
|
private:
|
|
void closeUnlocked();
|
|
|
|
modbus_t* ctx_{nullptr};
|
|
mutable std::mutex io_mutex_;
|
|
};
|
|
|
|
class RH56DFTPDexhand final : public AbstractDexHand {
|
|
public:
|
|
using FingerType = AbstractDexHand::FingerType;
|
|
using ResultantForce = AbstractDexHand::ResultantForce;
|
|
using TactileRegion = AbstractDexHand::TactileRegion;
|
|
using TactileRegionKey = AbstractDexHand::TactileRegionKey;
|
|
using TactileRegionData = AbstractDexHand::TactileRegionData;
|
|
using Status = AbstractDexHand::Status;
|
|
|
|
static constexpr size_t ANGLE_COMMAND_COUNT = 6;
|
|
static constexpr size_t TACTILE_REGION_SLOT_COUNT = RH56TactileBuffer::REGION_COUNT;
|
|
using RegionMask = std::bitset<TACTILE_REGION_SLOT_COUNT>;
|
|
|
|
explicit RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg);
|
|
RH56DFTPDexhand(
|
|
const config::RH56DFTPDexHandConfig& cfg,
|
|
std::unique_ptr<ModbusController> controller);
|
|
~RH56DFTPDexhand() override;
|
|
|
|
std::string typeName() const override { return "RH56DFTPDexhand"; }
|
|
bool init() override;
|
|
bool start() override;
|
|
bool stop() override;
|
|
|
|
Status state() const override;
|
|
std::string lastError() const override;
|
|
void getState(DexHandState& state) override;
|
|
bool stopOperationalActivity() override;
|
|
bool resumeOperationalActivity() override;
|
|
|
|
void setAngles(const std::vector<int>& finger_joint_angles) override;
|
|
void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
|
|
std::vector<TactileRegionData> getSensorData() override;
|
|
TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
|
|
ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
|
|
|
|
private:
|
|
struct TactileBufferLease {
|
|
std::shared_lock<std::shared_mutex> lock;
|
|
const RH56TactileBuffer* buffer{nullptr};
|
|
};
|
|
|
|
void initializeTactileBuffers();
|
|
bool ensureConnected();
|
|
void refreshTactileData(const RegionMask& mask);
|
|
void tactilePollingLoop();
|
|
|
|
std::shared_ptr<const TactileBufferLease> acquireActiveBufferLease() const;
|
|
std::vector<TactileRegionData> acquireAllRegionData() const;
|
|
TactileRegionData acquireRegionData(FingerType finger, TactileRegion region) const;
|
|
|
|
bool isRequestedMaskCovered(const RegionMask& mask) const;
|
|
bool isActiveBufferReady(const RegionMask& mask) const;
|
|
void ensureTactileMaskReady(const RegionMask& mask, bool allow_background);
|
|
|
|
void transitionTo(Status next_state);
|
|
void enterFault(const std::string& error);
|
|
bool isOperationalState(Status lifecycle) const;
|
|
|
|
std::string ip_address_;
|
|
int port_{6000};
|
|
|
|
std::unique_ptr<ModbusController> controller_;
|
|
config::RH56DFTPDexHandConfig dexhandCfg_;
|
|
|
|
mutable std::mutex lifecycle_mutex_;
|
|
Status lifecycle_state_{Status::CREATED};
|
|
std::string last_error_;
|
|
|
|
mutable std::mutex command_mutex_;
|
|
std::array<int, ANGLE_COMMAND_COUNT> last_commanded_angles_{};
|
|
// Kept separately from last_commanded_angles_: a failed Modbus block
|
|
// write may still have changed a prefix of the device registers. Such
|
|
// an attempt must remain visible to StopAll without being reported as
|
|
// a successfully accepted command.
|
|
std::array<int, ANGLE_COMMAND_COUNT> pending_angle_target_{};
|
|
bool angle_target_unconfirmed_{false};
|
|
|
|
// Normal command and tactile I/O take this gate in shared mode.
|
|
// StopAll first closes admission and then takes it exclusively, which
|
|
// drains every operation that crossed the boundary before the stop.
|
|
mutable std::shared_mutex operational_gate_;
|
|
std::atomic<bool> operational_paused_{false};
|
|
|
|
std::array<RH56TactileBuffer, 2> tactile_buffers_;
|
|
std::array<RegionMask, 2> tactile_buffer_masks_{};
|
|
mutable std::shared_mutex tactile_buffer_mutex_;
|
|
mutable std::mutex tactile_refresh_mutex_;
|
|
|
|
mutable std::mutex polling_mutex_;
|
|
std::condition_variable polling_cv_;
|
|
RegionMask requested_polling_mask_{};
|
|
std::thread tactile_thread_;
|
|
std::atomic<bool> tactile_thread_running_{false};
|
|
std::atomic<int> active_buffer_index_{0};
|
|
std::chrono::milliseconds tactile_poll_interval_{10};
|
|
};
|
|
}
|
|
|
|
#endif //RH56DFTP_DEXHAND_H
|