From d02d23a3f97727cdbd88edecb0b9b37cb9b4e9a2 Mon Sep 17 00:00:00 2001 From: lgv Date: Mon, 30 Mar 2026 10:53:07 +0800 Subject: [PATCH] fix: improve RH56 tactile data read speed --- .../applications/include/touch_screen_app.h | 15 +- cmvr-es/applications/src/touch_screen_app.cpp | 122 +++++++-- .../src/touch_screen_app_test.cpp | 4 +- .../touch_screen_app_config.pb.txt | 7 +- .../include/rh56dftp_dexhand.h | 4 +- .../include/rh56dftp_tactile_buffer.h | 7 +- .../rh56dftp_dexhand/src/rh56dftp_dexhand.cpp | 97 +++---- .../src/rh56dftp_dexhand_test.cpp | 4 +- cmvr-es/service/grpc/src/grpc_hlc_service.cpp | 1 + .../touch_screen_app_config.proto | 2 +- script/touch_data.py | 243 ++++++++++++++++++ 11 files changed, 418 insertions(+), 88 deletions(-) create mode 100644 script/touch_data.py diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h index 6b23e7d1..cab6abe8 100644 --- a/cmvr-es/applications/include/touch_screen_app.h +++ b/cmvr-es/applications/include/touch_screen_app.h @@ -57,14 +57,6 @@ public: ROBOT_COMMAND_FAILED // 向机器人下发控制命令失败。 }; - enum class TactileRegion { - TIP = 0, - FINGER, - PAD, - TIP_AND_FINGER, - THUMB_MIDDLE - }; - enum class AlignMode { POSE_AND_POSITION = 0, // 使用配置里的固定 rx/ry/rz 与位置一起对齐。 RX_RY_AND_POSITION, // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。 @@ -202,11 +194,14 @@ public: // 使用哪根手指的触觉阵列判断是否接触。 device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX}; // 使用该手指的哪个触觉区域。 - TactileRegion tactile_region{TactileRegion::TIP_AND_FINGER}; + device::AbstractDexHand::TactileRegion tactile_region{ + device::AbstractDexHand::TactileRegion::TIP}; // 触觉压力和阈值；总和超过该值认为已经接触。 double tactile_pressure_sum_threshold{100.0}; // 触觉峰值阈值；为 0 时表示不使用峰值判据。 double tactile_pressure_peak_threshold{0.0}; + // 当前帧非 0 触觉点数量阈值；只有达到该数量后，才进一步判断 sum/peak 阈值。 + int tactile_nonzero_count_threshold{1}; }; TouchScreenApp(); @@ -244,6 +239,7 @@ public: int targetV() const { return target_v_; } double lastTouchPressureSum() const { return last_touch_pressure_sum_; } double lastTouchPressurePeak() const { return last_touch_pressure_peak_; } + int lastTouchNonzeroCount() const { return last_touch_nonzero_count_; } int lastActiveTagId() const { return last_active_tag_id_; } const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; } @@ -306,6 +302,7 @@ private: double last_touch_pressure_sum_{0.0}; double last_touch_pressure_peak_{0.0}; + int last_touch_nonzero_count_{0}; Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()}; bool locked_target_rotation_valid_{false}; Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()}; diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp index d8a4155e..ac634e97 100644 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ b/cmvr-es/applications/src/touch_screen_app.cpp @@ -45,14 +45,20 @@ bool computeLinearMoveDeltaTool(const Eigen::Matrix& twist_base, } bool isTouchTriggered(const TouchScreenApp::Options& options, + const int nonzero_count, const double pressure_sum, const double pressure_peak) { + if (options.tactile_nonzero_count_threshold > 0 && + nonzero_count < options.tactile_nonzero_count_threshold) { + return false; + } return pressure_sum >= options.tactile_pressure_sum_threshold || (options.tactile_pressure_peak_threshold > 0.0 && pressure_peak >= options.tactile_pressure_peak_threshold); } void accumulateMatrixStats(const device::AbstractDexHand::TactileMatrixView& matrix, + int& nonzero_count_out, double& sum_out, double& peak_out) { if (!matrix.valid()) { @@ -63,6 +69,9 @@ void accumulateMatrixStats(const device::AbstractDexHand::TactileMatrixView& mat const auto* row_data = matrix.rowData(row); for (int col = 0; col < matrix.cols; ++col) { const double value = static_cast(row_data[col]); + if (row_data[col] != 0) { + ++nonzero_count_out; + } sum_out += value; peak_out = std::max(peak_out, value); } @@ -127,27 +136,48 @@ Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) { return aa.axis() * aa.angle(); } +bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target, + const Eigen::Matrix3d& R_current, + double& yaw_rad_out) { + if (!R_target.allFinite() || !R_current.allFinite()) { + return false; + } + + const Eigen::Vector3d z_ref = R_target.col(2); + const Eigen::Vector3d x_ref = R_target.col(0); + const Eigen::Vector3d y_ref = R_target.col(1); + + Eigen::Vector3d in_plane = R_current.col(0) - z_ref * z_ref.dot(R_current.col(0)); + if (in_plane.norm() <= 1e-9) { + in_plane = R_current.col(1) - z_ref * z_ref.dot(R_current.col(1)); + } + const double in_plane_norm = in_plane.norm(); + if (!std::isfinite(in_plane_norm) || in_plane_norm <= 1e-9) { + return false; + } + + in_plane /= in_plane_norm; + yaw_rad_out = std::atan2(y_ref.dot(in_plane), x_ref.dot(in_plane)); + return std::isfinite(yaw_rad_out); +} + bool appendRequestedTactileRegions( const device::AbstractDexHand::FingerType finger, - const TouchScreenApp::TactileRegion region, + const device::AbstractDexHand::TactileRegion region, std::vector& regions_out) { using DeviceTactileRegion = device::AbstractDexHand::TactileRegion; switch (region) { - case TouchScreenApp::TactileRegion::TIP: + case device::AbstractDexHand::TactileRegion::TIP: regions_out.emplace_back(finger, DeviceTactileRegion::TIP); return true; - case TouchScreenApp::TactileRegion::FINGER: + case device::AbstractDexHand::TactileRegion::FINGER: regions_out.emplace_back(finger, DeviceTactileRegion::FINGER); return true; - case TouchScreenApp::TactileRegion::PAD: + case device::AbstractDexHand::TactileRegion::PAD: regions_out.emplace_back(finger, DeviceTactileRegion::PAD); return true; - case TouchScreenApp::TactileRegion::TIP_AND_FINGER: - regions_out.emplace_back(finger, DeviceTactileRegion::TIP); - regions_out.emplace_back(finger, DeviceTactileRegion::FINGER); - return true; - case TouchScreenApp::TactileRegion::THUMB_MIDDLE: + case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: if (finger != device::AbstractDexHand::FingerType::THUMB) { return false; } @@ -197,21 +227,42 @@ device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreen return device::AbstractDexHand::FingerType::INDEX; } -TouchScreenApp::TactileRegion toTactileRegion( +const char* fingerTypeToString(const device::AbstractDexHand::FingerType finger) { + switch (finger) { + case device::AbstractDexHand::FingerType::PINKY: return "PINKY"; + case device::AbstractDexHand::FingerType::RING: return "RING"; + case device::AbstractDexHand::FingerType::MIDDLE: return "MIDDLE"; + case device::AbstractDexHand::FingerType::INDEX: return "INDEX"; + case device::AbstractDexHand::FingerType::THUMB: return "THUMB"; + case device::AbstractDexHand::FingerType::PALM: return "PALM"; + } + return "UNKNOWN"; +} + +const char* tactileRegionToString(const device::AbstractDexHand::TactileRegion region) { + switch (region) { + case device::AbstractDexHand::TactileRegion::TIP: return "TIP"; + case device::AbstractDexHand::TactileRegion::FINGER: return "FINGER"; + case device::AbstractDexHand::TactileRegion::PAD: return "PAD"; + case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE"; + case device::AbstractDexHand::TactileRegion::PALM_PAD: return "PALM_PAD"; + } + return "UNKNOWN"; +} + +device::AbstractDexHand::TactileRegion toTactileRegion( const cmvr::config::TouchScreenTactileRegion region) { switch (region) { case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP: - return TouchScreenApp::TactileRegion::TIP; + return device::AbstractDexHand::TactileRegion::TIP; case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER: - return TouchScreenApp::TactileRegion::FINGER; + return device::AbstractDexHand::TactileRegion::FINGER; case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD: - return TouchScreenApp::TactileRegion::PAD; - case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER: - return TouchScreenApp::TactileRegion::TIP_AND_FINGER; + return device::AbstractDexHand::TactileRegion::PAD; case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE: - return TouchScreenApp::TactileRegion::THUMB_MIDDLE; + return device::AbstractDexHand::TactileRegion::THUMB_MIDDLE; } - return TouchScreenApp::TactileRegion::TIP; + return device::AbstractDexHand::TactileRegion::TIP; } TouchScreenApp::AlignMode toAlignMode(const cmvr::config::TouchScreenAlignMode mode) { @@ -445,6 +496,7 @@ bool TouchScreenApp::init(const std::shared_ptr& robot, last_active_tag_id_ = -1; last_touch_pressure_sum_ = 0.0; last_touch_pressure_peak_ = 0.0; + last_touch_nonzero_count_ = 0; last_align_error_camera_.setZero(); last_status_ = Status::IDLE; } else { @@ -539,6 +591,7 @@ bool TouchScreenApp::startFromPixel(int u, int v) { align_stable_count_ = 0; last_touch_pressure_sum_ = 0.0; last_touch_pressure_peak_ = 0.0; + last_touch_nonzero_count_ = 0; last_active_tag_id_ = -1; last_align_error_camera_.setZero(); locked_target_rotation_valid_ = false; @@ -560,7 +613,15 @@ bool TouchScreenApp::step() { } if (dexhand_) { - updateTouchPressure(); + const bool tactile_ok = updateTouchPressure(); + // std::cout << "[TouchScreenApp][TACTILE] finger=" + // << fingerTypeToString(options_.tactile_finger) + // << ", region=" << tactileRegionToString(options_.tactile_region) + // << ", ok=" << (tactile_ok ? 1 : 0) + // << ", nonzero_count=" << last_touch_nonzero_count_ + // << ", pressure_sum=" << last_touch_pressure_sum_ + // << ", pressure_peak=" << last_touch_pressure_peak_ + // << std::endl; } switch (phase_) { @@ -621,6 +682,7 @@ void TouchScreenApp::stop() { last_active_tag_id_ = -1; last_touch_pressure_sum_ = 0.0; last_touch_pressure_peak_ = 0.0; + last_touch_nonzero_count_ = 0; last_align_error_camera_.setZero(); locked_target_rotation_valid_ = false; locked_target_rotation_.setIdentity(); @@ -874,6 +936,10 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig& if (config.has_tactile_pressure_peak_threshold()) { options.tactile_pressure_peak_threshold = config.tactile_pressure_peak_threshold(); } + if (config.has_tactile_nonzero_count_threshold()) { + options.tactile_nonzero_count_threshold = + std::max(0, config.tactile_nonzero_count_threshold()); + } options_out = options; return true; @@ -1042,9 +1108,8 @@ bool TouchScreenApp::stepAligning() { break; case AlignMode::RX_RY_AND_POSITION: if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) { - const Eigen::Matrix3d R_delta = R_target.transpose() * R_current; - const double locked_yaw_rad = std::atan2(R_delta(1, 0), R_delta(0, 0)); - if (!std::isfinite(locked_yaw_rad)) { + double locked_yaw_rad = 0.0; + if (!extractProjectedYawAboutTargetNormal(R_target, R_current, locked_yaw_rad)) { enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); return false; } @@ -1170,7 +1235,10 @@ bool TouchScreenApp::stepTouching() { return false; } - if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) { + if (isTouchTriggered(options_, + last_touch_nonzero_count_, + last_touch_pressure_sum_, + last_touch_pressure_peak_)) { if (options_.touch_use_speedl) { logTouchingSpeedLState(); } @@ -1206,7 +1274,10 @@ bool TouchScreenApp::stepTouching() { enterFailed(Status::TACTILE_UNAVAILABLE); return false; } - if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) { + if (isTouchTriggered(options_, + last_touch_nonzero_count_, + last_touch_pressure_sum_, + last_touch_pressure_peak_)) { if (!handleTouchTriggered(true)) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; @@ -1562,6 +1633,7 @@ void TouchScreenApp::enterFailed(const Status status) { bool TouchScreenApp::updateTouchPressure() { last_touch_pressure_sum_ = 0.0; last_touch_pressure_peak_ = 0.0; + last_touch_nonzero_count_ = 0; if (!dexhand_) { return false; } @@ -1573,6 +1645,7 @@ bool TouchScreenApp::updateTouchPressure() { return false; } + int nonzero_count = 0; double sum = 0.0; double peak = 0.0; try { @@ -1581,12 +1654,13 @@ bool TouchScreenApp::updateTouchPressure() { if (!sensor.valid()) { return false; } - accumulateMatrixStats(sensor.view, sum, peak); + accumulateMatrixStats(sensor.view, nonzero_count, sum, peak); } } catch (...) { return false; } + last_touch_nonzero_count_ = nonzero_count; last_touch_pressure_sum_ = sum; last_touch_pressure_peak_ = peak; return true; diff --git a/cmvr-es/applications/src/touch_screen_app_test.cpp b/cmvr-es/applications/src/touch_screen_app_test.cpp index bccb1dfd..6c6ae3de 100644 --- a/cmvr-es/applications/src/touch_screen_app_test.cpp +++ b/cmvr-es/applications/src/touch_screen_app_test.cpp @@ -45,6 +45,7 @@ void run_touch_once(int u, int v) { << ", target_c=[" << p_c_target.x() << ", " << p_c_target.y() << ", " << p_c_target.z() << "]" + << ", nonzero_count=" << app.lastTouchNonzeroCount() << ", pressure_sum=" << app.lastTouchPressureSum() << ", pressure_peak=" << app.lastTouchPressurePeak() << ", err_c=[" << app.lastAlignErrorCamera().x() << ", " @@ -58,7 +59,8 @@ void run_touch_once(int u, int v) { align_reached = true; } if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) { - std::cout << "touch triggered, pressure_sum=" << app.lastTouchPressureSum() + std::cout << "touch triggered, nonzero_count=" << app.lastTouchNonzeroCount() + << ", pressure_sum=" << app.lastTouchPressureSum() << ", pressure_peak=" << app.lastTouchPressurePeak() << "\n"; touch_triggered = true; } diff --git a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt index 410d517c..f4f7c78d 100644 --- a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt +++ b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt @@ -135,7 +135,8 @@ retract_twist_base { retract_acceleration: 3.0 retract_duration_s: 0.45 -tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX +tactile_finger: TOUCH_SCREEN_FINGER_TYPE_RING tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP -tactile_pressure_sum_threshold: 300000.0 -tactile_pressure_peak_threshold: 0.0 +tactile_pressure_sum_threshold: 200.0 +tactile_pressure_peak_threshold: 0 +tactile_nonzero_count_threshold: 4 diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h index b0fc27d1..d0861802 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h @@ -35,7 +35,7 @@ namespace cmvr::device { bool isOpen() const; void writeRegisters(int address, const uint16_t* values, int count); - void readRegisterBlock(int start_addr, int count, std::vector& values); + void readRegisterBlock(int start_address, int count, std::vector& values); private: void closeUnlocked(); @@ -121,7 +121,7 @@ namespace cmvr::device { std::thread tactile_thread_; std::atomic tactile_thread_running_{false}; std::atomic active_buffer_index_{0}; - std::chrono::milliseconds tactile_poll_interval_{20}; + std::chrono::milliseconds tactile_poll_interval_{10}; }; } diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h index bc070b06..8d333105 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h @@ -12,6 +12,7 @@ namespace cmvr::device { enum class RH56TactileFillOrder { ROW_MAJOR, + COLUMN_MAJOR_TOP_TO_BOTTOM, COLUMN_MAJOR_BOTTOM_TO_TOP }; @@ -57,8 +58,10 @@ namespace cmvr::device { return; } + const bool reverse_rows = layout.fill_order == RH56TactileFillOrder::COLUMN_MAJOR_BOTTOM_TO_TOP; for (int col = 0; col < layout.cols; ++col) { - for (int row = layout.rows - 1; row >= 0 && input_index < valueCount; --row, ++input_index) { + for (int row_index = 0; row_index < layout.rows && input_index < valueCount; ++row_index, ++input_index) { + const int row = reverse_rows ? (layout.rows - 1 - row_index) : row_index; destination[static_cast(row * layout.cols + col)] = sanitize_point(input[input_index]); } } @@ -114,7 +117,7 @@ namespace cmvr::device { 14, 950, 112, - RH56TactileFillOrder::COLUMN_MAJOR_BOTTOM_TO_TOP} + RH56TactileFillOrder::COLUMN_MAJOR_TOP_TO_BOTTOM} }}; return layouts; } diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp index 8ce5f0ee..b546ef63 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp @@ -23,8 +23,10 @@ namespace { constexpr int kDefaultPort = 6000; constexpr int kMaxRegistersPerRead = 125; - // The RH56 protocol documents addresses and lengths in bytes, while libmodbus - // expects holding-register indexes (16-bit words). + // The RH56 tactile map uses byte-style protocol addresses (3000, 3370, ...) + // while each Modbus read still returns 16-bit registers. Keep addresses in + // the protocol units that the datasheet/Python script use, and only convert + // byte lengths or address deltas to register counts when slicing buffers. constexpr int kRegisterByteWidth = 2; constexpr int kFingerTipByteLength = 18; // 3 x 3 constexpr int kFingerSurfaceByteLength = 192; // 12 x 8 @@ -37,10 +39,6 @@ namespace { return byte_length / kRegisterByteWidth; } - constexpr int toRegisterAddress(const int byte_address) { - return byte_address / kRegisterByteWidth; - } - constexpr int nextByteAddress(const int start_byte_addr, const int byte_length) { return start_byte_addr + byte_length; } @@ -57,12 +55,12 @@ namespace { struct TactileRegionConfig { FingerType finger; TactileRegion region; - int start_register; + int start_address; int register_count; }; struct TactileReadSegment { - int start_register; + int start_address; int register_count; std::vector region_indexes; }; @@ -72,84 +70,84 @@ namespace { const std::array kTactileRegionConfigs = {{ {FingerType::PINKY, TactileRegion::TIP, - toRegisterAddress(kTouchSensorBaseByteAddrPinky), + kTouchSensorBaseByteAddrPinky, toRegisterCount(kFingerTipByteLength)}, {FingerType::PINKY, TactileRegion::FINGER, - toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength)), + nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength), toRegisterCount(kFingerSurfaceByteLength)}, {FingerType::PINKY, TactileRegion::PAD, - toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength), - kFingerSurfaceByteLength)), + nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength), + kFingerSurfaceByteLength), toRegisterCount(kFingerPadByteLength)}, {FingerType::RING, TactileRegion::TIP, - toRegisterAddress(kTouchSensorBaseByteAddrRing), + kTouchSensorBaseByteAddrRing, toRegisterCount(kFingerTipByteLength)}, {FingerType::RING, TactileRegion::FINGER, - toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength)), + nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength), toRegisterCount(kFingerSurfaceByteLength)}, {FingerType::RING, TactileRegion::PAD, - toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength), - kFingerSurfaceByteLength)), + nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength), + kFingerSurfaceByteLength), toRegisterCount(kFingerPadByteLength)}, {FingerType::MIDDLE, TactileRegion::TIP, - toRegisterAddress(kTouchSensorBaseByteAddrMiddle), + kTouchSensorBaseByteAddrMiddle, toRegisterCount(kFingerTipByteLength)}, {FingerType::MIDDLE, TactileRegion::FINGER, - toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength)), + nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength), toRegisterCount(kFingerSurfaceByteLength)}, {FingerType::MIDDLE, TactileRegion::PAD, - toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength), - kFingerSurfaceByteLength)), + nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength), + kFingerSurfaceByteLength), toRegisterCount(kFingerPadByteLength)}, {FingerType::INDEX, TactileRegion::TIP, - toRegisterAddress(kTouchSensorBaseByteAddrIndex), + kTouchSensorBaseByteAddrIndex, toRegisterCount(kFingerTipByteLength)}, {FingerType::INDEX, TactileRegion::FINGER, - toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength)), + nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength), toRegisterCount(kFingerSurfaceByteLength)}, {FingerType::INDEX, TactileRegion::PAD, - toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength), - kFingerSurfaceByteLength)), + nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength), + kFingerSurfaceByteLength), toRegisterCount(kFingerPadByteLength)}, {FingerType::THUMB, TactileRegion::TIP, - toRegisterAddress(kTouchSensorBaseByteAddrThumb), + kTouchSensorBaseByteAddrThumb, toRegisterCount(kFingerTipByteLength)}, {FingerType::THUMB, TactileRegion::FINGER, - toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength)), + nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength), toRegisterCount(kFingerSurfaceByteLength)}, {FingerType::THUMB, TactileRegion::THUMB_MIDDLE, - toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength), - kFingerSurfaceByteLength)), + nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength), + kFingerSurfaceByteLength), toRegisterCount(kThumbMiddleByteLength)}, {FingerType::THUMB, TactileRegion::PAD, - toRegisterAddress(nextByteAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, - kFingerTipByteLength), - kFingerSurfaceByteLength), - kThumbMiddleByteLength)), + nextByteAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, + kFingerTipByteLength), + kFingerSurfaceByteLength), + kThumbMiddleByteLength), toRegisterCount(kThumbPadByteLength)}, {FingerType::PALM, TactileRegion::PALM_PAD, - toRegisterAddress(kTouchSensorBaseByteAddrPalm), + kTouchSensorBaseByteAddrPalm, kTouchSensorRegCountPalm} }}; @@ -201,8 +199,9 @@ namespace { const auto& config = kTactileRegionConfigs[i]; if (!segments.empty()) { auto& last_segment = segments.back(); - const int last_end_register = last_segment.start_register + last_segment.register_count; - if (last_end_register == config.start_register) { + const int last_end_address = + last_segment.start_address + last_segment.register_count * kRegisterByteWidth; + if (last_end_address == config.start_address) { last_segment.register_count += config.register_count; last_segment.region_indexes.push_back(i); continue; @@ -210,7 +209,7 @@ namespace { } TactileReadSegment segment; - segment.start_register = config.start_register; + segment.start_address = config.start_address; segment.register_count = config.register_count; segment.region_indexes.push_back(i); segments.push_back(std::move(segment)); @@ -281,7 +280,7 @@ void ModbusController::writeRegisters(const int address, const uint16_t* values, } } -void ModbusController::readRegisterBlock(const int start_addr, const int count, std::vector& values) { +void ModbusController::readRegisterBlock(const int start_address, const int count, std::vector& values) { values.assign(static_cast(count), 0); std::lock_guard lock(io_mutex_); @@ -291,15 +290,16 @@ void ModbusController::readRegisterBlock(const int start_addr, const int count, for (int offset = 0; offset < count; offset += kMaxRegistersPerRead) { const int current_count = std::min(kMaxRegistersPerRead, count - offset); - const int rc = modbus_read_registers(ctx_, start_addr + offset, current_count, values.data() + offset); + const int current_address = start_address + offset * kRegisterByteWidth; + const int rc = modbus_read_registers(ctx_, current_address, current_count, values.data() + offset); if (rc == -1) { throw std::runtime_error( - "Failed to read register block at " + std::to_string(start_addr + offset) + ": " + + "Failed to read register block at " + std::to_string(current_address) + ": " + modbus_strerror(errno)); } if (rc != current_count) { throw std::runtime_error( - "Incomplete register read at " + std::to_string(start_addr + offset) + ", expected " + + "Incomplete register read at " + std::to_string(current_address) + ", expected " + std::to_string(current_count) + ", got " + std::to_string(rc)); } } @@ -428,7 +428,7 @@ void RH56DFTPDexhand::setAngles(const std::vector& finger_joint_angles) { try { ensureConnected(); controller_->writeRegisters( - toRegisterAddress(kAngleSetByteAddress), + kAngleSetByteAddress, registers.data(), static_cast(registers.size())); @@ -518,10 +518,10 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { std::vector segment_values; for (const auto& segment : read_plan) { - controller_->readRegisterBlock(segment.start_register, segment.register_count, segment_values); + controller_->readRegisterBlock(segment.start_address, segment.register_count, segment_values); for (const size_t region_index : segment.region_indexes) { const auto& config = kTactileRegionConfigs[region_index]; - const int offset = config.start_register - segment.start_register; + const int offset = toRegisterCount(config.start_address - segment.start_address); write_buffer.assignRegionData( config.finger, config.region, @@ -541,12 +541,14 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { } void RH56DFTPDexhand::tactilePollingLoop() { + auto next_poll_deadline = std::chrono::steady_clock::now(); std::unique_lock lock(polling_mutex_); while (tactile_thread_running_.load(std::memory_order_acquire)) { if (requested_polling_mask_.none()) { polling_cv_.wait(lock, [this]() { return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_.any(); }); + next_poll_deadline = std::chrono::steady_clock::now(); continue; } @@ -559,7 +561,14 @@ void RH56DFTPDexhand::tactilePollingLoop() { } lock.lock(); - polling_cv_.wait_for(lock, tactile_poll_interval_, [this, mask]() { + next_poll_deadline += tactile_poll_interval_; + const auto now = std::chrono::steady_clock::now(); + if (next_poll_deadline <= now) { + next_poll_deadline = now; + continue; + } + + polling_cv_.wait_until(lock, next_poll_deadline, [this, mask]() { return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_ != mask; }); } diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp index e014323e..9c917999 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp @@ -61,8 +61,8 @@ TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) { ASSERT_NE(hand_config, nullptr); - const auto finger = DexHand::FingerType::INDEX; - const auto region = DexHand::TactileRegion::FINGER; + const auto finger = DexHand::FingerType::RING; + const auto region = DexHand::TactileRegion::TIP; const int iterations = 20000; const int warmup_ms = 200; const int read_interval_ms = 10; diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index c60d7a20..56f4b072 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp @@ -118,6 +118,7 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr: << ", phase=" << cmvr::app::TouchScreenApp::phaseToString(touch_app_.phase()) << ", active_tag=" << touch_app_.lastActiveTagId() + << ", nonzero_count=" << touch_app_.lastTouchNonzeroCount() << ", pressure_sum=" << touch_app_.lastTouchPressureSum() << ", pressure_peak=" << touch_app_.lastTouchPressurePeak(); last_logged_status = touch_app_.lastStatus(); diff --git a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto index 3c4f2dc5..862728e5 100644 --- a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto +++ b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto @@ -66,7 +66,6 @@ enum TouchScreenTactileRegion { TOUCH_SCREEN_TACTILE_REGION_TIP = 0; TOUCH_SCREEN_TACTILE_REGION_FINGER = 1; TOUCH_SCREEN_TACTILE_REGION_PAD = 2; - TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER = 3; TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4; } @@ -142,4 +141,5 @@ message TouchScreenAppConfig { optional TouchScreenTactileRegion tactile_region = 37; optional double tactile_pressure_sum_threshold = 38; optional double tactile_pressure_peak_threshold = 39; + optional int32 tactile_nonzero_count_threshold = 56; } diff --git a/script/touch_data.py b/script/touch_data.py new file mode 100644 index 00000000..613f0e5f --- /dev/null +++ b/script/touch_data.py @@ -0,0 +1,243 @@ +import time +from pymodbus.client import ModbusTcpClient +from pymodbus.pdu import ExceptionResponse + +# 定义 Modbus TCP 相关参数 +MODBUS_IP = "192.168.11.210" +MODBUS_PORT = 6000 + +# 定义各部分数据地址范围 +TOUCH_SENSOR_BASE_ADDR_PINKY = 3000 # 小拇指 +TOUCH_SENSOR_END_ADDR_PINKY = 3369 + +TOUCH_SENSOR_BASE_ADDR_RING = 3370 # 无名指 +TOUCH_SENSOR_END_ADDR_RING = 3739 + +TOUCH_SENSOR_BASE_ADDR_MIDDLE = 3740 # 中指 +TOUCH_SENSOR_END_ADDR_MIDDLE = 4109 + +TOUCH_SENSOR_BASE_ADDR_INDEX = 4110 # 食指 +TOUCH_SENSOR_END_ADDR_INDEX = 4479 + +TOUCH_SENSOR_BASE_ADDR_THUMB = 4480 # 大拇指 +TOUCH_SENSOR_END_ADDR_THUMB = 4899 + +TOUCH_SENSOR_BASE_ADDR_PALM = 4900 # 掌心 +TOUCH_SENSOR_END_ADDR_PALM = 5123 + +# Modbus 每次最多读取寄存器的数量 +MAX_REGISTERS_PER_READ = 125 + + +def read_register_range(client, start_addr, end_addr): + """ + 批量读取指定地址范围内的寄存器数据。 + """ + register_values = [] + # 分段读取寄存器 + for addr in range(start_addr, end_addr + 1, MAX_REGISTERS_PER_READ * 2): + current_count = min(MAX_REGISTERS_PER_READ, (end_addr - addr) // 2 + 1) + response = client.read_holding_registers(address=addr, count=current_count) + + if isinstance(response, ExceptionResponse) or response.isError(): + print(f"读取寄存器 {addr} 失败: {response}") + register_values.extend([0] * current_count) + else: + register_values.extend(response.registers) + + return register_values + +def format_finger_data(finger_name, data): + """ + 格式化四指触觉数据。 + """ + result = {} + + if finger_name != "大拇指": + # 四指格式 + if len(data) < 185: + print(f"{finger_name} 数据长度不足，至少185个数据，实际：{len(data)}") + return None + + idx = 0 + # 指端数据 3x3 + result['tip_end'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)] + idx += 9 + + # 指尖触觉数据 12x8 + result['tip_touch'] = [data[idx + i*8: idx + (i+1)*8] for i in range(12)] + idx += 96 + + # 指腹触觉数据 10x8 + result['finger_pad'] = [data[idx + i*8: idx + (i+1)*8] for i in range(10)] + idx += 80 + else: + # 大拇指格式 + if len(data) < 210: + print(f"{finger_name} 数据长度不足，至少210个数据，实际：{len(data)}") + return None + + idx = 0 + # 指端数据 3x3 + result['tip_end'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)] + idx += 9 + + # 指尖触觉数据 12x8 + result['tip_touch'] = [data[idx + i*8: idx + (i+1)*8] for i in range(12)] + idx += 96 + + # 指中触觉数据 3x3 + result['middle_touch'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)] + idx += 9 + + # 指腹触觉数据 12x8 + finger_pad = [data[idx + i*8: idx + (i+1)*8] for i in range(12)] + idx += 96 + + # 指腹触觉数据行元素反转 + # finger_pad = [row[::-1] for row in finger_pad] + # 指腹触觉数据行顺序反转 + #finger_pad.reverse() + + result['finger_pad'] = finger_pad + + return result + +def format_palm_data(data): + """ + 格式化掌心数据为14x8矩阵，然后转置为8x14矩阵。 + """ + expected_len = 14 * 8 + if len(data) < expected_len: + print(f"掌心数据长度不足，至少{expected_len}个数据，实际：{len(data)}") + return None + + # 生成原始矩阵（14行8列） + palm_matrix = [data[i*8:(i+1)*8] for i in range(14)] + + # 转置矩阵 + transposed = list(map(list, zip(*palm_matrix))) + + return transposed + + +def print_formatted_finger_data(finger_name, formatted_data): + if formatted_data is None: + print(f"{finger_name} 数据格式化失败") + return + + print(f"--- {finger_name} 指端指端数据 (3x3) ---") + for row in formatted_data.get('tip_end', []): + print(row) + + print(f"--- {finger_name} 指尖触觉数据 (12x8) ---") + for row in formatted_data.get('tip_touch', []): + print(row) + + if finger_name == "大拇指": + if 'middle_touch' in formatted_data: + print(f"--- {finger_name} 指中触觉数据 (3x3) ---") + for row in formatted_data['middle_touch']: + print(row) + else: + print(f"{finger_name} 指中触觉数据缺失") + + print(f"--- {finger_name} 指腹触觉数据 ({'12x8' if finger_name == '大拇指' else '10x8'}) ---") + for row in formatted_data.get('finger_pad', []): + print(row) + +def print_formatted_palm_data(palm_data): + if palm_data is None: + print("掌心数据格式化失败") + return + + print("--- 掌心数据 (8x14) ---") + for row in palm_data: + print(row) + +def read_firmware_version(client): + """ + 读取固件版本，返回十六进制字符串 地址14 + """ + response = client.read_holding_registers(address=14, count=1) + if isinstance(response, ExceptionResponse) or response.isError(): + print(f"读取固件版本失败：{response}") + return None + else: + reg_value = response.registers[0] + # 转换为16进制字符串 + hex_str = format(reg_value, '04X') # 4位，补零 + return hex_str + +def read_multiple_registers(): + client = ModbusTcpClient(MODBUS_IP, port=MODBUS_PORT) + client.connect() + + try: + while True: + start_time = time.time() + + # 读取各部分数据 + pinky_register_values = read_register_range( + client, + TOUCH_SENSOR_BASE_ADDR_PINKY, + TOUCH_SENSOR_END_ADDR_PINKY + ) + ring_register_values = read_register_range( + client, + TOUCH_SENSOR_BASE_ADDR_RING, + TOUCH_SENSOR_END_ADDR_RING + ) + middle_register_values = read_register_range( + client, + TOUCH_SENSOR_BASE_ADDR_MIDDLE, + TOUCH_SENSOR_END_ADDR_MIDDLE + ) + index_register_values = read_register_range( + client, + TOUCH_SENSOR_BASE_ADDR_INDEX, + TOUCH_SENSOR_END_ADDR_INDEX + ) + thumb_register_values = read_register_range( + client, + TOUCH_SENSOR_BASE_ADDR_THUMB, + TOUCH_SENSOR_END_ADDR_THUMB + ) + palm_register_values = read_register_range( + client, + TOUCH_SENSOR_BASE_ADDR_PALM, + TOUCH_SENSOR_END_ADDR_PALM + ) + + end_time = time.time() + frequency = 1 / (end_time - start_time) if end_time > start_time else float('inf') + + # 格式化数据 + pinky_formatted = format_finger_data("小拇指", pinky_register_values) + ring_formatted = format_finger_data("无名指", ring_register_values) + middle_formatted = format_finger_data("中指", middle_register_values) + index_formatted = format_finger_data("食指", index_register_values) + thumb_formatted = format_finger_data("大拇指", thumb_register_values) + palm_formatted = format_palm_data(palm_register_values) + + # 打印格式化数据 + print_formatted_finger_data("小拇指", pinky_formatted) + print_formatted_finger_data("无名指", ring_formatted) + print_formatted_finger_data("中指", middle_formatted) + print_formatted_finger_data("食指", index_formatted) + print_formatted_finger_data("大拇指", thumb_formatted) + print_formatted_palm_data(palm_formatted) + + print(f"读取频率：{frequency:.2f} Hz") + print("\n" + "="*40 + "\n") + + firmware_version_int = read_firmware_version(client) + if firmware_version_int is not None: + print(f"固件版本：{firmware_version_int}") + + finally: + client.close() + + +if __name__ == "__main__": + read_multiple_registers()