fix(touch): validate tactile readings and use newton thresholds
This commit is contained in:
parent
96e3bcf624
commit
149d8cdb5d
@ -28,6 +28,8 @@ dexhand {
|
||||
resultant_length: 3
|
||||
poll_interval_ms: 5
|
||||
response_timeout_ms: 200
|
||||
# 触觉数据最大有效期；超过此时间报不可用，不继续返回旧力值。
|
||||
max_sample_age_ms: 50
|
||||
response_header_bytes: 14
|
||||
tactile_rows: 1
|
||||
tactile_cols: 51
|
||||
|
||||
@ -102,7 +102,7 @@ touch_screen_task {
|
||||
finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
|
||||
region: TOUCH_SCREEN_TACTILE_REGION_TIP
|
||||
criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
|
||||
force_threshold: 1.0
|
||||
force_threshold: 0.1 # N
|
||||
}
|
||||
dwell_time_s: 0.0
|
||||
}
|
||||
|
||||
@ -111,7 +111,7 @@ touch_screen_task {
|
||||
finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
|
||||
region: TOUCH_SCREEN_TACTILE_REGION_TIP
|
||||
criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
|
||||
force_threshold: 1.0
|
||||
force_threshold: 0.1 # N
|
||||
}
|
||||
dwell_time_s: 0.0
|
||||
}
|
||||
|
||||
@ -9,6 +9,7 @@
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
@ -43,6 +44,17 @@ namespace cmvr::device {
|
||||
|
||||
using ResultantForce = TactilePoint;
|
||||
|
||||
// Physical force, separate from device-specific integer tactile data.
|
||||
struct ForceNewtons {
|
||||
double fx{0.0};
|
||||
double fy{0.0};
|
||||
double fz{0.0};
|
||||
|
||||
double magnitude() const {
|
||||
return std::hypot(fx, fy, fz);
|
||||
}
|
||||
};
|
||||
|
||||
enum class FingerType {
|
||||
PINKY,
|
||||
RING,
|
||||
@ -147,6 +159,11 @@ namespace cmvr::device {
|
||||
virtual std::vector<TactileRegionData> getSensorData() = 0;
|
||||
virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0;
|
||||
virtual ResultantForce getResultantForce(FingerType finger, TactileRegion region) = 0;
|
||||
// Backends must provide a documented conversion; raw pressure counts
|
||||
// cannot be assumed to represent newtons.
|
||||
virtual ForceNewtons getResultantForceNewtons(FingerType, TactileRegion) {
|
||||
throw std::runtime_error(typeName() + " does not provide force in newtons.");
|
||||
}
|
||||
|
||||
virtual void setPositions(const std::vector<int>&) {
|
||||
CMVR_LOG(ERROR) << "[AbstractDexHand] setPositions is not supported by this dexhand abstraction.";
|
||||
|
||||
@ -12,3 +12,12 @@ target_link_libraries(px_6ax_gen3
|
||||
)
|
||||
|
||||
install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib)
|
||||
|
||||
# Sensor-only executable: no DeviceManager, arm initialization or calibration.
|
||||
add_executable(px_6ax_gen3_real_test src/px_6ax_gen3_real_test.cpp)
|
||||
target_link_libraries(px_6ax_gen3_real_test PRIVATE
|
||||
px_6ax_gen3 cmvr_es::proto cmvr_es::logging pthread)
|
||||
|
||||
add_executable(px_6ax_gen3_test src/px_6ax_gen3_test.cpp)
|
||||
target_link_libraries(px_6ax_gen3_test PRIVATE
|
||||
px_6ax_gen3 cmvr_es::proto cmvr_es::logging gtest gtest_main pthread)
|
||||
|
||||
51
cmvr-es/devices/dexhand/px_6ax_gen3/README.md
Normal file
51
cmvr-es/devices/dexhand/px_6ax_gen3/README.md
Normal file
@ -0,0 +1,51 @@
|
||||
# PX-6AX GEN3 传感器读取与真实 USB 测试
|
||||
|
||||
真实测试只创建 PX6AXGen3 和 POSIX 串口，不初始化机械臂或 DeviceManager；禁用自动标定，传输层只允许 `0xFB` 读取命令。
|
||||
|
||||
在仓库根目录运行：
|
||||
|
||||
```bash
|
||||
# 每次读取都发起一个真实请求：检查串口应答耗时和按压力值。
|
||||
./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \
|
||||
--mode sync --duration-s 30 --csv /tmp/paxini-sync.csv --raw-log /tmp/paxini-frames.log
|
||||
|
||||
# 与触屏任务相同：后台轮询，前台读取最新有效缓存。
|
||||
./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \
|
||||
--mode stream --duration-s 30 --csv /tmp/paxini-stream.csv
|
||||
```
|
||||
|
||||
也可把 `--port` 指定为 `/dev/serial/by-id/` 下的稳定设备链接。设备端口取决于连接顺序；测试参数不会更改机器人部署配置里的串口。`module-id=2` 对应协议设备地址 3。
|
||||
|
||||
脚本会构建真实测试并优先加载本次构建的驱动和 protobuf。默认构建目录为 `cmake-build-debug`，可用 `CMVR_BUILD_DIR` 覆盖；构建目录须已完成 CMake 配置。串口应可读写，运行测试前退出占用同一串口的程序。
|
||||
|
||||
测试期间可用手轻按、松开传感器，终端每 100 ms 显示一次力值，CSV 记录每次 getter 调用。按 Ctrl-C 可结束。CSV 的无效读数留空，不能当作零力。进程在初始化失败、没有有效数据或存在读取失败时返回非零退出码。
|
||||
|
||||
- `sync` 的 `read_ms` 是一次驱动请求/应答调用的耗时；默认每 5 ms 请求一次。
|
||||
- `stream` 的 `read_ms` 是读取缓存耗时，成功次数包含重复快照，不能据此推算传感器实际更新频率。
|
||||
- `force_N`、`fz_N` 和 CSV 中的三个力分量均由驱动直接返回，单位为 N；例如原始值 1 对应 0.1 N、108 对应 10.8 N。传感器标称输出频率 83.3 Hz；请求频率可以高于内部测量更新频率。
|
||||
- 串口往返时间不包含“物理接触到传感器产生非零输出”的全部时间。需要按压试验或外部同步信号才能测量接触检测延迟。
|
||||
- `--raw-log` 会记录原始 TX/RX 和单调时钟时间，用于对照手册定位帧问题；写日志会给时间测量带来少量开销。
|
||||
|
||||
## 驱动行为
|
||||
|
||||
`getResultantForceNewtons()` 将合力寄存器的三个原始分量各乘以 0.1，返回以 N 为单位的浮点力值；原有 `getResultantForce()` 保留原始整数。触屏任务和 USB 测试使用牛顿接口，不再额外换算。触屏任务的 `force_threshold`、力值日志和 `lastTouchPressureSum()` 均使用 N；阈值 `0.1` 与旧版原始值阈值 `1.0` 对应相同力度。FZ 判据比较法向力，MAGNITUDE 判据比较三轴合力大小，多区域按原逻辑累加。
|
||||
|
||||
MuJoCo 零值触觉后端也实现牛顿接口。尚无确定换算系数的其他后端（如 RH56DFTP）调用该接口会明确报错，不会把原始压力计数当成 N。
|
||||
|
||||
合力应答按 `14 字节头部 + 3 字节数据 + 1 字节 LRC` 完整读取。验证帧长度、设备地址、预留位、功能码、寄存器地址、字节数及 LRC，并支持分片、请求回显和噪声后的重新定位。
|
||||
|
||||
读状态字节属于内部调试信息（手册 5.3.5）；实测正常读应答为 `0x01`，不能套用写应答 `0x00=成功` 的规则。自动标定的写应答要求完整 15 字节并且状态为 0。
|
||||
|
||||
`max_sample_age_ms` 默认 50 ms，可在设备配置中调整。每类数据分别记录请求开始时间：较晚返回的旧请求不能被重新标记为新数据。此值独立于 `response_timeout_ms`（默认 200 ms）。后台读取接口遇到过期或失效快照会抛出异常，不等待串口补读、不返回旧力值或伪造零值；触屏任务现有的异常捕获会将其识别为触觉不可用。
|
||||
|
||||
通信失败会使快照失效，后台继续尝试恢复，只有通过验证的新应答才能恢复有效数据。`stop()` 清除缓存；停止或故障状态下的 getter 报错，显式 `init()` / `start()` 后才能恢复。初始化重试失败会返回 false。
|
||||
|
||||
## 自动回归测试
|
||||
|
||||
真实设备无法稳定制造的坏帧和超时，用可注入的串口实现验证：
|
||||
|
||||
```bash
|
||||
cmake --build cmake-build-debug --target px_6ax_gen3_test -j 4
|
||||
LD_LIBRARY_PATH="$PWD/cmake-build-debug:$PWD/cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3:$PWD/cmake-build-debug/cmvr-es/hardware:$PWD/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \
|
||||
./cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3/px_6ax_gen3_test
|
||||
```
|
||||
@ -39,6 +39,8 @@ namespace cmvr::device {
|
||||
};
|
||||
|
||||
explicit PX6AXGen3(const config::PX6AXGen3& cfg);
|
||||
PX6AXGen3(const config::PX6AXGen3& cfg,
|
||||
std::unique_ptr<::cmvr::AbstractSerialTransport> serial);
|
||||
~PX6AXGen3() override;
|
||||
|
||||
std::string typeName() const override { return "PX6AXGen3"; }
|
||||
@ -54,7 +56,10 @@ namespace cmvr::device {
|
||||
void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
|
||||
std::vector<TactileRegionData> getSensorData() override;
|
||||
TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
|
||||
// Throws when no fresh, validated sample is available. A failed read is
|
||||
// never represented as a zero force (which would mean no contact).
|
||||
ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
|
||||
ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override;
|
||||
|
||||
private:
|
||||
struct SensorSnapshot {
|
||||
@ -64,6 +69,8 @@ namespace cmvr::device {
|
||||
int cols{0};
|
||||
bool tactile_valid{false};
|
||||
bool resultant_valid{false};
|
||||
std::chrono::steady_clock::time_point tactile_request_time{};
|
||||
std::chrono::steady_clock::time_point resultant_request_time{};
|
||||
};
|
||||
|
||||
static PollingReadMode parsePollingReadMode(config::PX6AXGen3PollingReadMode mode);
|
||||
@ -81,17 +88,19 @@ namespace cmvr::device {
|
||||
|
||||
bool isSupportedRegion(FingerType finger, TactileRegion region) const;
|
||||
bool isSnapshotReady(bool require_tactile, bool require_resultant) const;
|
||||
bool isSampleFresh(std::chrono::steady_clock::time_point request_time) const;
|
||||
TactileRegionData buildSupportedRegionSnapshot() const;
|
||||
std::pair<bool, bool> resolvePollingReadSelection() const;
|
||||
|
||||
void clearOperationalError();
|
||||
void handleRefreshFailure(const std::string& error, bool had_valid_snapshot);
|
||||
void handleRefreshFailure(const std::string& error);
|
||||
void transitionTo(Status next_state);
|
||||
void enterFault(const std::string& error);
|
||||
bool isOperationalState(Status lifecycle) const;
|
||||
|
||||
std::unique_ptr<::cmvr::AbstractSerialTransport> serial_;
|
||||
config::PX6AXGen3 config_;
|
||||
bool config_valid_{false};
|
||||
|
||||
mutable std::mutex lifecycle_mutex_;
|
||||
Status lifecycle_state_{Status::CREATED};
|
||||
@ -109,6 +118,7 @@ namespace cmvr::device {
|
||||
int tactile_rows_{1};
|
||||
int tactile_cols_{0};
|
||||
int response_timeout_ms_{200};
|
||||
std::chrono::milliseconds max_sample_age_{50};
|
||||
FingerType tactile_finger_{FingerType::INDEX};
|
||||
TactileRegion tactile_region_{TactileRegion::TIP};
|
||||
PollingReadMode polling_read_mode_{PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE};
|
||||
|
||||
@ -125,32 +125,13 @@ namespace {
|
||||
return frame;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> extractPayload(const std::vector<uint8_t>& response,
|
||||
const size_t frame_offset,
|
||||
const size_t response_header_bytes,
|
||||
const size_t payload_length) {
|
||||
if (response.size() < frame_offset + response_header_bytes + payload_length) {
|
||||
CMVR_LOG(ERROR) << "PX-6AX GEN3 response shorter than expected payload window.";
|
||||
return {};
|
||||
}
|
||||
return std::vector<uint8_t>(
|
||||
response.begin() + static_cast<std::ptrdiff_t>(frame_offset + response_header_bytes),
|
||||
response.begin() + static_cast<std::ptrdiff_t>(frame_offset + response_header_bytes + payload_length));
|
||||
}
|
||||
// UART read reply: 14-byte header, N payload bytes, one LRC byte.
|
||||
constexpr size_t kResponseHeaderBytes = 14U;
|
||||
constexpr size_t kResponseOverheadBytes = kResponseHeaderBytes + 1U;
|
||||
|
||||
size_t findResponseFrameOffset(const std::vector<uint8_t>& response,
|
||||
const size_t expected_frame_bytes) {
|
||||
if (response.size() < expected_frame_bytes) {
|
||||
return std::string::npos;
|
||||
}
|
||||
|
||||
for (size_t offset = 0; offset + expected_frame_bytes <= response.size(); ++offset) {
|
||||
if (response[offset] == 0xAA && response[offset + 1] == 0x55) {
|
||||
return offset;
|
||||
}
|
||||
}
|
||||
|
||||
return std::string::npos;
|
||||
uint16_t readLe16(const std::vector<uint8_t>& bytes, const size_t offset) {
|
||||
return static_cast<uint16_t>(bytes[offset]) |
|
||||
static_cast<uint16_t>(static_cast<uint16_t>(bytes[offset + 1U]) << 8U);
|
||||
}
|
||||
|
||||
std::string previewBytesHex(const std::vector<uint8_t>& data, const size_t max_bytes = 32U) {
|
||||
@ -171,39 +152,83 @@ namespace {
|
||||
return stream.str();
|
||||
}
|
||||
|
||||
std::vector<uint8_t> readFramedResponse(cmvr::AbstractSerialTransport& serial,
|
||||
const size_t expected_frame_bytes,
|
||||
const size_t max_prefix_bytes,
|
||||
const std::chrono::milliseconds timeout,
|
||||
const std::string& response_name) {
|
||||
std::vector<uint8_t> transact(cmvr::AbstractSerialTransport& serial,
|
||||
const std::vector<uint8_t>& request,
|
||||
const size_t payload_length,
|
||||
const std::chrono::milliseconds timeout) {
|
||||
if (!serial.flushInput()) {
|
||||
throw std::runtime_error("Failed to flush PX6AXGen3 input: " + serial.lastError());
|
||||
}
|
||||
if (!serial.write(request)) {
|
||||
throw std::runtime_error("Failed to send PX6AXGen3 request: " + serial.lastError());
|
||||
}
|
||||
|
||||
const size_t expected_size = kResponseOverheadBytes + payload_length;
|
||||
// Permit a request echo/noisy prefix, but bound resynchronization work.
|
||||
const size_t max_response_bytes = expected_size + 4096U;
|
||||
const auto deadline = std::chrono::steady_clock::now() + timeout;
|
||||
std::vector<uint8_t> response;
|
||||
const bool read_ok = serial.read(expected_frame_bytes, timeout, response);
|
||||
auto frame_offset = findResponseFrameOffset(response, expected_frame_bytes);
|
||||
|
||||
while (frame_offset == std::string::npos &&
|
||||
response.size() < expected_frame_bytes + max_prefix_bytes) {
|
||||
size_t offset = 0;
|
||||
std::string error = "Incomplete PX6AXGen3 response";
|
||||
while (response.size() < max_response_bytes) {
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
if (now >= deadline) {
|
||||
break;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> extra_bytes;
|
||||
const auto remaining_timeout = std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now);
|
||||
if (!serial.read(1U, remaining_timeout, extra_bytes)) {
|
||||
std::vector<uint8_t> chunk;
|
||||
const auto remaining = std::max(std::chrono::milliseconds(1),
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now));
|
||||
// Read available bytes; never wait for a guessed length before
|
||||
// identifying the header. This also handles split headers/echoes.
|
||||
const bool read_ok = serial.read(1U, remaining, chunk);
|
||||
if (chunk.size() > max_response_bytes - response.size()) {
|
||||
throw std::runtime_error("PX6AXGen3 response exceeds resynchronization limit");
|
||||
}
|
||||
response.insert(response.end(), chunk.begin(), chunk.end());
|
||||
while (offset + kResponseHeaderBytes <= response.size()) {
|
||||
if (response[offset] != 0xAA || response[offset + 1U] != 0x55) {
|
||||
++offset;
|
||||
continue;
|
||||
}
|
||||
// Frame length counts data[4] through data[N+13], excluding LRC.
|
||||
if (readLe16(response, offset + 2U) != payload_length + 10U ||
|
||||
response[offset + 4U] != request[4] ||
|
||||
response[offset + 5U] != 0x00 ||
|
||||
!std::equal(request.begin() + 6, request.begin() + 13,
|
||||
response.begin() + static_cast<std::ptrdiff_t>(offset + 6U))) {
|
||||
error = "PX6AXGen3 response does not match request (length/address/function)";
|
||||
++offset;
|
||||
continue;
|
||||
}
|
||||
if (response.size() - offset < expected_size) {
|
||||
break;
|
||||
}
|
||||
|
||||
response.insert(response.end(), extra_bytes.begin(), extra_bytes.end());
|
||||
frame_offset = findResponseFrameOffset(response, expected_frame_bytes);
|
||||
uint8_t sum = 0;
|
||||
for (size_t i = offset; i < offset + expected_size; ++i) {
|
||||
sum = static_cast<uint8_t>(sum + response[i]);
|
||||
}
|
||||
|
||||
if (!read_ok && frame_offset == std::string::npos) {
|
||||
CMVR_LOG(ERROR) << "Failed to read " << response_name << ": " << serial.lastError()
|
||||
<< ", raw=" << previewBytesHex(response);
|
||||
if (sum != 0U) {
|
||||
error = "Invalid PX6AXGen3 response LRC";
|
||||
++offset;
|
||||
continue;
|
||||
}
|
||||
|
||||
return response;
|
||||
// Manual 5.3.5: read status is internal/debug information;
|
||||
// hardware returns 0x01 on normal force reads. Only write ACKs
|
||||
// define 0x00 as success (5.4.2), so do not apply it to reads.
|
||||
if (request[6] == 0x79 && response[offset + 13U] != 0x00) {
|
||||
throw std::runtime_error("PX6AXGen3 returned status " +
|
||||
std::to_string(response[offset + 13U]));
|
||||
}
|
||||
return std::vector<uint8_t>(
|
||||
response.begin() + static_cast<std::ptrdiff_t>(offset + kResponseHeaderBytes),
|
||||
response.begin() + static_cast<std::ptrdiff_t>(offset + kResponseHeaderBytes + payload_length));
|
||||
}
|
||||
if (!read_ok || chunk.empty()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
throw std::runtime_error(error + "; " + serial.lastError() +
|
||||
", raw=" + previewBytesHex(response));
|
||||
}
|
||||
|
||||
std::array<int, 3> parseResultantPayload(const std::vector<uint8_t>& payload) {
|
||||
@ -303,21 +328,28 @@ PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadModeName(std::string value
|
||||
}
|
||||
|
||||
PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg)
|
||||
: serial_(std::make_unique<::cmvr::PosixSerialTransport>()),
|
||||
config_(cfg) {
|
||||
: PX6AXGen3(cfg, std::make_unique<::cmvr::PosixSerialTransport>()) {}
|
||||
|
||||
PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg,
|
||||
std::unique_ptr<::cmvr::AbstractSerialTransport> serial)
|
||||
: serial_(std::move(serial)), config_(cfg) {
|
||||
id_ = config_.id();
|
||||
port_name_ = config_.serial_port();
|
||||
if (!config_.sensor_model().empty()) {
|
||||
sensor_model_ = config_.sensor_model();
|
||||
}
|
||||
module_id_ = std::max(0, config_.module_id());
|
||||
module_id_ = config_.module_id();
|
||||
if (!serial_ || module_id_ < 0 || module_id_ > 254) {
|
||||
enterFault("PX6AXGen3 requires a serial transport and module_id in [0, 254].");
|
||||
return;
|
||||
}
|
||||
device_address_ = module_id_ + 1;
|
||||
if (config_.baud_rate() > 0) {
|
||||
baud_rate_ = config_.baud_rate();
|
||||
}
|
||||
distributed_length_ = config_.distributed_length();
|
||||
if (distributed_length_ <= 0) {
|
||||
enterFault("PX6AXGen3 requires config.distributed_length to be explicitly configured.");
|
||||
if (distributed_length_ <= 0 || distributed_length_ > 65525 || distributed_length_ % 3 != 0) {
|
||||
enterFault("PX6AXGen3 distributed_length must be a positive multiple of 3 fitting the UART frame.");
|
||||
return;
|
||||
}
|
||||
if (config_.resultant_length() > 0) {
|
||||
@ -326,6 +358,15 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg)
|
||||
if (config_.response_header_bytes() > 0) {
|
||||
response_header_bytes_ = config_.response_header_bytes();
|
||||
}
|
||||
if (resultant_length_ != 3 || response_header_bytes_ != kResponseHeaderBytes ||
|
||||
config_.resultant_length() < 0 || config_.response_header_bytes() < 0 ||
|
||||
config_.max_sample_age_ms() < 0) {
|
||||
enterFault("PX6AXGen3 requires resultant_length=3, response_header_bytes=14 and a nonnegative max_sample_age_ms.");
|
||||
return;
|
||||
}
|
||||
if (config_.max_sample_age_ms() > 0) {
|
||||
max_sample_age_ = std::chrono::milliseconds(config_.max_sample_age_ms());
|
||||
}
|
||||
if (config_.tactile_rows() > 0) {
|
||||
tactile_rows_ = config_.tactile_rows();
|
||||
}
|
||||
@ -350,6 +391,7 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg)
|
||||
poll_interval_ = std::chrono::milliseconds(config_.poll_interval_ms());
|
||||
}
|
||||
initializeSnapshot();
|
||||
config_valid_ = true;
|
||||
}
|
||||
|
||||
PX6AXGen3::~PX6AXGen3() {
|
||||
@ -357,15 +399,15 @@ PX6AXGen3::~PX6AXGen3() {
|
||||
}
|
||||
|
||||
bool PX6AXGen3::init() {
|
||||
try {
|
||||
ensureConnected();
|
||||
if (!isOperationalState(state())) {
|
||||
if (!config_valid_) {
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
refreshSensorDataWithRetry(
|
||||
5,
|
||||
std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2)));
|
||||
return isOperationalState(state());
|
||||
const auto [tactile, resultant] = resolvePollingReadSelection();
|
||||
return isOperationalState(state()) && isSnapshotReady(tactile, resultant);
|
||||
} catch (const std::exception& e) {
|
||||
enterFault("[PX6AXGen3](init): " + std::string(e.what()));
|
||||
return false;
|
||||
@ -373,8 +415,10 @@ bool PX6AXGen3::init() {
|
||||
}
|
||||
|
||||
bool PX6AXGen3::start() {
|
||||
if (!config_valid_) {
|
||||
return false;
|
||||
}
|
||||
if (polling_thread_running_.exchange(true, std::memory_order_acq_rel)) {
|
||||
transitionTo(Status::STREAMING);
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -383,19 +427,22 @@ bool PX6AXGen3::start() {
|
||||
polling_thread_.join();
|
||||
}
|
||||
|
||||
ensureConnected();
|
||||
if (!isOperationalState(state())) {
|
||||
polling_thread_running_.store(false, std::memory_order_release);
|
||||
return false;
|
||||
bool requested_polling;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(polling_mutex_);
|
||||
requested_polling = requested_polling_;
|
||||
}
|
||||
if (requested_polling_) {
|
||||
if (requested_polling) {
|
||||
refreshSensorDataWithRetry(
|
||||
5,
|
||||
std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2)));
|
||||
} else {
|
||||
std::lock_guard<std::mutex> lock(refresh_mutex_);
|
||||
ensureConnected();
|
||||
}
|
||||
|
||||
polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this);
|
||||
transitionTo(Status::STREAMING);
|
||||
polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this);
|
||||
polling_cv_.notify_all();
|
||||
return true;
|
||||
} catch (const std::exception& e) {
|
||||
@ -413,6 +460,8 @@ bool PX6AXGen3::stop() {
|
||||
polling_thread_.join();
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(refresh_mutex_);
|
||||
initializeSnapshot();
|
||||
closeConnection();
|
||||
|
||||
if (state() != Status::FAULT) {
|
||||
@ -434,10 +483,13 @@ std::string PX6AXGen3::lastError() const {
|
||||
void PX6AXGen3::getState(DexHandState& state_out) {
|
||||
DexHandState next_state{};
|
||||
next_state.is_initialized = isOperationalState(state());
|
||||
const auto [tactile, resultant] = resolvePollingReadSelection();
|
||||
const bool ready = isSnapshotReady(tactile, resultant);
|
||||
next_state.is_initialized = next_state.is_initialized && ready;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
if (latest_snapshot_.resultant_valid) {
|
||||
if (latest_snapshot_.resultant_valid && isSampleFresh(latest_snapshot_.resultant_request_time)) {
|
||||
next_state.hands[0].force = latest_snapshot_.resultant_force_tenths[2];
|
||||
}
|
||||
}
|
||||
@ -445,6 +497,8 @@ void PX6AXGen3::getState(DexHandState& state_out) {
|
||||
const auto error = lastError();
|
||||
if (!error.empty()) {
|
||||
next_state.hands[0].error_message.push_back(error);
|
||||
} else if (!ready) {
|
||||
next_state.hands[0].error_message.push_back("PX6AXGen3 sample is unavailable or stale.");
|
||||
}
|
||||
|
||||
state_out = std::move(next_state);
|
||||
@ -468,7 +522,8 @@ void PX6AXGen3::setTactilePollingRegions(const std::vector<TactileRegionKey>& re
|
||||
}
|
||||
polling_cv_.notify_all();
|
||||
|
||||
if (!regions.empty() && isOperationalState(state())) {
|
||||
if (!regions.empty() && isOperationalState(state()) &&
|
||||
!polling_thread_running_.load(std::memory_order_acquire)) {
|
||||
refreshSensorData();
|
||||
}
|
||||
}
|
||||
@ -482,8 +537,7 @@ std::vector<TactileRegionData> PX6AXGen3::getSensorData() {
|
||||
|
||||
TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion region) {
|
||||
if (!isSupportedRegion(finger, region)) {
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data.";
|
||||
return {};
|
||||
throw std::invalid_argument("PX6AXGen3 requested tactile region is not configured.");
|
||||
}
|
||||
|
||||
ensureSensorReady(true, true, false);
|
||||
@ -492,16 +546,14 @@ TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion regi
|
||||
|
||||
PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, TactileRegion region) {
|
||||
if (!isSupportedRegion(finger, region)) {
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data.";
|
||||
return {};
|
||||
throw std::invalid_argument("PX6AXGen3 requested resultant-force region is not configured.");
|
||||
}
|
||||
|
||||
ensureSensorReady(true, false, true);
|
||||
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
if (!latest_snapshot_.resultant_valid) {
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 resultant-force snapshot is not ready.";
|
||||
return {};
|
||||
if (!latest_snapshot_.resultant_valid || !isSampleFresh(latest_snapshot_.resultant_request_time)) {
|
||||
throw std::runtime_error("PX6AXGen3 resultant-force sample is unavailable or stale.");
|
||||
}
|
||||
|
||||
return ResultantForce{
|
||||
@ -511,29 +563,28 @@ PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, Tactil
|
||||
};
|
||||
}
|
||||
|
||||
PX6AXGen3::ForceNewtons PX6AXGen3::getResultantForceNewtons(FingerType finger, TactileRegion region) {
|
||||
const auto raw = getResultantForce(finger, region);
|
||||
// PX-6AX GEN3 manual 5.6.2: one resultant-force LSB is 0.1 N.
|
||||
constexpr double kNewtonsPerCount = 0.1;
|
||||
return {raw.fx * kNewtonsPerCount, raw.fy * kNewtonsPerCount, raw.fz * kNewtonsPerCount};
|
||||
}
|
||||
|
||||
void PX6AXGen3::initializeSnapshot() {
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
latest_snapshot_ = SensorSnapshot{};
|
||||
}
|
||||
|
||||
void PX6AXGen3::ensureConnected() {
|
||||
if (port_name_.empty()) {
|
||||
enterFault("PX6AXGen3 serial port is not configured.");
|
||||
return;
|
||||
if (!config_valid_ || port_name_.empty()) {
|
||||
throw std::runtime_error("PX6AXGen3 serial/configuration is invalid.");
|
||||
}
|
||||
|
||||
if (!serial_) {
|
||||
serial_ = std::make_unique<::cmvr::PosixSerialTransport>();
|
||||
}
|
||||
|
||||
if (!serial_->isOpen()) {
|
||||
if (!serial_->open(::cmvr::AbstractSerialTransport::Config{port_name_, baud_rate_})) {
|
||||
enterFault("Failed to open PX-6AX GEN3 serial transport: " + serial_->lastError());
|
||||
return;
|
||||
throw std::runtime_error("Failed to open PX6AXGen3 serial transport: " + serial_->lastError());
|
||||
}
|
||||
calibration_performed_ = false;
|
||||
}
|
||||
|
||||
calibrateIfRequested();
|
||||
if (!isOperationalState(state())) {
|
||||
transitionTo(Status::INITIALIZED);
|
||||
@ -553,27 +604,9 @@ void PX6AXGen3::calibrateIfRequested() {
|
||||
if (!auto_calibrate_ || calibration_performed_) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto frame = buildCommandFrame(CommandType::CALIBRATION, device_address_, distributed_length_);
|
||||
if (!serial_->flushInput()) {
|
||||
enterFault("Failed to flush serial input before calibration: " + serial_->lastError());
|
||||
return;
|
||||
}
|
||||
if (!serial_->write(frame)) {
|
||||
enterFault("Failed to send calibration command: " + serial_->lastError());
|
||||
return;
|
||||
}
|
||||
const auto response = readFramedResponse(
|
||||
*serial_,
|
||||
2U,
|
||||
frame.size(),
|
||||
std::chrono::milliseconds(response_timeout_ms_),
|
||||
"calibration response");
|
||||
if (findResponseFrameOffset(response, 2U) == std::string::npos) {
|
||||
enterFault("PX-6AX GEN3 calibration command did not receive a valid acknowledgment. raw=" +
|
||||
previewBytesHex(response));
|
||||
return;
|
||||
}
|
||||
// Write acknowledgment has a complete 14-byte header and LRC, no payload.
|
||||
transact(*serial_, frame, 0U, std::chrono::milliseconds(response_timeout_ms_));
|
||||
calibration_performed_ = true;
|
||||
}
|
||||
|
||||
@ -584,117 +617,51 @@ void PX6AXGen3::refreshSensorData() {
|
||||
|
||||
void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_resultant) {
|
||||
if (!read_distributed && !read_resultant) {
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 refreshSensorData requires at least one data type to read.";
|
||||
return;
|
||||
throw std::invalid_argument("PX6AXGen3 refresh requires at least one data type.");
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> refresh_lock(refresh_mutex_);
|
||||
const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant);
|
||||
|
||||
try {
|
||||
ensureConnected();
|
||||
if (!isOperationalState(state())) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<TactilePoint> tactile_points;
|
||||
int rows = 0;
|
||||
int cols = 0;
|
||||
bool tactile_valid = false;
|
||||
|
||||
if (read_distributed) {
|
||||
const auto distributed_frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_);
|
||||
if (!serial_->flushInput()) {
|
||||
handleRefreshFailure("Failed to flush serial input before distributed-force read: " + serial_->lastError(), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
if (!serial_->write(distributed_frame)) {
|
||||
handleRefreshFailure("Failed to send distributed-force command: " + serial_->lastError(), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
const size_t distributed_frame_bytes =
|
||||
static_cast<size_t>(response_header_bytes_) + static_cast<size_t>(distributed_length_);
|
||||
const auto distributed_response = readFramedResponse(
|
||||
*serial_,
|
||||
distributed_frame_bytes,
|
||||
distributed_frame.size(),
|
||||
std::chrono::milliseconds(response_timeout_ms_),
|
||||
"distributed tactile response");
|
||||
const auto distributed_frame_offset =
|
||||
findResponseFrameOffset(distributed_response, distributed_frame_bytes);
|
||||
if (distributed_frame_offset == std::string::npos) {
|
||||
handleRefreshFailure("Invalid distributed tactile response header. raw=" + previewBytesHex(distributed_response), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
|
||||
tactile_points = parseDistributedPayload(extractPayload(
|
||||
distributed_response,
|
||||
distributed_frame_offset,
|
||||
static_cast<size_t>(response_header_bytes_),
|
||||
static_cast<size_t>(distributed_length_)));
|
||||
std::tie(rows, cols) = resolveMatrixShape(tactile_rows_, tactile_cols_, tactile_points.size());
|
||||
tactile_valid = !tactile_points.empty();
|
||||
}
|
||||
|
||||
std::array<int, 3> resultant_force_tenths{};
|
||||
bool resultant_valid = false;
|
||||
// Publish force first: a slower distributed read must not postpone a
|
||||
// contact measurement. Each channel has its own request timestamp.
|
||||
if (read_resultant) {
|
||||
try {
|
||||
const auto resultant_frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_);
|
||||
if (!serial_->flushInput()) {
|
||||
handleRefreshFailure("Failed to flush serial input before resultant-force read: " + serial_->lastError(), had_valid_snapshot);
|
||||
return;
|
||||
const auto frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_);
|
||||
const auto request_time = std::chrono::steady_clock::now();
|
||||
const auto payload = transact(*serial_, frame, resultant_length_,
|
||||
std::chrono::milliseconds(response_timeout_ms_));
|
||||
if (!isSampleFresh(request_time)) {
|
||||
throw std::runtime_error("PX6AXGen3 resultant-force response arrived too late.");
|
||||
}
|
||||
if (!serial_->write(resultant_frame)) {
|
||||
handleRefreshFailure("Failed to send resultant-force command: " + serial_->lastError(), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
const size_t resultant_frame_bytes =
|
||||
static_cast<size_t>(response_header_bytes_) + static_cast<size_t>(resultant_length_);
|
||||
const auto resultant_response = readFramedResponse(
|
||||
*serial_,
|
||||
resultant_frame_bytes,
|
||||
resultant_frame.size(),
|
||||
std::chrono::milliseconds(response_timeout_ms_),
|
||||
"resultant-force response");
|
||||
const auto resultant_frame_offset =
|
||||
findResponseFrameOffset(resultant_response, resultant_frame_bytes);
|
||||
if (resultant_frame_offset != std::string::npos) {
|
||||
resultant_force_tenths = parseResultantPayload(extractPayload(
|
||||
resultant_response,
|
||||
resultant_frame_offset,
|
||||
static_cast<size_t>(response_header_bytes_),
|
||||
static_cast<size_t>(resultant_length_)));
|
||||
resultant_valid = true;
|
||||
} else {
|
||||
handleRefreshFailure("Invalid resultant-force response header. raw=" + previewBytesHex(resultant_response), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
if (!read_distributed) {
|
||||
handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot);
|
||||
return;
|
||||
}
|
||||
CMVR_LOG(WARNING) << "[PX6AXGen3] Failed to refresh resultant force: " << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
const auto force = parseResultantPayload(payload);
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
latest_snapshot_.resultant_force_tenths = force;
|
||||
latest_snapshot_.resultant_request_time = request_time;
|
||||
latest_snapshot_.resultant_valid = true;
|
||||
}
|
||||
if (read_distributed) {
|
||||
latest_snapshot_.tactile_points = std::move(tactile_points);
|
||||
const auto frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_);
|
||||
const auto request_time = std::chrono::steady_clock::now();
|
||||
const auto payload = transact(*serial_, frame, distributed_length_,
|
||||
std::chrono::milliseconds(response_timeout_ms_));
|
||||
if (!isSampleFresh(request_time)) {
|
||||
throw std::runtime_error("PX6AXGen3 distributed-force response arrived too late.");
|
||||
}
|
||||
auto points = parseDistributedPayload(payload);
|
||||
const auto [rows, cols] = resolveMatrixShape(tactile_rows_, tactile_cols_, points.size());
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
latest_snapshot_.tactile_points = std::move(points);
|
||||
latest_snapshot_.rows = rows;
|
||||
latest_snapshot_.cols = cols;
|
||||
latest_snapshot_.tactile_valid = tactile_valid;
|
||||
latest_snapshot_.tactile_request_time = request_time;
|
||||
latest_snapshot_.tactile_valid = true;
|
||||
}
|
||||
if (read_resultant) {
|
||||
latest_snapshot_.resultant_force_tenths = resultant_force_tenths;
|
||||
latest_snapshot_.resultant_valid = resultant_valid;
|
||||
}
|
||||
|
||||
clearOperationalError();
|
||||
} catch (const std::exception& e) {
|
||||
handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot);
|
||||
return;
|
||||
handleRefreshFailure(e.what());
|
||||
// Let initialization retries and synchronous callers see the failure.
|
||||
// The polling thread catches it and continues reconnecting in background.
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
@ -715,7 +682,7 @@ void PX6AXGen3::refreshSensorDataWithRetry(const int max_attempts,
|
||||
}
|
||||
}
|
||||
|
||||
handleRefreshFailure(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error, isSnapshotReady(true, true));
|
||||
throw std::runtime_error(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error);
|
||||
}
|
||||
|
||||
void PX6AXGen3::pollingLoop() {
|
||||
@ -754,18 +721,21 @@ void PX6AXGen3::pollingLoop() {
|
||||
void PX6AXGen3::ensureSensorReady(const bool allow_background,
|
||||
const bool require_tactile,
|
||||
const bool require_resultant) {
|
||||
const auto lifecycle = state();
|
||||
if (!config_valid_ || lifecycle == Status::STOPPED || lifecycle == Status::FAULT) {
|
||||
throw std::runtime_error("PX6AXGen3 is not operational: " + lastError());
|
||||
}
|
||||
const auto [polls_tactile, polls_resultant] = resolvePollingReadSelection();
|
||||
const bool background_covers_request =
|
||||
(!require_tactile || polls_tactile) &&
|
||||
(!require_resultant || polls_resultant);
|
||||
const bool background_ready = allow_background &&
|
||||
background_covers_request &&
|
||||
polling_thread_running_.load(std::memory_order_acquire) &&
|
||||
isSnapshotReady(require_tactile, require_resultant);
|
||||
|
||||
if (!background_ready) {
|
||||
refreshSensorData(require_tactile, require_resultant);
|
||||
if (allow_background && background_covers_request &&
|
||||
polling_thread_running_.load(std::memory_order_acquire)) {
|
||||
// The getter checks freshness while copying under snapshot_mutex_.
|
||||
// Never block the control loop on serial I/O to replace a stale sample.
|
||||
return;
|
||||
}
|
||||
refreshSensorData(require_tactile, require_resultant);
|
||||
}
|
||||
|
||||
bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion region) const {
|
||||
@ -774,8 +744,15 @@ bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion r
|
||||
|
||||
bool PX6AXGen3::isSnapshotReady(const bool require_tactile, const bool require_resultant) const {
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
return (!require_tactile || latest_snapshot_.tactile_valid) &&
|
||||
(!require_resultant || latest_snapshot_.resultant_valid);
|
||||
return (!require_tactile || (latest_snapshot_.tactile_valid &&
|
||||
isSampleFresh(latest_snapshot_.tactile_request_time))) &&
|
||||
(!require_resultant || (latest_snapshot_.resultant_valid &&
|
||||
isSampleFresh(latest_snapshot_.resultant_request_time)));
|
||||
}
|
||||
|
||||
bool PX6AXGen3::isSampleFresh(const std::chrono::steady_clock::time_point request_time) const {
|
||||
return request_time != std::chrono::steady_clock::time_point{} &&
|
||||
std::chrono::steady_clock::now() - request_time <= max_sample_age_;
|
||||
}
|
||||
|
||||
TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const {
|
||||
@ -785,9 +762,8 @@ TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const {
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex_);
|
||||
if (!latest_snapshot_.tactile_valid) {
|
||||
CMVR_LOG(ERROR) << "PX6AXGen3 tactile snapshot is not ready.";
|
||||
return {};
|
||||
if (!latest_snapshot_.tactile_valid || !isSampleFresh(latest_snapshot_.tactile_request_time)) {
|
||||
throw std::runtime_error("PX6AXGen3 tactile sample is unavailable or stale.");
|
||||
}
|
||||
*snapshot = latest_snapshot_.tactile_points;
|
||||
rows = latest_snapshot_.rows;
|
||||
@ -823,21 +799,15 @@ void PX6AXGen3::clearOperationalError() {
|
||||
}
|
||||
}
|
||||
|
||||
void PX6AXGen3::handleRefreshFailure(const std::string& error, const bool had_valid_snapshot) {
|
||||
closeConnection();
|
||||
|
||||
if (had_valid_snapshot) {
|
||||
void PX6AXGen3::handleRefreshFailure(const std::string& error) {
|
||||
// Invalidate before any close/reconnect work. Keep the worker alive so a
|
||||
// subsequent valid transaction can restore service, but never expose old data.
|
||||
initializeSnapshot();
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex_);
|
||||
if (lifecycle_state_ == Status::INITIALIZED || lifecycle_state_ == Status::STREAMING) {
|
||||
last_error_ = error;
|
||||
}
|
||||
}
|
||||
CMVR_LOG(WARNING) << error;
|
||||
return;
|
||||
}
|
||||
|
||||
enterFault(error);
|
||||
closeConnection();
|
||||
}
|
||||
|
||||
void PX6AXGen3::transitionTo(const Status next_state) {
|
||||
@ -857,6 +827,7 @@ void PX6AXGen3::enterFault(const std::string& error) {
|
||||
|
||||
polling_thread_running_.store(false, std::memory_order_release);
|
||||
polling_cv_.notify_all();
|
||||
initializeSnapshot();
|
||||
closeConnection();
|
||||
|
||||
CMVR_LOG(ERROR) << error;
|
||||
|
||||
@ -0,0 +1,201 @@
|
||||
#include "../include/px_6ax_gen3.h"
|
||||
#include "hardware/include/posix_serial_transport.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <csignal>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace {
|
||||
using Clock = std::chrono::steady_clock;
|
||||
volatile std::sig_atomic_t interrupted = 0;
|
||||
void interrupt(int) { interrupted = 1; }
|
||||
|
||||
// Wrap the actual POSIX serial transport; optionally capture every transmitted
|
||||
// and received byte to diagnose framing/USB latency without a second reader.
|
||||
class TraceTransport final : public cmvr::AbstractSerialTransport {
|
||||
public:
|
||||
explicit TraceTransport(const std::string& path) {
|
||||
if (!path.empty()) {
|
||||
trace_.open(path);
|
||||
if (!trace_) throw std::runtime_error("Cannot open raw log: " + path);
|
||||
}
|
||||
}
|
||||
bool open(const Config& cfg) override { return serial_.open(cfg); }
|
||||
bool close() override { return serial_.close(); }
|
||||
bool isOpen() const override { return serial_.isOpen(); }
|
||||
bool flushInput() override { return serial_.flushInput(); }
|
||||
bool write(const std::vector<uint8_t>& data) override {
|
||||
// Only permit sensor read requests, even if driver defaults change.
|
||||
if (data.size() != 14 || data[6] != 0xFB) {
|
||||
throw std::runtime_error("Real test only permits force read commands.");
|
||||
}
|
||||
const bool ok = serial_.write(data);
|
||||
record(ok ? "TX" : "TX_FAILED", data);
|
||||
return ok;
|
||||
}
|
||||
bool read(size_t n, std::chrono::milliseconds timeout, std::vector<uint8_t>& data) override {
|
||||
const bool ok = serial_.read(n, timeout, data);
|
||||
record(ok ? "RX" : "RX_FAILED", data);
|
||||
return ok;
|
||||
}
|
||||
std::string lastError() const override { return serial_.lastError(); }
|
||||
private:
|
||||
void record(const char* direction, const std::vector<uint8_t>& data) {
|
||||
if (!trace_.is_open()) return;
|
||||
trace_ << std::fixed << std::setprecision(3)
|
||||
<< std::chrono::duration<double, std::milli>(Clock::now() - begin_).count()
|
||||
<< " ms " << direction;
|
||||
for (const auto byte : data) {
|
||||
trace_ << ' ' << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte);
|
||||
}
|
||||
trace_ << std::dec << std::setfill(' ') << '\n';
|
||||
trace_.flush();
|
||||
}
|
||||
cmvr::PosixSerialTransport serial_;
|
||||
std::ofstream trace_;
|
||||
Clock::time_point begin_{Clock::now()};
|
||||
};
|
||||
|
||||
struct Options {
|
||||
std::string port{"/dev/ttyACM0"};
|
||||
std::string mode{"sync"};
|
||||
std::string csv;
|
||||
std::string raw_log;
|
||||
int module_id{2};
|
||||
int poll_ms{5};
|
||||
int max_age_ms{50};
|
||||
int timeout_ms{200};
|
||||
double duration_s{10.0};
|
||||
};
|
||||
|
||||
void usage() {
|
||||
std::cout << "Usage: px_6ax_gen3_real_test [--port /dev/ttyACM0] [--module-id 2]\n"
|
||||
" [--duration-s 10] [--mode sync|stream] [--poll-ms 5]\n"
|
||||
" [--max-age-ms 50] [--timeout-ms 200] [--csv samples.csv]\n"
|
||||
" [--raw-log frames.log]\n"
|
||||
"sync: each read sends a new force request; latency is request/response time.\n"
|
||||
"stream: exercise background polling and cached getters used by the task;\n"
|
||||
" getter counts include repeated samples, not sensor update frequency.\n"
|
||||
"Only reads the sensor. No calibration or robot commands. Ctrl-C stops.\n";
|
||||
}
|
||||
|
||||
Options parseOptions(int argc, char** argv) {
|
||||
Options o;
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
const std::string arg = argv[i];
|
||||
if (arg == "--help") { usage(); std::exit(0); }
|
||||
if (++i >= argc) throw std::invalid_argument("Missing value for " + arg);
|
||||
const std::string value = argv[i];
|
||||
if (arg == "--port") o.port = value;
|
||||
else if (arg == "--mode") o.mode = value;
|
||||
else if (arg == "--module-id") o.module_id = std::stoi(value);
|
||||
else if (arg == "--duration-s") o.duration_s = std::stod(value);
|
||||
else if (arg == "--poll-ms") o.poll_ms = std::stoi(value);
|
||||
else if (arg == "--max-age-ms") o.max_age_ms = std::stoi(value);
|
||||
else if (arg == "--timeout-ms") o.timeout_ms = std::stoi(value);
|
||||
else if (arg == "--csv") o.csv = value;
|
||||
else if (arg == "--raw-log") o.raw_log = value;
|
||||
else throw std::invalid_argument("Unknown option: " + arg);
|
||||
}
|
||||
if ((o.mode != "sync" && o.mode != "stream") || !std::isfinite(o.duration_s) ||
|
||||
o.duration_s <= 0 || o.poll_ms <= 0 || o.max_age_ms <= 0 || o.timeout_ms <= 0) {
|
||||
throw std::invalid_argument("Invalid mode, duration or timing option");
|
||||
}
|
||||
return o;
|
||||
}
|
||||
|
||||
double percentile(const std::vector<double>& sorted, double fraction) {
|
||||
return sorted.empty() ? 0.0 : sorted[static_cast<size_t>((sorted.size() - 1) * fraction)];
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
try {
|
||||
const auto options = parseOptions(argc, argv);
|
||||
std::signal(SIGINT, interrupt);
|
||||
std::signal(SIGTERM, interrupt);
|
||||
cmvr::config::PX6AXGen3 cfg;
|
||||
cfg.set_serial_port(options.port);
|
||||
cfg.set_module_id(options.module_id);
|
||||
cfg.set_baud_rate(921600);
|
||||
cfg.set_distributed_length(153);
|
||||
cfg.set_resultant_length(3);
|
||||
cfg.set_poll_interval_ms(options.poll_ms);
|
||||
cfg.set_response_timeout_ms(options.timeout_ms);
|
||||
cfg.set_max_sample_age_ms(options.max_age_ms);
|
||||
cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE);
|
||||
cfg.set_auto_calibrate(false);
|
||||
cmvr::device::PX6AXGen3 sensor(cfg, std::make_unique<TraceTransport>(options.raw_log));
|
||||
std::ofstream csv;
|
||||
if (!options.csv.empty()) {
|
||||
csv.open(options.csv);
|
||||
if (!csv) throw std::runtime_error("Cannot open CSV: " + options.csv);
|
||||
csv << "elapsed_ms,read_ms,valid,fx_N,fy_N,fz_N\n";
|
||||
}
|
||||
std::cout << "port=" << options.port << " module_id=" << options.module_id
|
||||
<< " device_address=" << options.module_id + 1 << " mode=" << options.mode
|
||||
<< " max_sample_age_ms=" << options.max_age_ms << '\n';
|
||||
if (!sensor.init()) throw std::runtime_error("Sensor init failed: " + sensor.lastError());
|
||||
if (options.mode == "stream" && !sensor.start()) {
|
||||
throw std::runtime_error("Sensor start failed: " + sensor.lastError());
|
||||
}
|
||||
const auto begin = Clock::now();
|
||||
auto next = begin;
|
||||
auto next_print = begin;
|
||||
std::vector<double> durations;
|
||||
size_t failures = 0;
|
||||
double max_fz = 0.0;
|
||||
while (!interrupted && std::chrono::duration<double>(Clock::now() - begin).count() < options.duration_s) {
|
||||
const auto read_begin = Clock::now();
|
||||
cmvr::device::AbstractDexHand::ForceNewtons force;
|
||||
bool valid = true;
|
||||
std::string error;
|
||||
try {
|
||||
force = sensor.getResultantForceNewtons(cmvr::device::AbstractDexHand::FingerType::INDEX,
|
||||
cmvr::device::AbstractDexHand::TactileRegion::TIP);
|
||||
} catch (const std::exception& e) {
|
||||
valid = false;
|
||||
error = e.what();
|
||||
++failures;
|
||||
}
|
||||
const auto now = Clock::now();
|
||||
const double read_ms = std::chrono::duration<double, std::milli>(now - read_begin).count();
|
||||
const double elapsed_ms = std::chrono::duration<double, std::milli>(now - begin).count();
|
||||
if (valid) { durations.push_back(read_ms); max_fz = std::max(max_fz, force.fz); }
|
||||
if (csv.is_open()) {
|
||||
csv << std::fixed << std::setprecision(3) << elapsed_ms << ',' << read_ms << ',' << valid;
|
||||
if (valid) csv << ',' << force.fx << ',' << force.fy << ',' << force.fz;
|
||||
else csv << ",,,";
|
||||
csv << '\n';
|
||||
}
|
||||
if (now >= next_print) {
|
||||
std::cout << std::fixed << std::setprecision(3) << "t_ms=" << elapsed_ms
|
||||
<< " read_ms=" << read_ms;
|
||||
if (valid) std::cout << " force_N=[" << force.fx << ',' << force.fy << ',' << force.fz
|
||||
<< "] fz_N=" << force.fz;
|
||||
else std::cout << " UNAVAILABLE: " << error;
|
||||
std::cout << std::endl;
|
||||
next_print = now + std::chrono::milliseconds(100);
|
||||
}
|
||||
next += std::chrono::milliseconds(options.poll_ms);
|
||||
if (next < now) next = now;
|
||||
std::this_thread::sleep_until(next);
|
||||
}
|
||||
const double elapsed_s = std::chrono::duration<double>(Clock::now() - begin).count();
|
||||
sensor.stop();
|
||||
std::sort(durations.begin(), durations.end());
|
||||
std::cout << "SUMMARY mode=" << options.mode << " valid_reads=" << durations.size()
|
||||
<< " unavailable_reads=" << failures << " valid_reads_per_s=" << durations.size() / elapsed_s
|
||||
<< " p50_ms=" << percentile(durations, .5) << " p95_ms=" << percentile(durations, .95)
|
||||
<< " max_ms=" << percentile(durations, 1) << " max_fz_N=" << max_fz << '\n';
|
||||
return durations.empty() || failures != 0 ? 1 : 0;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << e.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
318
cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp
Normal file
318
cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp
Normal file
@ -0,0 +1,318 @@
|
||||
#include "../include/px_6ax_gen3.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <functional>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace {
|
||||
using Sensor = cmvr::device::PX6AXGen3;
|
||||
using Bytes = std::vector<uint8_t>;
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
void checksum(Bytes& frame) {
|
||||
unsigned sum = 0;
|
||||
for (size_t i = 0; i + 1 < frame.size(); ++i) sum += frame[i];
|
||||
frame.back() = static_cast<uint8_t>(-sum);
|
||||
}
|
||||
|
||||
Bytes reply(const Bytes& request) {
|
||||
const bool is_write = request[6] == 0x79;
|
||||
const size_t n = is_write ? 0U : request[11] | (size_t(request[12]) << 8U);
|
||||
Bytes frame{0xAA, 0x55, static_cast<uint8_t>((n + 10U) & 0xffU),
|
||||
static_cast<uint8_t>((n + 10U) >> 8U)};
|
||||
frame.insert(frame.end(), request.begin() + 4, request.begin() + 13);
|
||||
// Real device returns status=1 on reads; zero is only the write ACK status.
|
||||
frame.push_back(is_write ? 0 : 1);
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
frame.push_back(i % 3U == 0 ? 0x80 : (i % 3U == 1 ? 0x7f : 10));
|
||||
}
|
||||
frame.push_back(0);
|
||||
checksum(frame);
|
||||
return frame;
|
||||
}
|
||||
|
||||
class ScriptedTransport final : public cmvr::AbstractSerialTransport {
|
||||
public:
|
||||
bool open(const Config&) override { opened = true; return true; }
|
||||
bool close() override { opened = false; return true; }
|
||||
bool isOpen() const override { return opened; }
|
||||
bool flushInput() override { pending.clear(); return true; }
|
||||
bool write(const Bytes& request) override {
|
||||
++writes;
|
||||
unsigned sum = 0;
|
||||
for (auto byte : request) sum += byte;
|
||||
EXPECT_EQ(sum % 256, 0U);
|
||||
pending = dropping.load() ? Bytes{} : make_reply(request);
|
||||
return true;
|
||||
}
|
||||
bool read(size_t, std::chrono::milliseconds timeout, Bytes& out) override {
|
||||
out.clear();
|
||||
if (pending.empty()) {
|
||||
waiting.store(true);
|
||||
std::this_thread::sleep_for(timeout);
|
||||
waiting.store(false);
|
||||
return false;
|
||||
}
|
||||
const size_t n = std::min(chunk_size, pending.size());
|
||||
out.assign(pending.begin(), pending.begin() + n);
|
||||
pending.erase(pending.begin(), pending.begin() + n);
|
||||
return true;
|
||||
}
|
||||
std::string lastError() const override { return "scripted timeout"; }
|
||||
std::function<Bytes(const Bytes&)> make_reply{reply};
|
||||
size_t chunk_size{256};
|
||||
std::atomic<int> writes{0};
|
||||
std::atomic<bool> dropping{false};
|
||||
std::atomic<bool> waiting{false};
|
||||
private:
|
||||
bool opened{false};
|
||||
Bytes pending;
|
||||
};
|
||||
|
||||
cmvr::config::PX6AXGen3 config() {
|
||||
cmvr::config::PX6AXGen3 cfg;
|
||||
cfg.set_serial_port("scripted");
|
||||
cfg.set_module_id(2);
|
||||
cfg.set_distributed_length(153);
|
||||
cfg.set_poll_interval_ms(5);
|
||||
cfg.set_response_timeout_ms(5);
|
||||
cfg.set_max_sample_age_ms(50);
|
||||
cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE);
|
||||
return cfg;
|
||||
}
|
||||
|
||||
Sensor::ResultantForce force(Sensor& sensor) {
|
||||
return sensor.getResultantForce(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
|
||||
}
|
||||
|
||||
Sensor::ForceNewtons forceNewtons(cmvr::device::AbstractDexHand& sensor) {
|
||||
return sensor.getResultantForceNewtons(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
|
||||
}
|
||||
|
||||
bool waitFor(const std::function<bool()>& predicate) {
|
||||
const auto deadline = std::chrono::steady_clock::now() + 1s;
|
||||
do {
|
||||
if (predicate()) return true;
|
||||
std::this_thread::sleep_for(1ms);
|
||||
} while (std::chrono::steady_clock::now() < deadline);
|
||||
return predicate();
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, ParsesRealReadStatusAndSignedForcesAcrossByteFragments) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->chunk_size = 1;
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
const auto f = force(sensor);
|
||||
EXPECT_EQ(f.fx, -128);
|
||||
EXPECT_EQ(f.fy, 127);
|
||||
EXPECT_EQ(f.fz, 10);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, NewtonInterfacePreservesSignsAndComputesPhysicalMagnitude) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
serial->make_reply = [](const Bytes& request) {
|
||||
auto frame = reply(request);
|
||||
frame[14] = 0xfd; // -3 raw = -0.3 N
|
||||
frame[15] = 4;
|
||||
frame[16] = 12;
|
||||
checksum(frame);
|
||||
return frame;
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
const auto writes_before = transport->writes.load();
|
||||
const auto f = forceNewtons(sensor);
|
||||
EXPECT_EQ(transport->writes.load(), writes_before + 1);
|
||||
EXPECT_DOUBLE_EQ(f.fx, -0.3);
|
||||
EXPECT_DOUBLE_EQ(f.fy, 0.4);
|
||||
EXPECT_DOUBLE_EQ(f.fz, 1.2);
|
||||
EXPECT_DOUBLE_EQ(f.magnitude(), 1.3);
|
||||
EXPECT_EQ(force(sensor).fz, 12); // Raw interface remains unchanged.
|
||||
}
|
||||
|
||||
class NewtonForce : public testing::TestWithParam<int> {};
|
||||
TEST_P(NewtonForce, ConvertsRawFzOnceAndPreservesPointOneNewtonTrigger) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
const int raw_fz = GetParam();
|
||||
serial->make_reply = [raw_fz](const Bytes& request) {
|
||||
auto frame = reply(request);
|
||||
frame[14] = 0;
|
||||
frame[15] = 0;
|
||||
frame[16] = static_cast<uint8_t>(raw_fz);
|
||||
checksum(frame);
|
||||
return frame;
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
const auto f = forceNewtons(sensor);
|
||||
EXPECT_DOUBLE_EQ(f.fz, static_cast<double>(raw_fz) / 10.0);
|
||||
EXPECT_DOUBLE_EQ(f.magnitude(), f.fz);
|
||||
EXPECT_EQ(f.fz >= 0.1, raw_fz >= 1);
|
||||
EXPECT_EQ(force(sensor).fz, raw_fz);
|
||||
}
|
||||
INSTANTIATE_TEST_SUITE_P(PhysicalUnits, NewtonForce, testing::Values(0, 1, 10, 108, 255));
|
||||
|
||||
TEST(PX6AXGen3, ResynchronizesAfterEchoNoiseAndInvalidFrame) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->chunk_size = 7;
|
||||
serial->make_reply = [](const Bytes& request) {
|
||||
auto bad = reply(request);
|
||||
bad.back() ^= 1;
|
||||
Bytes frames{0xAA, 0x00};
|
||||
frames.insert(frames.end(), request.begin(), request.end());
|
||||
frames.insert(frames.end(), bad.begin(), bad.end());
|
||||
const auto valid = reply(request);
|
||||
frames.insert(frames.end(), valid.begin(), valid.end());
|
||||
return frames;
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
EXPECT_EQ(force(sensor).fz, 10);
|
||||
}
|
||||
|
||||
class InvalidFrame : public testing::TestWithParam<int> {};
|
||||
TEST_P(InvalidFrame, RejectsResponseAndFailsInitializationAfterRetries) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
const int kind = GetParam();
|
||||
serial->make_reply = [kind](const Bytes& request) {
|
||||
auto frame = reply(request);
|
||||
if (kind == 0) { frame.pop_back(); return frame; } // missing LRC
|
||||
if (kind == 1) { frame.back() ^= 1; return frame; } // bad LRC
|
||||
// Frame size, device, reserved, function, register, returned byte count.
|
||||
const int offsets[]{2, 4, 5, 6, 7, 11};
|
||||
frame[offsets[kind - 2]] ^= 1;
|
||||
checksum(frame); // Valid LRC must not bypass request matching.
|
||||
return frame;
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
EXPECT_FALSE(sensor.init());
|
||||
EXPECT_EQ(transport->writes.load(), 5);
|
||||
EXPECT_EQ(sensor.state(), Sensor::Status::FAULT);
|
||||
EXPECT_FALSE(sensor.lastError().empty());
|
||||
EXPECT_THROW(force(sensor), std::runtime_error);
|
||||
}
|
||||
INSTANTIATE_TEST_SUITE_P(ProtocolValidation, InvalidFrame, testing::Range(0, 8));
|
||||
|
||||
TEST(PX6AXGen3, RetriesTransientStartupFailure) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
int count = 0;
|
||||
serial->make_reply = [&count](const Bytes& request) {
|
||||
++count;
|
||||
return count < 3 ? Bytes{} : reply(request);
|
||||
};
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
EXPECT_EQ(count, 3);
|
||||
EXPECT_TRUE(sensor.lastError().empty());
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, StaleGetterFailsWithoutWaitingForSerialAndRecovers) {
|
||||
auto cfg = config();
|
||||
cfg.set_response_timeout_ms(200);
|
||||
cfg.set_max_sample_age_ms(30);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
ASSERT_TRUE(sensor.start());
|
||||
ASSERT_EQ(force(sensor).fz, 10);
|
||||
transport->dropping.store(true);
|
||||
ASSERT_TRUE(waitFor([&] { return transport->waiting.load(); }));
|
||||
std::this_thread::sleep_for(40ms);
|
||||
const auto begin = std::chrono::steady_clock::now();
|
||||
EXPECT_THROW(force(sensor), std::runtime_error);
|
||||
EXPECT_THROW(forceNewtons(sensor), std::runtime_error);
|
||||
EXPECT_LT(std::chrono::steady_clock::now() - begin, 50ms);
|
||||
cmvr::device::DexHandState state;
|
||||
sensor.getState(state);
|
||||
EXPECT_FALSE(state.is_initialized);
|
||||
EXPECT_FALSE(state.hands[0].error_message.empty());
|
||||
transport->dropping.store(false);
|
||||
ASSERT_TRUE(waitFor([&] {
|
||||
try { return force(sensor).fz == 10; }
|
||||
catch (const std::exception&) { return false; }
|
||||
}));
|
||||
sensor.stop();
|
||||
const auto writes = transport->writes.load();
|
||||
EXPECT_THROW(force(sensor), std::runtime_error);
|
||||
EXPECT_THROW(forceNewtons(sensor), std::runtime_error);
|
||||
EXPECT_EQ(transport->writes.load(), writes);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, ReadFailureInvalidatesPreviouslyValidSample) {
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
Sensor sensor(config(), std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
transport->dropping.store(true);
|
||||
EXPECT_THROW(force(sensor), std::runtime_error);
|
||||
cmvr::device::DexHandState state;
|
||||
sensor.getState(state);
|
||||
EXPECT_FALSE(state.is_initialized);
|
||||
transport->dropping.store(false);
|
||||
EXPECT_EQ(force(sensor).fz, 10);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, RejectsLateResponseEvenWithValidChecksum) {
|
||||
auto cfg = config();
|
||||
cfg.set_max_sample_age_ms(2);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->make_reply = [](const Bytes& request) {
|
||||
std::this_thread::sleep_for(5ms);
|
||||
return reply(request);
|
||||
};
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
EXPECT_FALSE(sensor.init());
|
||||
EXPECT_NE(sensor.lastError().find("too late"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, DistributedDataIsValidatedAndParsed) {
|
||||
auto cfg = config();
|
||||
cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_FORCE);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->chunk_size = 5;
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
ASSERT_TRUE(sensor.init());
|
||||
auto data = sensor.getSensorData(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP);
|
||||
ASSERT_TRUE(data.valid());
|
||||
ASSERT_EQ(data.view.pointCount(), 51);
|
||||
EXPECT_EQ(data.view.at(0, 50).fx, -128);
|
||||
EXPECT_EQ(data.view.at(0, 50).fz, 10);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, CalibrationRequiresFullSuccessfulWriteAcknowledgment) {
|
||||
auto cfg = config();
|
||||
cfg.set_auto_calibrate(true);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
serial->make_reply = [](const Bytes& request) {
|
||||
if (request[6] == 0x79) return Bytes{0xAA, 0x55};
|
||||
return reply(request);
|
||||
};
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
EXPECT_FALSE(sensor.init());
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, CalibrationAcceptsFullWriteAcknowledgment) {
|
||||
auto cfg = config();
|
||||
cfg.set_auto_calibrate(true);
|
||||
Sensor sensor(cfg, std::make_unique<ScriptedTransport>());
|
||||
ASSERT_TRUE(sensor.init());
|
||||
EXPECT_EQ(force(sensor).fz, 10);
|
||||
}
|
||||
|
||||
TEST(PX6AXGen3, InvalidConfigurationCannotBeResurrectedByInitOrStart) {
|
||||
auto cfg = config();
|
||||
cfg.set_response_header_bytes(15);
|
||||
auto serial = std::make_unique<ScriptedTransport>();
|
||||
auto* transport = serial.get();
|
||||
Sensor sensor(cfg, std::move(serial));
|
||||
EXPECT_FALSE(sensor.init());
|
||||
EXPECT_FALSE(sensor.start());
|
||||
EXPECT_EQ(transport->writes.load(), 0);
|
||||
}
|
||||
}
|
||||
@ -38,6 +38,7 @@ public:
|
||||
std::vector<TactileRegionData> getSensorData() override;
|
||||
TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
|
||||
ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
|
||||
ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override;
|
||||
|
||||
private:
|
||||
TactileRegionData makeRegionData(FingerType finger, TactileRegion region);
|
||||
|
||||
@ -83,6 +83,11 @@ ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce(
|
||||
return TactilePoint::fromFz(0);
|
||||
}
|
||||
|
||||
ZeroSimTouchDexHand::ForceNewtons ZeroSimTouchDexHand::getResultantForceNewtons(
|
||||
const FingerType, const TactileRegion) {
|
||||
return {};
|
||||
}
|
||||
|
||||
ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData(
|
||||
const FingerType finger, const TactileRegion region) {
|
||||
tactile_points_[0] = TactilePoint::fromFz(0);
|
||||
|
||||
@ -95,6 +95,7 @@ public:
|
||||
|
||||
int targetU() const;
|
||||
int targetV() const;
|
||||
// Selected force criterion summed over requested tactile regions, in N.
|
||||
double lastTouchPressureSum() const;
|
||||
int lastTouchNonzeroCount() const;
|
||||
int lastActiveTagId() const;
|
||||
@ -183,8 +184,8 @@ private:
|
||||
int pbvs_debug_count_{0};
|
||||
int last_active_tag_id_{-1};
|
||||
|
||||
double last_touch_pressure_sum_{0.0};
|
||||
double last_touch_resultant_fz_{0.0};
|
||||
double last_touch_pressure_sum_{0.0}; // N
|
||||
double last_touch_resultant_fz_{0.0}; // N
|
||||
int last_touch_nonzero_count_{0};
|
||||
Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
|
||||
Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
|
||||
|
||||
@ -116,15 +116,15 @@ bool isTouchTriggered(const TouchScreenTaskConfig& config,
|
||||
return resultant_force_value >= config.touch().tactile().force_threshold();
|
||||
}
|
||||
|
||||
double tactileForceValue(const device::AbstractDexHand::TactilePoint& point,
|
||||
double tactileForceValue(const device::AbstractDexHand::ForceNewtons& point,
|
||||
const cmvr::config::TouchScreenTactileCriterion criterion) {
|
||||
switch (criterion) {
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ:
|
||||
return static_cast<double>(point.fz);
|
||||
return point.fz;
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE:
|
||||
return point.magnitude();
|
||||
}
|
||||
return static_cast<double>(point.fz);
|
||||
return point.fz;
|
||||
}
|
||||
|
||||
Eigen::Matrix3d rotationFromTargetEuler(const double rx,
|
||||
@ -2022,9 +2022,10 @@ bool TouchScreenTask::updateTouchPressure() {
|
||||
double resultant_fz = 0.0;
|
||||
try {
|
||||
for (const auto& tactile_region : tactile_regions) {
|
||||
const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second);
|
||||
const auto resultant_force = dexhand_->getResultantForceNewtons(
|
||||
tactile_region.first, tactile_region.second);
|
||||
resultant_value += tactileForceValue(resultant_force, tactile.criterion());
|
||||
resultant_fz += static_cast<double>(resultant_force.fz);
|
||||
resultant_fz += resultant_force.fz;
|
||||
}
|
||||
} catch (...) {
|
||||
return false;
|
||||
@ -2043,9 +2044,9 @@ void TouchScreenTask::logTouchPressure(const bool force) {
|
||||
return;
|
||||
}
|
||||
last_touch_pressure_log_time_ = now;
|
||||
CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << last_touch_resultant_fz_
|
||||
<< ", criterion_value=" << last_touch_pressure_sum_
|
||||
<< ", threshold=" << config_.touch().tactile().force_threshold()
|
||||
CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz_N=" << last_touch_resultant_fz_
|
||||
<< ", criterion_value_N=" << last_touch_pressure_sum_
|
||||
<< ", threshold_N=" << config_.touch().tactile().force_threshold()
|
||||
<< ", triggered=" << isTouchTriggered(config_, last_touch_pressure_sum_);
|
||||
}
|
||||
|
||||
|
||||
@ -36,6 +36,9 @@ message PX6AXGen3{
|
||||
string tactile_region = 16;
|
||||
string sensor_name = 17;
|
||||
PX6AXGen3PollingReadMode polling_read_mode = 18;
|
||||
// Maximum age since the sample's request was sent. 0 uses 50 ms.
|
||||
// Independent of response_timeout_ms: stale cached data must fail promptly.
|
||||
int32 max_sample_age_ms = 19;
|
||||
}
|
||||
|
||||
message ZeroSimTouchDexHand {
|
||||
|
||||
@ -112,6 +112,8 @@ message TouchScreenTactileTriggerConfig {
|
||||
optional TouchScreenFingerType finger = 1;
|
||||
optional TouchScreenTactileRegion region = 2;
|
||||
optional TouchScreenTactileCriterion criterion = 3;
|
||||
// Threshold in newtons (N), applied to the selected force criterion summed
|
||||
// over the requested tactile regions. Equality also triggers contact.
|
||||
optional double force_threshold = 4;
|
||||
}
|
||||
|
||||
|
||||
12
script/test_px_6ax_gen3.sh
Executable file
12
script/test_px_6ax_gen3.sh
Executable file
@ -0,0 +1,12 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
# Real USB sensor test. The executable only sends force-read requests.
|
||||
# Example: ./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --duration-s 20 --mode sync
|
||||
repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}"
|
||||
cmake --build "$build_dir" --target px_6ax_gen3_real_test -j 4
|
||||
sensor_build_dir="$build_dir/cmvr-es/devices/dexhand/px_6ax_gen3"
|
||||
# Put build libraries ahead of installed ones so this tests the current driver/proto.
|
||||
exec env LD_LIBRARY_PATH="$build_dir:$sensor_build_dir:$build_dir/cmvr-es/hardware:$repo_root/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \
|
||||
"$sensor_build_dir/px_6ax_gen3_real_test" "$@"
|
||||
Loading…
Reference in New Issue
Block a user